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

Further fix in iSAM2 marginalization

release/4.3a0
Richard Roberts 2013-03-20 13:48:16 +00:00
parent 38ee915669
commit e2aca2039e
2 changed files with 46 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

72
gtsam/nonlinear/ISAM2.cpp
View File

@ -797,10 +797,9 @@ void ISAM2::experimentalMarginalizeLeaves(const FastList<Key>& leafKeys)
// Keep track of marginal factors - map from clique to the marginal factors // Keep track of marginal factors - map from clique to the marginal factors
// that should be incorporated into it, passed up from it's children. // that should be incorporated into it, passed up from it's children.
multimap<sharedClique, GaussianFactor::shared_ptr> marginalFactors; multimap<sharedClique, GaussianFactor::shared_ptr> marginalFactors;
FastSet<size_t> factorIndicesToRemove;
// Remove each variable and its subtrees // 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 if(nodes_[j]) { // If the index was not already removed by removing another subtree
sharedClique clique = nodes_[j]; sharedClique clique = nodes_[j];
@ -825,9 +824,9 @@ void ISAM2::experimentalMarginalizeLeaves(const FastList<Key>& leafKeys)
const Cliques removedCliques = this->removeSubtree(clique); // Remove the subtree and throw away the cliques const Cliques removedCliques = this->removeSubtree(clique); // Remove the subtree and throw away the cliques
BOOST_FOREACH(const sharedClique& removedClique, removedCliques) { BOOST_FOREACH(const sharedClique& removedClique, removedCliques) {
marginalFactors.erase(removedClique); marginalFactors.erase(removedClique);
// Mark factors for removal
BOOST_FOREACH(Index indexInClique, removedClique->conditional()->frontals()) { 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 { else {
@ -839,39 +838,50 @@ void ISAM2::experimentalMarginalizeLeaves(const FastList<Key>& leafKeys)
// Add child marginals and remove marginalized subtrees // Add child marginals and remove marginalized subtrees
GaussianFactorGraph graph; GaussianFactorGraph graph;
FastSet<size_t> factorsInSubtreeRoot;
Cliques subtreesToRemove; Cliques subtreesToRemove;
BOOST_FOREACH(const sharedClique& child, clique->children()) { BOOST_FOREACH(const sharedClique& child, clique->children()) {
graph.push_back(child->cachedFactor()); // Add child marginal
// Remove subtree if child depends on any marginalized keys // Remove subtree if child depends on any marginalized keys
BOOST_FOREACH(Index parentIndex, child->conditional()->parents()) { BOOST_FOREACH(Index parentIndex, child->conditional()->parents()) {
if(indices.find(parentIndex) != indices.end()) { if(indices.find(parentIndex) != indices.end()) {
subtreesToRemove.push_back(child); subtreesToRemove.push_back(child);
graph.push_back(child->cachedFactor()); // Add child marginal
break; break;
} }
} }
} }
Cliques childrenRemoved;
BOOST_FOREACH(const sharedClique& childToRemove, subtreesToRemove) { BOOST_FOREACH(const sharedClique& childToRemove, subtreesToRemove) {
const Cliques removedCliques = this->removeSubtree(childToRemove); // Remove the subtree and throw away the cliques 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) { BOOST_FOREACH(const sharedClique& removedClique, removedCliques) {
marginalFactors.erase(removedClique); } marginalFactors.erase(removedClique);
// Mark factors for removal BOOST_FOREACH(Index indexInClique, removedClique->conditional()->frontals()) {
BOOST_FOREACH(Index indexInClique, childToRemove->conditional()->frontals()) { if(indices.find(indexInClique) == indices.end())
factorIndicesToRemove.insert(variableIndex_[indexInClique].begin(), variableIndex_[indexInClique].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 // Gather remaining children after we removed marginalized subtrees
vector<sharedClique> orphans(clique->children().begin(), clique->children().end()); vector<sharedClique> orphans(clique->children().begin(), clique->children().end());
// Add a factor for the current clique to the linear graph to reeliminate // Add the factors that are pulled into the current clique by the marginalized variables.
graph.push_back(clique->conditional()->toFactor()); // These are the factors that involve *marginalized* frontal variables in this clique
graph.push_back(clique->cachedFactor()); // but do not involve frontal variables of any of its children.
FastSet<size_t> 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 // Remove the current clique
sharedClique parent = clique->parent(); sharedClique parent = clique->parent();
this->removeClique(clique); 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 // Reeliminate the linear graph to get the marginal and discard the conditional
const FastSet<Index> cliqueFrontals(clique->conditional()->beginFrontals(), clique->conditional()->endFrontals()); const FastSet<Index> cliqueFrontals(clique->conditional()->beginFrontals(), clique->conditional()->endFrontals());
@ -890,21 +900,20 @@ void ISAM2::experimentalMarginalizeLeaves(const FastList<Key>& leafKeys)
// Recover the conditional on the remaining subset of frontal variables // Recover the conditional on the remaining subset of frontal variables
// of this clique being martially marginalized. // of this clique being martially marginalized.
vector<Index> newFrontals( size_t nToEliminate = std::find(clique->conditional()->beginFrontals(), clique->conditional()->endFrontals(), j) - clique->conditional()->begin() + 1;
std::find(clique->conditional()->beginFrontals(), clique->conditional()->endFrontals(), j) + 1, GaussianFactorGraph graph2;
clique->conditional()->endFrontals()); // New frontals are all of those *after* the marginalized key graph2.push_back(clique->conditional()->toFactor());
pair<GaussianConditional::shared_ptr, GaussianFactorGraph> eliminationResult2 = GaussianFactorGraph::EliminationResult eliminationResult2 =
eliminationResult1.second.eliminate(newFrontals, params_.factorization == ISAM2Params::QR ?
params_.factorization==ISAM2Params::QR ? EliminateQR : EliminatePreferCholesky); EliminateQR(graph2, nToEliminate) :
EliminatePreferCholesky(graph2, nToEliminate);
// Construct the marginalized clique GaussianFactorGraph graph3;
sharedClique newClique; graph3.push_back(eliminationResult2.second);
if(params_.factorization == ISAM2Params::QR) GaussianFactorGraph::EliminationResult eliminationResult3 =
newClique.reset(new Clique(GaussianFactorGraph::EliminationResult( params_.factorization == ISAM2Params::QR ?
eliminationResult2.first, boost::make_shared<JacobianFactor>(eliminationResult2.second)))); EliminateQR(graph3, clique->conditional()->nrFrontals() - nToEliminate) :
else EliminatePreferCholesky(graph3, clique->conditional()->nrFrontals() - nToEliminate);
newClique.reset(new Clique(GaussianFactorGraph::EliminationResult( sharedClique newClique = boost::make_shared<Clique>(make_pair(eliminationResult3.first, clique->cachedFactor()));
eliminationResult2.first, boost::make_shared<HessianFactor>(eliminationResult2.second))));
// Add the marginalized clique to the BayesTree // Add the marginalized clique to the BayesTree
this->addClique(newClique, parent); this->addClique(newClique, parent);
@ -934,6 +943,9 @@ void ISAM2::experimentalMarginalizeLeaves(const FastList<Key>& leafKeys)
variableIndex_.augment(factorsToAdd); // Augment the variable index variableIndex_.augment(factorsToAdd); // Augment the variable index
// Remove the factors to remove that have been summarized in the newly-added marginal factors // Remove the factors to remove that have been summarized in the newly-added marginal factors
FastSet<size_t> factorIndicesToRemove;
BOOST_FOREACH(Index j, indices) {
factorIndicesToRemove.insert(variableIndex_[j].begin(), variableIndex_[j].end()); }
vector<size_t> removedFactorIndices; vector<size_t> removedFactorIndices;
SymbolicFactorGraph removedFactors; SymbolicFactorGraph removedFactors;
BOOST_FOREACH(size_t i, factorIndicesToRemove) { BOOST_FOREACH(size_t i, factorIndicesToRemove) {

20
tests/testGaussianISAM2.cpp
View File

@ -871,11 +871,14 @@ namespace {
assert(actualAugmentedHessian.unaryExpr(std::ptr_fun(&std::isfinite<double>)).all()); assert(actualAugmentedHessian.unaryExpr(std::ptr_fun(&std::isfinite<double>)).all());
// Check full marginalization // Check full marginalization
//cout << "treeEqual" << endl;
bool treeEqual = assert_equal(expectedAugmentedHessian, actualAugmentedHessian, 1e-6); bool treeEqual = assert_equal(expectedAugmentedHessian, actualAugmentedHessian, 1e-6);
//bool linEqual = assert_equal(expected2AugmentedHessian, actualAugmentedHessian, 1e-6); //bool linEqual = assert_equal(expected2AugmentedHessian, actualAugmentedHessian, 1e-6);
//cout << "nonlinEqual" << endl;
bool nonlinEqual = assert_equal(expected3AugmentedHessian, actualAugmentedHessian, 1e-6); bool nonlinEqual = assert_equal(expected3AugmentedHessian, actualAugmentedHessian, 1e-6);
//bool linCorrect = assert_equal(expected3AugmentedHessian, expected2AugmentedHessian, 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); //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); actual3AugmentedHessian.bottomRightCorner(1,1) = expected3AugmentedHessian.bottomRightCorner(1,1); bool afterNonlinCorrect = assert_equal(expected3AugmentedHessian, actual3AugmentedHessian, 1e-6);
bool ok = treeEqual && /*linEqual &&*/ nonlinEqual && /*linCorrect &&*/ /*afterLinCorrect &&*/ afterNonlinCorrect; bool ok = treeEqual && /*linEqual &&*/ nonlinEqual && /*linCorrect &&*/ /*afterLinCorrect &&*/ afterNonlinCorrect;
@ -992,37 +995,22 @@ TEST_UNSAFE(ISAM2, marginalizeLeaves4)
NonlinearFactorGraph factors; NonlinearFactorGraph factors;
factors.add(PriorFactor<LieVector>(0, LieVector(0.0), noiseModel::Unit::Create(1))); 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>(0, 2, LieVector(0.0), noiseModel::Unit::Create(1)));
factors.add(BetweenFactor<LieVector>(1, 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 values;
values.insert(0, LieVector(0.0)); values.insert(0, LieVector(0.0));
values.insert(1, LieVector(0.0)); values.insert(1, LieVector(0.0));
values.insert(2, 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; FastMap<Key,int> constrainedKeys;
constrainedKeys.insert(make_pair(0,0)); constrainedKeys.insert(make_pair(0,0));
constrainedKeys.insert(make_pair(1,1)); constrainedKeys.insert(make_pair(1,1));
constrainedKeys.insert(make_pair(2,2)); 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); isam.update(factors, values, FastVector<size_t>(), constrainedKeys);
FastList<Key> leafKeys; FastList<Key> leafKeys;
leafKeys.push_back(isam.getOrdering().key(0));
leafKeys.push_back(isam.getOrdering().key(1)); leafKeys.push_back(isam.getOrdering().key(1));
EXPECT(checkMarginalizeLeaves(isam, leafKeys)); EXPECT(checkMarginalizeLeaves(isam, leafKeys));
} }