Remove old marginalization code in IncrementalFixedLagSmoother.cpp
parent
67b0b78ea1
commit
14c3467520
|
@ -23,34 +23,8 @@
|
|||
#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) {
|
||||
|
||||
// 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,
|
||||
const KeyFormatter& keyFormatter) const {
|
||||
|
@ -119,20 +93,9 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update(
|
|||
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 =
|
||||
BayesTreeMarginalizationHelper<ISAM2>::gatherAdditionalKeysToReEliminate(
|
||||
isam_, marginalizableKeys);
|
||||
#endif
|
||||
KeyList additionalMarkedKeys(additionalKeys.begin(), additionalKeys.end());
|
||||
|
||||
// Update iSAM2
|
||||
|
|
Loading…
Reference in New Issue