12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Revamped ISAM2::marginalizeLeaves again, more tree algorithm instead of relying on sorted keys, hopefully correct this time.

release/4.3a0
Richard Roberts 2013-03-18 19:28:02 +00:00
parent 467cd225af
commit 596c5cdae8
2 changed files with 242 additions and 115 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

242
gtsam/nonlinear/ISAM2.cpp
View File

@ -24,8 +24,9 @@ using namespace boost::assign;
#include <gtsam/base/timing.h> #include <gtsam/base/timing.h>
#include <gtsam/base/debug.h> #include <gtsam/base/debug.h>
#include <gtsam/linear/GaussianJunctionTree.h>
#include <gtsam/inference/BayesTree.h> #include <gtsam/inference/BayesTree.h>
#include <gtsam/linear/GaussianJunctionTree.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/linear/HessianFactor.h> #include <gtsam/linear/HessianFactor.h>
#include <gtsam/linear/GaussianFactorGraph.h> #include <gtsam/linear/GaussianFactorGraph.h>
@ -792,120 +793,147 @@ void ISAM2::experimentalMarginalizeLeaves(const FastList<Key>& leafKeys)
BOOST_FOREACH(Key key, leafKeys) { BOOST_FOREACH(Key key, leafKeys) {
indices.insert(ordering_[key]); indices.insert(ordering_[key]);
} }
FastSet<Index> origIndices = indices;
// For each clique containing variables to be marginalized, we need to // Keep track of marginal factors - map from clique to the marginal factors
// reeliminate the marginalized variables and add their linear contribution // that should be incorporated into it, passed up from it's children.
// factor into the factor graph. This results in some extra work if we are multimap<sharedClique, GaussianFactor::shared_ptr> marginalFactors;
// marginalizing multiple cliques on the same path.
// To do this, first build a map containing all cliques containing variables to be
// marginalized that determines the last-ordered variable eliminated in that
// clique.
FastMap<sharedClique, Index> cliquesToMarginalize;
BOOST_FOREACH(Index jI, indices)
{
assert(nodes_[jI]); // Assert that the nodes structure contains this index
pair<FastMap<sharedClique,Index>::iterator, bool> insertResult =
cliquesToMarginalize.insert(make_pair(nodes_[jI], jI));
// If insert found a map entry existing, see if our current index is
// eliminated later and modify the entry if so.
if(!insertResult.second && jI > insertResult.first->second)
insertResult.first->second = jI;
}
#ifndef NDEBUG
// Check that the cliques have sorted frontal keys - we assume that here.
for(FastMap<sharedClique,Index>::const_iterator c = cliquesToMarginalize.begin();
c != cliquesToMarginalize.end(); ++c)
{
FastSet<Key> keys(c->first->conditional()->beginFrontals(), c->first->conditional()->endFrontals());
assert(std::equal(c->first->conditional()->beginFrontals(), c->first->conditional()->endFrontals(), keys.begin()));
}
#endif
// Now loop over the indices, the iterator jI is advanced inside the loop.
FastSet<size_t> factorIndicesToRemove; FastSet<size_t> factorIndicesToRemove;
GaussianFactorGraph factorsToAdd;
for(FastSet<Index>::iterator jI = indices.begin(); jI != indices.end(); )
{
const Index j = *jI;
// Retrieve the clique and the latest-ordered index corresponding to this index // Remove each variable and its subtrees
FastMap<sharedClique,Index>::iterator clique_lastIndex = cliquesToMarginalize.find(nodes_[*jI]); BOOST_FOREACH(Index j, indices) {
assert(clique_lastIndex != cliquesToMarginalize.end()); // Assert that we indexed the clique if(nodes_[j]) { // If the index was not already removed by removing another subtree
sharedClique clique = nodes_[j];
const size_t originalnFrontals = clique_lastIndex->first->conditional()->nrFrontals(); // See if we should remove the whole clique
bool marginalizeEntireClique = true;
BOOST_FOREACH(Index frontal, clique->conditional()->frontals()) {
if(indices.find(frontal) == indices.end()) {
marginalizeEntireClique = false;
break; } }
// Check that the clique has no children // Remove either the whole clique or part of it
if(!clique_lastIndex->first->children().empty()) if(marginalizeEntireClique) {
throw MarginalizeNonleafException(ordering_.key(*jI), params_.keyFormatter); // Remove the whole clique and its subtree, and keep the marginal factor.
GaussianFactor::shared_ptr marginalFactor = clique->cachedFactor();
// We do not need the marginal factors associated with this clique
// because their information is already incorporated in the new
// marginal factor. So, now associate this marginal factor with the
// parent of this clique.
marginalFactors.insert(make_pair(clique->parent(), marginalFactor));
// Now remove this clique and its subtree - all of its marginal
// information has been stored in marginalFactors.
const Cliques removedCliques = this->removeSubtree(clique); // Remove the subtree and throw away the cliques
BOOST_FOREACH(const sharedClique& removedClique, removedCliques) {
marginalFactors.erase(removedClique);
// Mark factors for removal
BOOST_FOREACH(Index indexInClique, removedClique->conditional()->frontals()) {
factorIndicesToRemove.insert(variableIndex_[indexInClique].begin(), variableIndex_[indexInClique].end()); }
}
}
else {
// Reeliminate the current clique and the marginals from its children,
// then keep only the marginal on the non-marginalized variables. We
// get the childrens' marginals from any existing children, plus
// the marginals from the marginalFactors multimap, which come from any
// subtrees already marginalized out.
// Check that all previous variables in the clique are also being eliminated and no later ones. // Add child marginals and remove marginalized subtrees
// At the same time, remove the indices marginalized with this clique from the indices set. GaussianFactorGraph graph;
// This is where the iterator j is advanced. Cliques subtreesToRemove;
size_t nFrontals = 0; BOOST_FOREACH(const sharedClique& child, clique->children()) {
{ graph.push_back(child->cachedFactor()); // Add child marginal
bool foundLast = false; // Remove subtree if child depends on any marginalized keys
BOOST_FOREACH(Index cliqueVar, clique_lastIndex->first->conditional()->frontals()) { BOOST_FOREACH(Index parentIndex, child->conditional()->parents()) {
if(!foundLast && indices.find(cliqueVar) == indices.end()) if(indices.find(parentIndex) != indices.end()) {
throw MarginalizeNonleafException(ordering_.key(j), params_.keyFormatter); subtreesToRemove.push_back(child);
if(foundLast && indices.find(cliqueVar) != indices.end()) break;
throw MarginalizeNonleafException(ordering_.key(j), params_.keyFormatter); }
if(!foundLast) { }
++ nFrontals; }
if(cliqueVar == *jI) BOOST_FOREACH(const sharedClique& childToRemove, subtreesToRemove) {
indices.erase(jI++); // j gets advanced here const Cliques removedCliques = this->removeSubtree(childToRemove); // Remove the subtree and throw away the cliques
BOOST_FOREACH(const sharedClique& removedClique, removedCliques) {
marginalFactors.erase(removedClique); }
// Mark factors for removal
BOOST_FOREACH(Index indexInClique, childToRemove->conditional()->frontals()) {
factorIndicesToRemove.insert(variableIndex_[indexInClique].begin(), variableIndex_[indexInClique].end()); }
}
// Gather remaining children after we removed marginalized subtrees
vector<sharedClique> orphans(clique->children().begin(), clique->children().end());
// Add a factor for the current clique to the linear graph to reeliminate
graph.push_back(clique->conditional()->toFactor());
graph.push_back(clique->cachedFactor());
// Remove the current clique
sharedClique parent = clique->parent();
this->removeClique(clique);
// Mark factors for removal
BOOST_FOREACH(Index indexInClique, clique->conditional()->frontals()) {
factorIndicesToRemove.insert(variableIndex_[indexInClique].begin(), variableIndex_[indexInClique].end()); }
// Reeliminate the linear graph to get the marginal and discard the conditional
const FastSet<Index> cliqueFrontals(clique->conditional()->beginFrontals(), clique->conditional()->endFrontals());
FastSet<Index> cliqueFrontalsToEliminate;
std::set_intersection(cliqueFrontals.begin(), cliqueFrontals.end(), indices.begin(), indices.end(),
std::inserter(cliqueFrontalsToEliminate, cliqueFrontalsToEliminate.end()));
vector<Index> cliqueFrontalsToEliminateV(cliqueFrontalsToEliminate.begin(), cliqueFrontalsToEliminate.end());
pair<GaussianConditional::shared_ptr, GaussianFactorGraph> eliminationResult1 =
graph.eliminate(cliqueFrontalsToEliminateV,
params_.factorization==ISAM2Params::QR ? EliminateQR : EliminatePreferCholesky);
// Add the resulting marginal
BOOST_FOREACH(const GaussianFactor::shared_ptr& marginal, eliminationResult1.second) {
if(marginal)
marginalFactors.insert(make_pair(clique, marginal)); }
// Recover the conditional on the remaining subset of frontal variables
// of this clique being martially marginalized.
vector<Index> newFrontals(
std::find(clique->conditional()->beginFrontals(), clique->conditional()->endFrontals(), j) + 1,
clique->conditional()->endFrontals()); // New frontals are all of those *after* the marginalized key
pair<GaussianConditional::shared_ptr, GaussianFactorGraph> eliminationResult2 =
eliminationResult1.second.eliminate(newFrontals,
params_.factorization==ISAM2Params::QR ? EliminateQR : EliminatePreferCholesky);
// Construct the marginalized clique
sharedClique newClique;
if(params_.factorization == ISAM2Params::QR)
newClique.reset(new Clique(GaussianFactorGraph::EliminationResult(
eliminationResult2.first, boost::make_shared<JacobianFactor>(eliminationResult2.second))));
else else
indices.erase(cliqueVar); newClique.reset(new Clique(GaussianFactorGraph::EliminationResult(
} eliminationResult2.first, boost::make_shared<HessianFactor>(eliminationResult2.second))));
if(cliqueVar == clique_lastIndex->second)
foundLast = true; // Add the marginalized clique to the BayesTree
// Mark factors to be removed this->addClique(newClique, parent);
BOOST_FOREACH(size_t i, variableIndex_[cliqueVar]) {
factorIndicesToRemove.insert(i); // Add the orphans
BOOST_FOREACH(const sharedClique& orphan, orphans) {
this->addClique(orphan, newClique); }
} }
} }
} }
// Reeliminate the factored conditional + marginal that was previously making up the clique // At this point we have updated the BayesTree, now update the remaining iSAM2 data structures
GaussianFactorGraph cliqueGraph;
cliqueGraph.push_back(clique_lastIndex->first->conditional()->toFactor());
assert(clique_lastIndex->first->cachedFactor()); // Assert that we have the cached marginal piece
cliqueGraph.push_back(clique_lastIndex->first->cachedFactor());
pair<Clique::sharedConditional, GaussianFactor::shared_ptr> eliminationResult =
params_.factorization==ISAM2Params::QR ?
EliminateQR(cliqueGraph, nFrontals) :
EliminatePreferCholesky(cliqueGraph, nFrontals);
// Now we discard the conditional part and add the marginal part back into // Gather factors to add - the new marginal factors
// the graph. Also we need to rebuild the leaf clique using the marginal. GaussianFactorGraph factorsToAdd;
typedef pair<sharedClique, GaussianFactor::shared_ptr> Clique_Factor;
// Add the marginal into the factor graph BOOST_FOREACH(const Clique_Factor& clique_factor, marginalFactors) {
factorsToAdd.push_back(eliminationResult.second); if(clique_factor.second)
factorsToAdd.push_back(clique_factor.second);
// Get the parent of the clique to be removed nonlinearFactors_.push_back(boost::make_shared<LinearContainerFactor>(
sharedClique parent = clique_lastIndex->first->parent(); clique_factor.second, ordering_));
if(params_.cacheLinearizedFactors)
// Remove the clique linearFactors_.push_back(clique_factor.second);
this->removeClique(clique_lastIndex->first); BOOST_FOREACH(Index factorIndex, *clique_factor.second) {
fixedVariables_.insert(ordering_.key(factorIndex)); }
// Add a new leaf clique if necessary
const size_t newnFrontals = originalnFrontals - nFrontals;
if(newnFrontals > 0) {
// Do the elimination for the new leaf clique
GaussianFactorGraph newCliqueGraph;
newCliqueGraph.push_back(eliminationResult.second);
pair<Clique::sharedConditional, GaussianFactor::shared_ptr> newEliminationResult =
params_.factorization==ISAM2Params::QR ?
EliminateQR(newCliqueGraph, newnFrontals) :
EliminatePreferCholesky(newCliqueGraph, newnFrontals);
// Create and add the new clique
this->addClique(ISAM2Clique::Create(newEliminationResult), parent);
}
} }
variableIndex_.augment(factorsToAdd); // Augment the variable index
// Remove any factors touching the marginalized-out variables // Remove the factors to remove that have been summarized in the newly-added marginal factors
vector<size_t> removedFactorIndices; vector<size_t> removedFactorIndices;
SymbolicFactorGraph removedFactors; SymbolicFactorGraph removedFactors;
BOOST_FOREACH(size_t i, factorIndicesToRemove) { BOOST_FOREACH(size_t i, factorIndicesToRemove) {
@ -917,18 +945,6 @@ void ISAM2::experimentalMarginalizeLeaves(const FastList<Key>& leafKeys)
} }
variableIndex_.remove(removedFactorIndices, removedFactors); variableIndex_.remove(removedFactorIndices, removedFactors);
// Add the new factors and fix linearization points of involved variables
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factorsToAdd) {
nonlinearFactors_.push_back(boost::make_shared<LinearContainerFactor>(
factor, ordering_));
if(params_.cacheLinearizedFactors)
linearFactors_.push_back(factor);
BOOST_FOREACH(Index factorIndex, *factor) {
fixedVariables_.insert(ordering_.key(factorIndex));
}
}
variableIndex_.augment(factorsToAdd); // Augment the variable index
// Remove the marginalized variables // Remove the marginalized variables
Impl::RemoveVariables(FastSet<Key>(leafKeys.begin(), leafKeys.end()), root_, theta_, variableIndex_, delta_, deltaNewton_, RgProd_, Impl::RemoveVariables(FastSet<Key>(leafKeys.begin(), leafKeys.end()), root_, theta_, variableIndex_, delta_, deltaNewton_, RgProd_,
deltaReplacedMask_, ordering_, nodes_, linearFactors_, fixedVariables_); deltaReplacedMask_, ordering_, nodes_, linearFactors_, fixedVariables_);

119
tests/testGaussianISAM2.cpp
View File

@ -884,7 +884,68 @@ namespace {
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST_UNSAFE(ISAM2, marginalizeLeaves) TEST_UNSAFE(ISAM2, marginalizeLeaves1)
{
ISAM2 isam;
NonlinearFactorGraph factors;
factors.add(PriorFactor<LieVector>(0, LieVector(0.0), noiseModel::Unit::Create(1)));
factors.add(BetweenFactor<LieVector>(0, 1, LieVector(0.0), noiseModel::Unit::Create(1)));
factors.add(BetweenFactor<LieVector>(1, 2, LieVector(0.0), noiseModel::Unit::Create(1)));
factors.add(BetweenFactor<LieVector>(0, 2, LieVector(0.0), noiseModel::Unit::Create(1)));
Values values;
values.insert(0, LieVector(0.0));
values.insert(1, LieVector(0.0));
values.insert(2, LieVector(0.0));
FastMap<Key,int> constrainedKeys;
constrainedKeys.insert(make_pair(0,0));
constrainedKeys.insert(make_pair(1,1));
constrainedKeys.insert(make_pair(2,2));
isam.update(factors, values, FastVector<size_t>(), constrainedKeys);
FastList<Key> leafKeys;
leafKeys.push_back(isam.getOrdering().key(0));
EXPECT(checkMarginalizeLeaves(isam, leafKeys));
}
/* ************************************************************************* */
TEST_UNSAFE(ISAM2, marginalizeLeaves2)
{
ISAM2 isam;
NonlinearFactorGraph factors;
factors.add(PriorFactor<LieVector>(0, LieVector(0.0), noiseModel::Unit::Create(1)));
factors.add(BetweenFactor<LieVector>(0, 1, LieVector(0.0), noiseModel::Unit::Create(1)));
factors.add(BetweenFactor<LieVector>(1, 2, LieVector(0.0), noiseModel::Unit::Create(1)));
factors.add(BetweenFactor<LieVector>(0, 2, LieVector(0.0), noiseModel::Unit::Create(1)));
factors.add(BetweenFactor<LieVector>(2, 3, LieVector(0.0), noiseModel::Unit::Create(1)));
Values values;
values.insert(0, LieVector(0.0));
values.insert(1, LieVector(0.0));
values.insert(2, LieVector(0.0));
values.insert(3, LieVector(0.0));
FastMap<Key,int> constrainedKeys;
constrainedKeys.insert(make_pair(0,0));
constrainedKeys.insert(make_pair(1,1));
constrainedKeys.insert(make_pair(2,2));
constrainedKeys.insert(make_pair(3,3));
isam.update(factors, values, FastVector<size_t>(), constrainedKeys);
FastList<Key> leafKeys;
leafKeys.push_back(isam.getOrdering().key(0));
EXPECT(checkMarginalizeLeaves(isam, leafKeys));
}
/* ************************************************************************* */
TEST_UNSAFE(ISAM2, marginalizeLeaves3)
{ {
ISAM2 isam; ISAM2 isam;
@ -909,15 +970,65 @@ TEST_UNSAFE(ISAM2, marginalizeLeaves)
values.insert(4, LieVector(0.0)); values.insert(4, LieVector(0.0));
values.insert(5, LieVector(0.0)); values.insert(5, LieVector(0.0));
isam.update(factors, values); FastMap<Key,int> constrainedKeys;
constrainedKeys.insert(make_pair(0,0));
constrainedKeys.insert(make_pair(1,1));
constrainedKeys.insert(make_pair(2,2));
constrainedKeys.insert(make_pair(3,3));
constrainedKeys.insert(make_pair(4,4));
constrainedKeys.insert(make_pair(5,5));
isam.update(factors, values, FastVector<size_t>(), constrainedKeys);
FastList<Key> leafKeys; FastList<Key> leafKeys;
leafKeys.push_back(0); leafKeys.push_back(isam.getOrdering().key(0));
EXPECT(checkMarginalizeLeaves(isam, leafKeys)); EXPECT(checkMarginalizeLeaves(isam, leafKeys));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST_UNSAFE(ISAM2, marginalizeLeaves2) TEST_UNSAFE(ISAM2, marginalizeLeaves4)
{
ISAM2 isam;
NonlinearFactorGraph factors;
factors.add(PriorFactor<LieVector>(0, LieVector(0.0), noiseModel::Unit::Create(1)));
factors.add(BetweenFactor<LieVector>(0, 1, LieVector(0.0), noiseModel::Unit::Create(1)));
factors.add(BetweenFactor<LieVector>(1, 2, LieVector(0.0), noiseModel::Unit::Create(1)));
factors.add(BetweenFactor<LieVector>(0, 2, LieVector(0.0), noiseModel::Unit::Create(1)));
factors.add(BetweenFactor<LieVector>(2, 3, LieVector(0.0), noiseModel::Unit::Create(1)));
factors.add(BetweenFactor<LieVector>(3, 4, LieVector(0.0), noiseModel::Unit::Create(1)));
factors.add(BetweenFactor<LieVector>(4, 5, LieVector(0.0), noiseModel::Unit::Create(1)));
factors.add(BetweenFactor<LieVector>(3, 5, LieVector(0.0), noiseModel::Unit::Create(1)));
Values values;
values.insert(0, LieVector(0.0));
values.insert(1, LieVector(0.0));
values.insert(2, LieVector(0.0));
values.insert(3, LieVector(0.0));
values.insert(4, LieVector(0.0));
values.insert(5, LieVector(0.0));
FastMap<Key,int> constrainedKeys;
constrainedKeys.insert(make_pair(0,0));
constrainedKeys.insert(make_pair(1,1));
constrainedKeys.insert(make_pair(2,2));
constrainedKeys.insert(make_pair(3,3));
constrainedKeys.insert(make_pair(4,4));
constrainedKeys.insert(make_pair(5,5));
isam.update(factors, values, FastVector<size_t>(), constrainedKeys);
FastList<Key> leafKeys;
leafKeys.push_back(isam.getOrdering().key(0));
leafKeys.push_back(isam.getOrdering().key(1));
EXPECT(checkMarginalizeLeaves(isam, leafKeys));
}
/* ************************************************************************* */
TEST_UNSAFE(ISAM2, marginalizeLeaves5)
{ {
// Create isam2 // Create isam2
ISAM2 isam = createSlamlikeISAM2(); ISAM2 isam = createSlamlikeISAM2();