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

Fix marginalization in IncrementalFixedLagSmoother. 

Add BayesTreeMarginalizationHelper.h and use the new helper
to gather the additional keys to re-eliminate when marginalizing
variables in IncrementalFixedLagSmoother.
release/4.3a0
Jeffrey 2024-10-28 23:38:04 +08:00
parent 1dd3b180b1
commit 896e52ca27
3 changed files with 196 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

174
gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h Normal file
View File

@ -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 <gtsam/inference/BayesTree.h>
#include <gtsam/inference/BayesTreeCliqueBase.h>
#include <gtsam/base/debug.h>
#include "gtsam_unstable/dllexport.h"
namespace gtsam {
/**
* This class provides helper functions for marginalizing variables from a Bayes Tree.
*/
template <typename BayesTree>
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<Key>& additionalKeys) {
const bool debug = ISDEBUG("BayesTreeMarginalizationHelper");
std::set<Key> marginalizableKeySet(marginalizableKeys.begin(), marginalizableKeys.end());
std::set<sharedClique> checkedCliques;
std::set<sharedClique> 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

10
gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp
View File

@ -20,11 +20,15 @@
*/
#include <gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h>
#include <gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h>
#include <gtsam/base/debug.h>
// #define GTSAM_OLD_MARGINALIZATION
namespace gtsam {
/* ************************************************************************* */
#ifdef GTSAM_OLD_MARGINALIZATION
void recursiveMarkAffectedKeys(const Key& key,
const ISAM2Clique::shared_ptr& clique, std::set<Key>& 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<Key> 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<ISAM2>::gatherAdditionalKeysToReEliminate(
isam_, marginalizableKeys, additionalKeys);
#endif
KeyList additionalMarkedKeys(additionalKeys.begin(), additionalKeys.end());
// Update iSAM2

15
gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp
View File

@ -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<Point2>(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<Point2>(key_7, key_6, Point2(1.0, 0.0), odometerNoise));
if (i % 8 == 0)
newFactors.push_back(BetweenFactor<Point2>(key_8, key_7, Point2(1.0, 0.0), odometerNoise));
if (i % 9 == 0)
newFactors.push_back(BetweenFactor<Point2>(key_9, key_8, Point2(1.0, 0.0), odometerNoise));
if (i % 10 == 0)
newFactors.push_back(BetweenFactor<Point2>(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);