From 0f951643bd1395a11068a6132f8e773d40c2d268 Mon Sep 17 00:00:00 2001 From: Grady Williams Date: Tue, 15 Feb 2022 13:52:12 -0800 Subject: [PATCH 1/4] Adding failing tests for ISAM2 marginalization --- tests/testGaussianISAM2.cpp | 150 ++++++++++++++++++++++++++++++++++++ 1 file changed, 150 insertions(+) diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index e8e916f046..efe34ee31f 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -11,6 +11,7 @@ #include #include #include +#include #include #include #include @@ -662,6 +663,76 @@ namespace { bool ok = treeEqual && /*linEqual &&*/ nonlinEqual && /*linCorrect &&*/ /*afterLinCorrect &&*/ afterNonlinCorrect; return ok; } + + boost::optional> createOrderingConstraints(const ISAM2& isam, const KeyVector& newKeys, const KeySet& marginalizableKeys) + { + if (marginalizableKeys.empty()) { + return boost::none; + } else { + FastMap constrainedKeys = FastMap(); + // Generate ordering constraints so that the marginalizable variables will be eliminated first + // Set all existing and new variables to Group1 + for (const auto& key_val : isam.getDelta()) { + constrainedKeys.emplace(key_val.first, 1); + } + for (const auto& key : newKeys) { + constrainedKeys.emplace(key, 1); + } + // And then re-assign the marginalizable variables to Group0 so that they'll all be leaf nodes + for (const auto& key : marginalizableKeys) { + constrainedKeys.at(key) = 0; + } + return constrainedKeys; + } + } + + void markAffectedKeys(const Key& key, const ISAM2Clique::shared_ptr& rootClique, KeyList& additionalKeys) + { + std::stack frontier; + frontier.push(rootClique); + // Basic DFS to find additional keys + while (!frontier.empty()) { + // Get the top of the stack + const ISAM2Clique::shared_ptr clique = frontier.top(); + frontier.pop(); + // Check if we have more keys and children to add + if (std::find(clique->conditional()->beginParents(), clique->conditional()->endParents(), key) != + clique->conditional()->endParents()) { + for (Key i : clique->conditional()->frontals()) { + additionalKeys.push_back(i); + } + for (const ISAM2Clique::shared_ptr& child : clique->children) { + frontier.push(child); + } + } + } + } + + bool updateAndMarginalize(const NonlinearFactorGraph& newFactors, const Values& newValues, const KeySet& marginalizableKeys, ISAM2& isam) + { + // Force ISAM2 to put marginalizable variables at the beginning + const boost::optional> orderingConstraints = createOrderingConstraints(isam, newValues.keys(), marginalizableKeys); + + // Mark additional keys between the marginalized keys and the leaves + KeyList additionalMarkedKeys; + for (Key key : marginalizableKeys) { + ISAM2Clique::shared_ptr clique = isam[key]; + for (const ISAM2Clique::shared_ptr& child : clique->children) { + markAffectedKeys(key, child, additionalMarkedKeys); + } + } + + // Update + isam.update(newFactors, newValues, FactorIndices{}, orderingConstraints, boost::none, additionalMarkedKeys); + + if (!marginalizableKeys.empty()) { + FastList leafKeys(marginalizableKeys.begin(), marginalizableKeys.end()); + return checkMarginalizeLeaves(isam, leafKeys); + } + else { + return true; + } + } } /* ************************************************************************* */ @@ -795,6 +866,85 @@ TEST(ISAM2, marginalizeLeaves5) EXPECT(checkMarginalizeLeaves(isam, marginalizeKeys)); } +/* ************************************************************************* */ +TEST(ISAM2, marginalizeLeaves6) +{ + const boost::shared_ptr nm = noiseModel::Isotropic::Sigma(6, 1.0); + + int gridDim = 10; + + auto idxToKey = [gridDim](int i, int j){return i * gridDim + j;}; + + NonlinearFactorGraph factors; + Values values; + ISAM2 isam; + + // Create a grid of pose variables + for (int i = 0; i < gridDim; ++i) { + for (int j = 0; j < gridDim; ++j) { + Pose3 pose = Pose3(Rot3::identity(), Point3(i, j, 0)); + Key key = idxToKey(i, j); + // Place a prior on the first pose + factors.addPrior(key, Pose3(Rot3::identity(), Point3(i, j, 0)), nm); + values.insert(key, pose); + if (i > 0) { + factors.emplace_shared>(idxToKey(i - 1, j), key, Pose3(Rot3::identity(), Point3(1, 0, 0)),nm); + } + if (j > 0) { + factors.emplace_shared>(idxToKey(i, j - 1), key, Pose3(Rot3::identity(), Point3(0, 1, 0)),nm); + } + } + } + + // Optimize the graph + EXPECT(updateAndMarginalize(factors, values, {}, isam)); + auto estimate = isam.calculateBestEstimate(); + + // Get the list of keys + std::vector key_list(gridDim * gridDim); + std::iota(key_list.begin(), key_list.end(), 0); + + // Shuffle the keys -> we will marginalize the keys one by one in this order + std::shuffle(key_list.begin(), key_list.end(), std::default_random_engine(1234)); + + for (const auto& key : key_list) { + KeySet marginalKeys; + marginalKeys.insert(key); + EXPECT(updateAndMarginalize({}, {}, marginalKeys, isam)); + estimate = isam.calculateBestEstimate(); + } +} + +/* ************************************************************************* */ +TEST(ISAM2, MarginalizeRoot) +{ + const boost::shared_ptr nm = noiseModel::Isotropic::Sigma(6, 1.0); + + NonlinearFactorGraph factors; + Values values; + ISAM2 isam; + + // Create a factor graph with one variable and a prior + Pose3 root = Pose3::identity(); + Key rootKey(0); + values.insert(rootKey, root); + factors.addPrior(rootKey, Pose3::identity(), nm); + + // Optimize the graph + EXPECT(updateAndMarginalize(factors, values, {}, isam)); + auto estimate = isam.calculateBestEstimate(); + EXPECT(estimate.size() == 1); + + // And remove the node from the graph + KeySet marginalizableKeys; + marginalizableKeys.insert(rootKey); + + EXPECT(updateAndMarginalize({}, {}, marginalizableKeys, isam)); + + estimate = isam.calculateBestEstimate(); + EXPECT(estimate.empty()); +} + /* ************************************************************************* */ TEST(ISAM2, marginalCovariance) { From 9e1046f40e21bd66b4db5fe300aa0e46b39365a4 Mon Sep 17 00:00:00 2001 From: Grady Williams Date: Sat, 16 Apr 2022 13:53:39 -0700 Subject: [PATCH 2/4] Test for not increasing graph size when adding marginal factors --- tests/testGaussianISAM2.cpp | 31 +++++++++++++++++++++++++++++++ 1 file changed, 31 insertions(+) diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index efe34ee31f..940051b96a 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -945,6 +945,37 @@ TEST(ISAM2, MarginalizeRoot) EXPECT(estimate.empty()); } +/* ************************************************************************* */ +TEST(ISAM2, marginalizationSize) +{ + const boost::shared_ptr nm = noiseModel::Isotropic::Sigma(6, 1.0); + + NonlinearFactorGraph factors; + Values values; + ISAM2Params params; + params.findUnusedFactorSlots = true; + ISAM2 isam{params}; + + // Create a pose variable + Key aKey(0); + values.insert(aKey, Pose3::identity()); + factors.addPrior(aKey, Pose3::identity(), nm); + // Create another pose variable linked to the first + Pose3 b = Pose3::identity(); + Key bKey(1); + values.insert(bKey, Pose3::identity()); + factors.emplace_shared>(aKey, bKey, Pose3::identity(), nm); + // Optimize the graph + EXPECT(updateAndMarginalize(factors, values, {}, isam)); + + // Now remove a variable -> we should not see the number of factors increase + gtsam::KeySet to_remove; + to_remove.insert(aKey); + const auto numFactorsBefore = isam.getFactorsUnsafe().size(); + updateAndMarginalize({}, {}, to_remove, isam); + EXPECT(numFactorsBefore == isam.getFactorsUnsafe().size()); +} + /* ************************************************************************* */ TEST(ISAM2, marginalCovariance) { From 66720e0b0276360d1581335332714d2c7e1ecc93 Mon Sep 17 00:00:00 2001 From: Grady Williams Date: Sat, 16 Apr 2022 13:29:53 -0700 Subject: [PATCH 3/4] Bugfixes for ISAM2 --- gtsam/nonlinear/ISAM2.cpp | 66 +++++++++++++++++++++++-------------- tests/testGaussianISAM2.cpp | 7 ++-- 2 files changed, 45 insertions(+), 28 deletions(-) diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index f56b23777c..a7feca2398 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -552,9 +552,12 @@ void ISAM2::marginalizeLeaves( // 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[clique->parent()->conditional()->front()].push_back( - marginalFactor); + // parent of this clique. If the clique is a root and has no parent, then + // we can discard it without keeping track of the marginal factor. + if (clique->parent()) { + marginalFactors[clique->parent()->conditional()->front()].push_back( + marginalFactor); + } // Now remove this clique and its subtree - all of its marginal // information has been stored in marginalFactors. trackingRemoveSubtree(clique); @@ -632,7 +635,7 @@ void ISAM2::marginalizeLeaves( // Make the clique's matrix appear as a subset const DenseIndex dimToRemove = cg->matrixObject().offset(nToRemove); - cg->matrixObject().firstBlock() = nToRemove; + cg->matrixObject().firstBlock() += nToRemove; cg->matrixObject().rowStart() = dimToRemove; // Change the keys in the clique @@ -658,42 +661,55 @@ void ISAM2::marginalizeLeaves( // At this point we have updated the BayesTree, now update the remaining iSAM2 // data structures + // Remove the factors to remove that will be summarized in marginal factors + NonlinearFactorGraph removedFactors; + for (const auto index : factorIndicesToRemove) { + removedFactors.push_back(nonlinearFactors_[index]); + nonlinearFactors_.remove(index); + if (params_.cacheLinearizedFactors) { + linearFactors_.remove(index); + } + } + variableIndex_.remove(factorIndicesToRemove.begin(), + factorIndicesToRemove.end(), removedFactors); + // Gather factors to add - the new marginal factors - GaussianFactorGraph factorsToAdd; + GaussianFactorGraph factorsToAdd{}; + NonlinearFactorGraph nonlinearFactorsToAdd{}; for (const auto& key_factors : marginalFactors) { for (const auto& factor : key_factors.second) { if (factor) { factorsToAdd.push_back(factor); - if (marginalFactorsIndices) - marginalFactorsIndices->push_back(nonlinearFactors_.size()); - nonlinearFactors_.push_back( - boost::make_shared(factor)); - if (params_.cacheLinearizedFactors) linearFactors_.push_back(factor); + nonlinearFactorsToAdd.emplace_shared(factor); for (Key factorKey : *factor) { fixedVariables_.insert(factorKey); } } } } - variableIndex_.augment(factorsToAdd); // Augment the variable index - - // Remove the factors to remove that have been summarized in the newly-added - // marginal factors - NonlinearFactorGraph removedFactors; - for (const auto index : factorIndicesToRemove) { - removedFactors.push_back(nonlinearFactors_[index]); - nonlinearFactors_.remove(index); - if (params_.cacheLinearizedFactors) linearFactors_.remove(index); + // Add the nonlinear factors and keep track of the new factor indices + auto newFactorIndices = nonlinearFactors_.add_factors(nonlinearFactorsToAdd, + params_.findUnusedFactorSlots); + // Add cached linear factors. + if (params_.cacheLinearizedFactors){ + linearFactors_.resize(nonlinearFactors_.size()); + for (std::size_t i = 0; i < nonlinearFactorsToAdd.size(); ++i){ + linearFactors_[newFactorIndices[i]] = factorsToAdd[i]; + } } - variableIndex_.remove(factorIndicesToRemove.begin(), - factorIndicesToRemove.end(), removedFactors); - - if (deletedFactorsIndices) - deletedFactorsIndices->assign(factorIndicesToRemove.begin(), - factorIndicesToRemove.end()); + // Augment the variable index + variableIndex_.augment(factorsToAdd, newFactorIndices); // Remove the marginalized variables removeVariables(KeySet(leafKeys.begin(), leafKeys.end())); + + if (deletedFactorsIndices) { + deletedFactorsIndices->assign(factorIndicesToRemove.begin(), + factorIndicesToRemove.end()); + } + if (marginalFactorsIndices){ + *marginalFactorsIndices = std::move(newFactorIndices); + } } /* ************************************************************************* */ diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 940051b96a..b2a679d658 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -714,16 +714,17 @@ namespace { const boost::optional> orderingConstraints = createOrderingConstraints(isam, newValues.keys(), marginalizableKeys); // Mark additional keys between the marginalized keys and the leaves - KeyList additionalMarkedKeys; + KeyList markedKeys; for (Key key : marginalizableKeys) { + markedKeys.push_back(key); ISAM2Clique::shared_ptr clique = isam[key]; for (const ISAM2Clique::shared_ptr& child : clique->children) { - markAffectedKeys(key, child, additionalMarkedKeys); + markAffectedKeys(key, child, markedKeys); } } // Update - isam.update(newFactors, newValues, FactorIndices{}, orderingConstraints, boost::none, additionalMarkedKeys); + isam.update(newFactors, newValues, FactorIndices{}, orderingConstraints, boost::none, markedKeys); if (!marginalizableKeys.empty()) { FastList leafKeys(marginalizableKeys.begin(), marginalizableKeys.end()); From 44d948e98e95bee9e67d8735ef29214d00ef866e Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Mon, 7 Aug 2023 23:31:58 -0700 Subject: [PATCH 4/4] Remove the boost references Signed-off-by: Fan Jiang --- tests/testGaussianISAM2.cpp | 36 ++++++++++++++++++------------------ 1 file changed, 18 insertions(+), 18 deletions(-) diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index e78d36f0e5..1ea60dac95 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -659,10 +659,10 @@ namespace { return ok; } - boost::optional> createOrderingConstraints(const ISAM2& isam, const KeyVector& newKeys, const KeySet& marginalizableKeys) + std::optional> createOrderingConstraints(const ISAM2& isam, const KeyVector& newKeys, const KeySet& marginalizableKeys) { if (marginalizableKeys.empty()) { - return boost::none; + return {}; } else { FastMap constrainedKeys = FastMap(); // Generate ordering constraints so that the marginalizable variables will be eliminated first @@ -706,7 +706,7 @@ namespace { bool updateAndMarginalize(const NonlinearFactorGraph& newFactors, const Values& newValues, const KeySet& marginalizableKeys, ISAM2& isam) { // Force ISAM2 to put marginalizable variables at the beginning - const boost::optional> orderingConstraints = createOrderingConstraints(isam, newValues.keys(), marginalizableKeys); + auto orderingConstraints = createOrderingConstraints(isam, newValues.keys(), marginalizableKeys); // Mark additional keys between the marginalized keys and the leaves KeyList markedKeys; @@ -719,7 +719,7 @@ namespace { } // Update - isam.update(newFactors, newValues, FactorIndices{}, orderingConstraints, boost::none, markedKeys); + isam.update(newFactors, newValues, FactorIndices{}, orderingConstraints, {}, markedKeys); if (!marginalizableKeys.empty()) { FastList leafKeys(marginalizableKeys.begin(), marginalizableKeys.end()); @@ -865,7 +865,7 @@ TEST(ISAM2, marginalizeLeaves5) /* ************************************************************************* */ TEST(ISAM2, marginalizeLeaves6) { - const boost::shared_ptr nm = noiseModel::Isotropic::Sigma(6, 1.0); + auto nm = noiseModel::Isotropic::Sigma(6, 1.0); int gridDim = 10; @@ -878,16 +878,16 @@ TEST(ISAM2, marginalizeLeaves6) // Create a grid of pose variables for (int i = 0; i < gridDim; ++i) { for (int j = 0; j < gridDim; ++j) { - Pose3 pose = Pose3(Rot3::identity(), Point3(i, j, 0)); + Pose3 pose = Pose3(Rot3::Identity(), Point3(i, j, 0)); Key key = idxToKey(i, j); // Place a prior on the first pose - factors.addPrior(key, Pose3(Rot3::identity(), Point3(i, j, 0)), nm); + factors.addPrior(key, Pose3(Rot3::Identity(), Point3(i, j, 0)), nm); values.insert(key, pose); if (i > 0) { - factors.emplace_shared>(idxToKey(i - 1, j), key, Pose3(Rot3::identity(), Point3(1, 0, 0)),nm); + factors.emplace_shared>(idxToKey(i - 1, j), key, Pose3(Rot3::Identity(), Point3(1, 0, 0)),nm); } if (j > 0) { - factors.emplace_shared>(idxToKey(i, j - 1), key, Pose3(Rot3::identity(), Point3(0, 1, 0)),nm); + factors.emplace_shared>(idxToKey(i, j - 1), key, Pose3(Rot3::Identity(), Point3(0, 1, 0)),nm); } } } @@ -914,17 +914,17 @@ TEST(ISAM2, marginalizeLeaves6) /* ************************************************************************* */ TEST(ISAM2, MarginalizeRoot) { - const boost::shared_ptr nm = noiseModel::Isotropic::Sigma(6, 1.0); + auto nm = noiseModel::Isotropic::Sigma(6, 1.0); NonlinearFactorGraph factors; Values values; ISAM2 isam; // Create a factor graph with one variable and a prior - Pose3 root = Pose3::identity(); + Pose3 root = Pose3::Identity(); Key rootKey(0); values.insert(rootKey, root); - factors.addPrior(rootKey, Pose3::identity(), nm); + factors.addPrior(rootKey, Pose3::Identity(), nm); // Optimize the graph EXPECT(updateAndMarginalize(factors, values, {}, isam)); @@ -944,7 +944,7 @@ TEST(ISAM2, MarginalizeRoot) /* ************************************************************************* */ TEST(ISAM2, marginalizationSize) { - const boost::shared_ptr nm = noiseModel::Isotropic::Sigma(6, 1.0); + auto nm = noiseModel::Isotropic::Sigma(6, 1.0); NonlinearFactorGraph factors; Values values; @@ -954,13 +954,13 @@ TEST(ISAM2, marginalizationSize) // Create a pose variable Key aKey(0); - values.insert(aKey, Pose3::identity()); - factors.addPrior(aKey, Pose3::identity(), nm); + values.insert(aKey, Pose3::Identity()); + factors.addPrior(aKey, Pose3::Identity(), nm); // Create another pose variable linked to the first - Pose3 b = Pose3::identity(); + Pose3 b = Pose3::Identity(); Key bKey(1); - values.insert(bKey, Pose3::identity()); - factors.emplace_shared>(aKey, bKey, Pose3::identity(), nm); + values.insert(bKey, Pose3::Identity()); + factors.emplace_shared>(aKey, bKey, Pose3::Identity(), nm); // Optimize the graph EXPECT(updateAndMarginalize(factors, values, {}, isam));