diff --git a/gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h b/gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h new file mode 100644 index 000000000..fe261d10e --- /dev/null +++ b/gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h @@ -0,0 +1,174 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file BayesTreeMarginalizationHelper.h + * @brief Helper functions for marginalizing variables from a Bayes Tree. + * + * @author Jeffrey (Zhiwei Wang) + * @date Oct 28, 2024 + */ + +// \callgraph +#pragma once + +#include +#include +#include +#include "gtsam_unstable/dllexport.h" + +namespace gtsam { + +/** + * This class provides helper functions for marginalizing variables from a Bayes Tree. + */ +template +class GTSAM_UNSTABLE_EXPORT BayesTreeMarginalizationHelper { + +public: + using Clique = typename BayesTree::Clique; + using sharedClique = typename BayesTree::sharedClique; + + /** Get the additional keys that need to be re-eliminated when marginalizing + * the variables in @p marginalizableKeys from the Bayes tree @p bayesTree. + * + * @param[in] bayesTree The Bayes tree to be marginalized. + * @param[in] marginalizableKeys The keys to be marginalized. + * + * + * When marginalizing a variable @f$ \theta @f$ in a Bayes tree, some nodes + * may need to be re-eliminated. The variable to be marginalized should be + * eliminated first. + * + * 1. If @f$ \theta @f$ is already in a leaf node @f$ L @f$, and all other + * frontal variables within @f$ L @f$ are to be marginalized, then this + * node does not need to be re-eliminated; the entire node can be directly + * marginalized. + * + * 2. If @f$ \theta @f$ is in a leaf node @f$ L @f$, but @f$ L @f$ contains + * other frontal variables that do not need to be marginalized: + * a. If all other non-marginalized frontal variables are listed after + * @f$ \theta @f$ (each node contains a frontal list, with variables to + * be eliminated earlier in the list), then node @f$ L @f$ does not + * need to be re-eliminated. + * b. Otherwise, if there are non-marginalized nodes listed before + * @f$ \theta @f$, then node @f$ L @f$ needs to be re-eliminated, and + * correspondingly, all nodes between @f$ L @f$ and the root need to be + * re-eliminated. + * + * 3. If @f$ \theta @f$ is in an intermediate node @f$ M @f$ (non-leaf node), + * but none of @f$ M @f$'s child nodes depend on variable @f$ \theta @f$ + * (they only depend on other variables within @f$ M @f$), then during the + * process of marginalizing @f$ \theta @f$, @f$ M @f$ can be treated as a + * leaf node, and @f$ M @f$ should be processed following the same + * approach as for leaf nodes. + * + * In this case, the original elimination of @f$ \theta @f$ does not + * depend on the elimination results of variables in the child nodes. + * + * 4. If @f$ \theta @f$ is in an intermediate node @f$ M @f$ (non-leaf node), + * and there exist child nodes that depend on variable @f$ \theta @f$, + * then not only does node @f$ M @f$ need to be re-eliminated, but all + * child nodes dependent on @f$ \theta @f$, including descendant nodes + * recursively dependent on @f$ \theta @f$, also need to be re-eliminated. + * + * The frontal variables in child nodes were originally eliminated before + * @f$ \theta @f$ and their elimination results are relied upon by + * @f$ \theta @f$'s elimination. When re-eliminating, they should be + * eliminated after @f$ \theta @f$. + */ + static void gatherAdditionalKeysToReEliminate( + const BayesTree& bayesTree, + const KeyVector& marginalizableKeys, + std::set& additionalKeys) { + const bool debug = ISDEBUG("BayesTreeMarginalizationHelper"); + + std::set marginalizableKeySet(marginalizableKeys.begin(), marginalizableKeys.end()); + std::set checkedCliques; + + std::set dependentCliques; + for (const Key& key : marginalizableKeySet) { + sharedClique clique = bayesTree[key]; + if (checkedCliques.count(clique)) { + continue; + } + checkedCliques.insert(clique); + + bool is_leaf = clique->children.empty(); + bool need_reeliminate = false; + bool has_non_marginalizable_ahead = false; + for (Key i: clique->conditional()->frontals()) { + if (marginalizableKeySet.count(i)) { + if (has_non_marginalizable_ahead) { + need_reeliminate = true; + break; + } else { + // Check whether there're child nodes dependent on this key. + for(const sharedClique& child: clique->children) { + if (std::find(child->conditional()->beginParents(), + child->conditional()->endParents(), i) + != child->conditional()->endParents()) { + need_reeliminate = true; + break; + } + } + } + } else { + has_non_marginalizable_ahead = true; + } + } + + if (!need_reeliminate) { + continue; + } else { + // need to re-eliminate this clique and all its children that depend on + // a marginalizable key + for (Key i: clique->conditional()->frontals()) { + additionalKeys.insert(i); + for (const sharedClique& child: clique->children) { + if (!dependentCliques.count(child) && + std::find(child->conditional()->beginParents(), + child->conditional()->endParents(), i) + != child->conditional()->endParents()) { + dependentCliques.insert(child); + } + } + } + } + } + + // Recursively add the dependent keys + while (!dependentCliques.empty()) { + auto begin = dependentCliques.begin(); + sharedClique clique = *begin; + dependentCliques.erase(begin); + + for (Key key : clique->conditional()->frontals()) { + additionalKeys.insert(key); + } + + for (const sharedClique& child: clique->children) { + dependentCliques.insert(child); + } + } + + if (debug) { + std::cout << "BayesTreeMarginalizationHelper: Additional keys to re-eliminate: "; + for (const Key& key : additionalKeys) { + std::cout << DefaultKeyFormatter(key) << " "; + } + std::cout << std::endl; + } + } +}; +// BayesTreeMarginalizationHelper + +}/// namespace gtsam diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp index 52e56260d..9d27f5713 100644 --- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp @@ -20,11 +20,15 @@ */ #include +#include #include +// #define GTSAM_OLD_MARGINALIZATION + namespace gtsam { /* ************************************************************************* */ +#ifdef GTSAM_OLD_MARGINALIZATION void recursiveMarkAffectedKeys(const Key& key, const ISAM2Clique::shared_ptr& clique, std::set& additionalKeys) { @@ -45,6 +49,7 @@ void recursiveMarkAffectedKeys(const Key& key, } // If the key was not found in the separator/parents, then none of its children can have it either } +#endif /* ************************************************************************* */ void IncrementalFixedLagSmoother::print(const std::string& s, @@ -116,12 +121,17 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update( // Mark additional keys between the marginalized keys and the leaves std::set additionalKeys; +#ifdef GTSAM_OLD_MARGINALIZATION for(Key key: marginalizableKeys) { ISAM2Clique::shared_ptr clique = isam_[key]; for(const ISAM2Clique::shared_ptr& child: clique->children) { recursiveMarkAffectedKeys(key, child, additionalKeys); } } +#else + BayesTreeMarginalizationHelper::gatherAdditionalKeysToReEliminate( + isam_, marginalizableKeys, additionalKeys); +#endif KeyList additionalMarkedKeys(additionalKeys.begin(), additionalKeys.end()); // Update iSAM2 diff --git a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp index cb4dcbf9a..cd2ba593b 100644 --- a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp @@ -99,7 +99,7 @@ TEST( IncrementalFixedLagSmoother, Example ) // Create a Fixed-Lag Smoother typedef IncrementalFixedLagSmoother::KeyTimestampMap Timestamps; - IncrementalFixedLagSmoother smoother(9.0, ISAM2Params()); + IncrementalFixedLagSmoother smoother(12.0, ISAM2Params()); // Create containers to keep the full graph Values fullinit; @@ -226,6 +226,7 @@ TEST( IncrementalFixedLagSmoother, Example ) newFactors.push_back(BetweenFactor(key1, key2, Point2(1.0, 0.0), odometerNoise)); newValues.insert(key2, Point2(double(i)+0.1, -0.1)); newTimestamps[key2] = double(i); + ++i; fullgraph.push_back(newFactors); fullinit.insert(newValues); @@ -275,10 +276,12 @@ TEST( IncrementalFixedLagSmoother, Example ) } { + SETDEBUG("BayesTreeMarginalizationHelper", true); PrintSymbolicTree(smoother.getISAM2(), "Bayes Tree Before marginalization test:"); - i = 17; - while(i <= 200) { + // Do pressure test on marginalization. Enlarge max_i to enhance the test. + const int max_i = 500; + while(i <= max_i) { Key key_0 = MakeKey(i); Key key_1 = MakeKey(i-1); Key key_2 = MakeKey(i-2); @@ -288,6 +291,8 @@ TEST( IncrementalFixedLagSmoother, Example ) Key key_6 = MakeKey(i-6); Key key_7 = MakeKey(i-7); Key key_8 = MakeKey(i-8); + Key key_9 = MakeKey(i-9); + Key key_10 = MakeKey(i-10); NonlinearFactorGraph newFactors; Values newValues; @@ -309,6 +314,10 @@ TEST( IncrementalFixedLagSmoother, Example ) newFactors.push_back(BetweenFactor(key_7, key_6, Point2(1.0, 0.0), odometerNoise)); if (i % 8 == 0) newFactors.push_back(BetweenFactor(key_8, key_7, Point2(1.0, 0.0), odometerNoise)); + if (i % 9 == 0) + newFactors.push_back(BetweenFactor(key_9, key_8, Point2(1.0, 0.0), odometerNoise)); + if (i % 10 == 0) + newFactors.push_back(BetweenFactor(key_10, key_9, Point2(1.0, 0.0), odometerNoise)); newValues.insert(key_0, Point2(double(i)+0.1, -0.1)); newTimestamps[key_0] = double(i);