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

Remove old marginalization code in IncrementalFixedLagSmoother.cpp

release/4.3a0
Jeffrey 2024-10-31 19:05:54 +08:00
parent 67b0b78ea1
commit 14c3467520
1 changed files with 0 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp
View File

@ -23,34 +23,8 @@
#include <gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h> #include <gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h>
#include <gtsam/base/debug.h> #include <gtsam/base/debug.h>
// #define GTSAM_OLD_MARGINALIZATION
namespace gtsam { namespace gtsam {
/* ************************************************************************* */
#ifdef GTSAM_OLD_MARGINALIZATION
void recursiveMarkAffectedKeys(const Key& key,
const ISAM2Clique::shared_ptr& clique, std::set<Key>& additionalKeys) {
// Check if the separator keys of the current clique contain the specified key
if (std::find(clique->conditional()->beginParents(),
clique->conditional()->endParents(), key)
!= clique->conditional()->endParents()) {
// Mark the frontal keys of the current clique
for(Key i: clique->conditional()->frontals()) {
additionalKeys.insert(i);
}
// Recursively mark all of the children
for(const ISAM2Clique::shared_ptr& child: clique->children) {
recursiveMarkAffectedKeys(key, child, additionalKeys);
}
}
// 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, void IncrementalFixedLagSmoother::print(const std::string& s,
const KeyFormatter& keyFormatter) const { const KeyFormatter& keyFormatter) const {
@ -119,20 +93,9 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update(
std::cout << std::endl; std::cout << std::endl;
} }
// Mark additional keys between the marginalized keys and the leaves
#ifdef GTSAM_OLD_MARGINALIZATION
std::set<Key> additionalKeys;
for(Key key: marginalizableKeys) {
ISAM2Clique::shared_ptr clique = isam_[key];
for(const ISAM2Clique::shared_ptr& child: clique->children) {
recursiveMarkAffectedKeys(key, child, additionalKeys);
}
}
#else
std::set<Key> additionalKeys = std::set<Key> additionalKeys =
BayesTreeMarginalizationHelper<ISAM2>::gatherAdditionalKeysToReEliminate( BayesTreeMarginalizationHelper<ISAM2>::gatherAdditionalKeysToReEliminate(
isam_, marginalizableKeys); isam_, marginalizableKeys);
#endif
KeyList additionalMarkedKeys(additionalKeys.begin(), additionalKeys.end()); KeyList additionalMarkedKeys(additionalKeys.begin(), additionalKeys.end());
// Update iSAM2 // Update iSAM2