From e2aca2039ee89fb7edea7de36cd254746a0b34a9 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Wed, 20 Mar 2013 13:48:16 +0000 Subject: [PATCH] Further fix in iSAM2 marginalization --- gtsam/nonlinear/ISAM2.cpp | 72 +++++++++++++++++++++---------------- tests/testGaussianISAM2.cpp | 20 +++-------- 2 files changed, 46 insertions(+), 46 deletions(-) diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 640e0f87a..5fc86bf99 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -797,10 +797,9 @@ void ISAM2::experimentalMarginalizeLeaves(const FastList& leafKeys) // Keep track of marginal factors - map from clique to the marginal factors // that should be incorporated into it, passed up from it's children. multimap marginalFactors; - FastSet factorIndicesToRemove; // Remove each variable and its subtrees - BOOST_FOREACH(Index j, indices) { + BOOST_REVERSE_FOREACH(Index j, indices) { if(nodes_[j]) { // If the index was not already removed by removing another subtree sharedClique clique = nodes_[j]; @@ -825,9 +824,9 @@ void ISAM2::experimentalMarginalizeLeaves(const FastList& leafKeys) 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()); } + if(indices.find(indexInClique) == indices.end()) + throw runtime_error("Requesting to marginalize variables that are not leaves, the ISAM2 object is now in an inconsistent state so should no longer be used."); } } } else { @@ -839,39 +838,50 @@ void ISAM2::experimentalMarginalizeLeaves(const FastList& leafKeys) // Add child marginals and remove marginalized subtrees GaussianFactorGraph graph; + FastSet factorsInSubtreeRoot; Cliques subtreesToRemove; BOOST_FOREACH(const sharedClique& child, clique->children()) { - graph.push_back(child->cachedFactor()); // Add child marginal // Remove subtree if child depends on any marginalized keys BOOST_FOREACH(Index parentIndex, child->conditional()->parents()) { if(indices.find(parentIndex) != indices.end()) { subtreesToRemove.push_back(child); + graph.push_back(child->cachedFactor()); // Add child marginal break; } } } + Cliques childrenRemoved; BOOST_FOREACH(const sharedClique& childToRemove, subtreesToRemove) { const Cliques removedCliques = this->removeSubtree(childToRemove); // Remove the subtree and throw away the cliques + childrenRemoved.insert(childrenRemoved.end(), removedCliques.begin(), removedCliques.end()); 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()); } + marginalFactors.erase(removedClique); + BOOST_FOREACH(Index indexInClique, removedClique->conditional()->frontals()) { + if(indices.find(indexInClique) == indices.end()) + throw runtime_error("Requesting to marginalize variables that are not leaves, the ISAM2 object is now in an inconsistent state so should no longer be used."); } + } } // Gather remaining children after we removed marginalized subtrees vector 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()); + // Add the factors that are pulled into the current clique by the marginalized variables. + // These are the factors that involve *marginalized* frontal variables in this clique + // but do not involve frontal variables of any of its children. + FastSet factorsFromMarginalizedInClique; + BOOST_FOREACH(Index indexInClique, clique->conditional()->frontals()) { + if(indices.find(indexInClique) != indices.end()) + factorsFromMarginalizedInClique.insert(variableIndex_[indexInClique].begin(), variableIndex_[indexInClique].end()); } + BOOST_FOREACH(const sharedClique& removedChild, childrenRemoved) { + BOOST_FOREACH(Index indexInClique, removedChild->conditional()->frontals()) { + BOOST_FOREACH(size_t factorInvolving, variableIndex_[indexInClique]) { + factorsFromMarginalizedInClique.erase(factorInvolving); } } } + BOOST_FOREACH(size_t i, factorsFromMarginalizedInClique) { + graph.push_back(nonlinearFactors_[i]->linearize(theta_, ordering_)); } // 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 cliqueFrontals(clique->conditional()->beginFrontals(), clique->conditional()->endFrontals()); @@ -890,21 +900,20 @@ void ISAM2::experimentalMarginalizeLeaves(const FastList& leafKeys) // Recover the conditional on the remaining subset of frontal variables // of this clique being martially marginalized. - vector 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 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(eliminationResult2.second)))); - else - newClique.reset(new Clique(GaussianFactorGraph::EliminationResult( - eliminationResult2.first, boost::make_shared(eliminationResult2.second)))); + size_t nToEliminate = std::find(clique->conditional()->beginFrontals(), clique->conditional()->endFrontals(), j) - clique->conditional()->begin() + 1; + GaussianFactorGraph graph2; + graph2.push_back(clique->conditional()->toFactor()); + GaussianFactorGraph::EliminationResult eliminationResult2 = + params_.factorization == ISAM2Params::QR ? + EliminateQR(graph2, nToEliminate) : + EliminatePreferCholesky(graph2, nToEliminate); + GaussianFactorGraph graph3; + graph3.push_back(eliminationResult2.second); + GaussianFactorGraph::EliminationResult eliminationResult3 = + params_.factorization == ISAM2Params::QR ? + EliminateQR(graph3, clique->conditional()->nrFrontals() - nToEliminate) : + EliminatePreferCholesky(graph3, clique->conditional()->nrFrontals() - nToEliminate); + sharedClique newClique = boost::make_shared(make_pair(eliminationResult3.first, clique->cachedFactor())); // Add the marginalized clique to the BayesTree this->addClique(newClique, parent); @@ -934,6 +943,9 @@ void ISAM2::experimentalMarginalizeLeaves(const FastList& leafKeys) variableIndex_.augment(factorsToAdd); // Augment the variable index // Remove the factors to remove that have been summarized in the newly-added marginal factors + FastSet factorIndicesToRemove; + BOOST_FOREACH(Index j, indices) { + factorIndicesToRemove.insert(variableIndex_[j].begin(), variableIndex_[j].end()); } vector removedFactorIndices; SymbolicFactorGraph removedFactors; BOOST_FOREACH(size_t i, factorIndicesToRemove) { diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 3f13e49a5..1db7fc245 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -871,11 +871,14 @@ namespace { assert(actualAugmentedHessian.unaryExpr(std::ptr_fun(&std::isfinite)).all()); // Check full marginalization + //cout << "treeEqual" << endl; bool treeEqual = assert_equal(expectedAugmentedHessian, actualAugmentedHessian, 1e-6); //bool linEqual = assert_equal(expected2AugmentedHessian, actualAugmentedHessian, 1e-6); + //cout << "nonlinEqual" << endl; 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); + //cout << "nonlinCorrect" << endl; actual3AugmentedHessian.bottomRightCorner(1,1) = expected3AugmentedHessian.bottomRightCorner(1,1); bool afterNonlinCorrect = assert_equal(expected3AugmentedHessian, actual3AugmentedHessian, 1e-6); bool ok = treeEqual && /*linEqual &&*/ nonlinEqual && /*linCorrect &&*/ /*afterLinCorrect &&*/ afterNonlinCorrect; @@ -992,37 +995,22 @@ TEST_UNSAFE(ISAM2, marginalizeLeaves4) NonlinearFactorGraph factors; factors.add(PriorFactor(0, LieVector(0.0), noiseModel::Unit::Create(1))); - - factors.add(BetweenFactor(0, 1, LieVector(0.0), noiseModel::Unit::Create(1))); - factors.add(BetweenFactor(1, 2, LieVector(0.0), noiseModel::Unit::Create(1))); factors.add(BetweenFactor(0, 2, LieVector(0.0), noiseModel::Unit::Create(1))); - - factors.add(BetweenFactor(2, 3, LieVector(0.0), noiseModel::Unit::Create(1))); - - factors.add(BetweenFactor(3, 4, LieVector(0.0), noiseModel::Unit::Create(1))); - factors.add(BetweenFactor(4, 5, LieVector(0.0), noiseModel::Unit::Create(1))); - factors.add(BetweenFactor(3, 5, LieVector(0.0), noiseModel::Unit::Create(1))); + factors.add(BetweenFactor(1, 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)); - values.insert(3, LieVector(0.0)); - values.insert(4, LieVector(0.0)); - values.insert(5, LieVector(0.0)); FastMap 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(), constrainedKeys); FastList leafKeys; - leafKeys.push_back(isam.getOrdering().key(0)); leafKeys.push_back(isam.getOrdering().key(1)); EXPECT(checkMarginalizeLeaves(isam, leafKeys)); }