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

Fixed bugs in iSAM2 leaf marginalization found in HMF testing - set of factors to remove, adding new leaf clique from remaining variables. Also relies on bug fix in BayesTree::removeClique a couple of commits ago.

release/4.3a0
Richard Roberts 2013-03-05 05:47:27 +00:00
parent fe8fc6dd24
commit 72db4793c5
2 changed files with 115 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
gtsam/nonlinear/ISAM2.cpp
View File

@ -792,6 +792,7 @@ void ISAM2::experimentalMarginalizeLeaves(const FastList<Key>& leafKeys)
BOOST_FOREACH(Key key, leafKeys) {
indices.insert(ordering_[key]);
}
FastSet<Index> origIndices = indices;
// For each clique containing variables to be marginalized, we need to
// reeliminate the marginalized variables and add their linear contribution
@ -823,7 +824,7 @@ void ISAM2::experimentalMarginalizeLeaves(const FastList<Key>& leafKeys)
}
#endif
// Now loop over the indices
// Now loop over the indices, the iterator jI is advanced inside the loop.
FastSet<size_t> factorIndicesToRemove;
GaussianFactorGraph factorsToAdd;
for(FastSet<Index>::iterator jI = indices.begin(); jI != indices.end(); )
@ -834,22 +835,19 @@ void ISAM2::experimentalMarginalizeLeaves(const FastList<Key>& leafKeys)
FastMap<sharedClique,Index>::iterator clique_lastIndex = cliquesToMarginalize.find(nodes_[*jI]);
assert(clique_lastIndex != cliquesToMarginalize.end()); // Assert that we indexed the clique
const size_t originalnFrontals = clique_lastIndex->first->conditional()->nrFrontals();
// Check that the clique has no children
if(!clique_lastIndex->first->children().empty())
throw MarginalizeNonleafException(ordering_.key(*jI), params_.keyFormatter);
// Mark factors to be removed
BOOST_FOREACH(size_t i, variableIndex_[*jI]) {
factorIndicesToRemove.insert(i);
}
// Check that all previous variables in the clique are also being eliminated and no later ones.
// At the same time, remove the indices marginalized with this clique from the indices set.
// This is where the iterator j is advanced.
size_t nFrontals = 0;
{
bool foundLast = false;
BOOST_FOREACH(Index cliqueVar, *clique_lastIndex->first->conditional()) {
BOOST_FOREACH(Index cliqueVar, clique_lastIndex->first->conditional()->frontals()) {
if(!foundLast && indices.find(cliqueVar) == indices.end())
throw MarginalizeNonleafException(ordering_.key(j), params_.keyFormatter);
if(foundLast && indices.find(cliqueVar) != indices.end())
@ -863,6 +861,10 @@ void ISAM2::experimentalMarginalizeLeaves(const FastList<Key>& leafKeys)
}
if(cliqueVar == clique_lastIndex->second)
foundLast = true;
// Mark factors to be removed
BOOST_FOREACH(size_t i, variableIndex_[cliqueVar]) {
factorIndicesToRemove.insert(i);
}
}
}
@ -876,11 +878,31 @@ void ISAM2::experimentalMarginalizeLeaves(const FastList<Key>& leafKeys)
EliminateQR(cliqueGraph, nFrontals) :
EliminatePreferCholesky(cliqueGraph, nFrontals);
// Now we discard the conditional part and add the marginal part back into
// the graph. Also we need to rebuild the leaf clique using the marginal.
// Add the marginal into the factor graph
factorsToAdd.push_back(eliminationResult.second);
// Get the parent of the clique to be removed
sharedClique parent = clique_lastIndex->first->parent();
// Remove the clique
this->removeClique(clique_lastIndex->first);
// 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);
}
}
// Remove any factors touching the marginalized-out variables
@ -893,6 +915,7 @@ void ISAM2::experimentalMarginalizeLeaves(const FastList<Key>& leafKeys)
if(params_.cacheLinearizedFactors)
linearFactors_.remove(i);
}
variableIndex_.remove(removedFactorIndices, removedFactors);
// Add the new factors and fix linearization points of involved variables
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factorsToAdd) {
@ -907,7 +930,6 @@ void ISAM2::experimentalMarginalizeLeaves(const FastList<Key>& leafKeys)
variableIndex_.augment(factorsToAdd); // Augment the variable index
// Remove the marginalized variables
variableIndex_.remove(removedFactorIndices, removedFactors);
Impl::RemoveVariables(FastSet<Key>(leafKeys.begin(), leafKeys.end()), root_, theta_, variableIndex_, delta_, deltaNewton_, RgProd_,
deltaReplacedMask_, ordering_, nodes_, linearFactors_, fixedVariables_);
}

109
tests/testGaussianISAM2.cpp
View File

@ -6,6 +6,7 @@
#include <gtsam/base/debug.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/inference/SymbolicFactorGraph.h>
@ -832,41 +833,99 @@ TEST(ISAM2, slamlike_solution_partial_relinearization_check)
CHECK(isam_check(fullgraph, fullinit, isam, *this, result_));
}
/* ************************************************************************* */
TEST_UNSAFE(ISAM2, marginalizeLeaves)
{
// Create isam2
ISAM2 isam = createSlamlikeISAM2();
// Get linearization point
Values soln = isam.calculateBestEstimate();
// Calculate expected marginal
GaussianFactorGraph isamAsGraph(isam);
GaussianSequentialSolver solver(isamAsGraph);
namespace {
bool checkMarginalizeLeaves(ISAM2& isam, const FastList<Key>& leafKeys) {
Matrix expectedAugmentedHessian, expected3AugmentedHessian;
vector<Index> toKeep;
const Index lastVar = isam.getOrdering().size() - 1;
for(Index i=0; i<=lastVar; ++i)
if(i != isam.getOrdering()[0])
if(find(leafKeys.begin(), leafKeys.end(), isam.getOrdering().key(i)) == leafKeys.end())
toKeep.push_back(i);
GaussianFactorGraph marginalgfg = *solver.jointFactorGraph(toKeep);
vector<Index> toFrontI;
toFrontI.push_back(isam.getOrdering()[0]);
Permutation toFront = Permutation::PullToFront(toFrontI, lastVar+1);
marginalgfg.permuteWithInverse(*toFront.inverse());
Matrix expectedAugmentedHessian = marginalgfg.augmentedHessian();
// Marginalize
FastList<Key> marginalizeKeys;
marginalizeKeys.push_back(isam.getOrdering().key(0));
isam.experimentalMarginalizeLeaves(marginalizeKeys);
// Calculate expected marginal from iSAM2 tree
GaussianFactorGraph isamAsGraph(isam);
GaussianSequentialSolver solver(isamAsGraph);
GaussianFactorGraph marginalgfg = *solver.jointFactorGraph(toKeep);
expectedAugmentedHessian = marginalgfg.augmentedHessian();
//// Calculate expected marginal from cached linear factors
//assert(isam.params().cacheLinearizedFactors);
//GaussianSequentialSolver solver2(isam.linearFactors_, isam.params().factorization == ISAM2Params::QR);
//expected2AugmentedHessian = solver2.jointFactorGraph(toKeep)->augmentedHessian();
// Calculate expected marginal from original nonlinear factors
GaussianSequentialSolver solver3(
*isam.getFactorsUnsafe().linearize(isam.getLinearizationPoint(), isam.getOrdering()),
isam.params().factorization == ISAM2Params::QR);
expected3AugmentedHessian = solver3.jointFactorGraph(toKeep)->augmentedHessian();
// Do marginalization
isam.experimentalMarginalizeLeaves(leafKeys);
// Check
GaussianFactorGraph actualMarginalGraph(isam);
Matrix actualAugmentedHessian = actualMarginalGraph.augmentedHessian();
//Matrix actual2AugmentedHessian = linearFactors_.augmentedHessian();
Matrix actual3AugmentedHessian = isam.getFactorsUnsafe().linearize(
isam.getLinearizationPoint(), isam.getOrdering())->augmentedHessian();
assert(actualAugmentedHessian.unaryExpr(std::ptr_fun(&std::isfinite<double>)).all());
LONGS_EQUAL(lastVar-1, isam.getOrdering().size()-1);
EXPECT(assert_equal(expectedAugmentedHessian, actualAugmentedHessian));
// Check full marginalization
bool treeEqual = assert_equal(expectedAugmentedHessian, actualAugmentedHessian, 1e-6);
//bool linEqual = assert_equal(expected2AugmentedHessian, actualAugmentedHessian, 1e-6);
bool nonlinEqual = assert_equal(expected3AugmentedHessian, actualAugmentedHessian, 1e-6);
//bool linCorrect = assert_equal(expected3AugmentedHessian, expected2AugmentedHessian, 1e-6);
//actual2AugmentedHessian.bottomRightCorner(1,1) = expected3AugmentedHessian.bottomRightCorner(1,1); bool afterLinCorrect = assert_equal(expected3AugmentedHessian, actual2AugmentedHessian, 1e-6);
actual3AugmentedHessian.bottomRightCorner(1,1) = expected3AugmentedHessian.bottomRightCorner(1,1); bool afterNonlinCorrect = assert_equal(expected3AugmentedHessian, actual3AugmentedHessian, 1e-6);
bool ok = treeEqual && /*linEqual &&*/ nonlinEqual && /*linCorrect &&*/ /*afterLinCorrect &&*/ afterNonlinCorrect;
return ok;
}
}
/* ************************************************************************* */
TEST_UNSAFE(ISAM2, marginalizeLeaves)
{
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));
isam.update(factors, values);
FastList<Key> leafKeys;
leafKeys.push_back(0);
EXPECT(checkMarginalizeLeaves(isam, leafKeys));
}
/* ************************************************************************* */
TEST_UNSAFE(ISAM2, marginalizeLeaves2)
{
// Create isam2
ISAM2 isam = createSlamlikeISAM2();
// Marginalize
FastList<Key> marginalizeKeys;
marginalizeKeys.push_back(isam.getOrdering().key(0));
EXPECT(checkMarginalizeLeaves(isam, marginalizeKeys));
}
/* ************************************************************************* */