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

Merge pull request #2083 from borglab/move-ifls 

Move IncrementalFixedLagSmoother from unstable to stable
release/4.3a0
Frank Dellaert 2025-04-06 18:43:19 -04:00 committed by GitHub
parent 4226da3f0d 655ded3933
commit c2f9c5e4af
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
8 changed files with 546 additions and 503 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam_unstable/examples/IncrementalFixedLagSmootherExample.cpp → examples/IncrementalFixedLagSmootherExample.cpp
View File

@ -59,7 +59,7 @@
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h>
#include <gtsam/nonlinear/IncrementalFixedLagSmoother.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/dataset.h> // for writeG2o

358
gtsam/nonlinear/BayesTreeMarginalizationHelper.h Normal file
View File

@ -0,0 +1,358 @@
/* ----------------------------------------------------------------------------
* 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 <unordered_map>
#include <unordered_set>
#include <deque>
#include <gtsam/inference/BayesTree.h>
#include <gtsam/inference/BayesTreeCliqueBase.h>
#include <gtsam/base/debug.h>
#include "gtsam/dllexport.h"
namespace gtsam {
/**
* This class provides helper functions for marginalizing variables from a Bayes Tree.
*/
template <typename BayesTree>
class GTSAM_EXPORT BayesTreeMarginalizationHelper {
public:
using Clique = typename BayesTree::Clique;
using sharedClique = typename BayesTree::sharedClique;
/**
* This function identifies variables that need to be re-eliminated before
* performing marginalization.
*
* Re-elimination is necessary for a clique containing marginalizable
* variables if:
*
* 1. Some non-marginalizable variables appear before marginalizable ones
* in that clique;
* 2. Or it has a child node depending on a marginalizable variable AND the
* subtree rooted at that child contains non-marginalizables.
*
* In addition, for any descendant node depending on a marginalizable
* variable, if the subtree rooted at that descendant contains
* non-marginalizable variables (i.e., it lies on a path from one of the
* aforementioned cliques that require re-elimination to a node containing
* non-marginalizable variables at the leaf side), then it also needs to
* be re-eliminated.
*
* @param[in] bayesTree The Bayes tree
* @param[in] marginalizableKeys Keys to be marginalized
* @return Set of additional keys that need to be re-eliminated
*/
static std::unordered_set<Key>
gatherAdditionalKeysToReEliminate(
const BayesTree& bayesTree,
const KeyVector& marginalizableKeys) {
const bool debug = ISDEBUG("BayesTreeMarginalizationHelper");
std::unordered_set<const Clique*> additionalCliques =
gatherAdditionalCliquesToReEliminate(bayesTree, marginalizableKeys);
std::unordered_set<Key> additionalKeys;
for (const Clique* clique : additionalCliques) {
addCliqueToKeySet(clique, &additionalKeys);
}
if (debug) {
std::cout << "BayesTreeMarginalizationHelper: Additional keys to re-eliminate: ";
for (const Key& key : additionalKeys) {
std::cout << DefaultKeyFormatter(key) << " ";
}
std::cout << std::endl;
}
return additionalKeys;
}
protected:
/**
* This function identifies cliques that need to be re-eliminated before
* performing marginalization.
* See the docstring of @ref gatherAdditionalKeysToReEliminate().
*/
static std::unordered_set<const Clique*>
gatherAdditionalCliquesToReEliminate(
const BayesTree& bayesTree,
const KeyVector& marginalizableKeys) {
std::unordered_set<const Clique*> additionalCliques;
std::unordered_set<Key> marginalizableKeySet(
marginalizableKeys.begin(), marginalizableKeys.end());
CachedSearch cachedSearch;
// Check each clique that contains a marginalizable key
for (const Clique* clique :
getCliquesContainingKeys(bayesTree, marginalizableKeySet)) {
if (additionalCliques.count(clique)) {
// The clique has already been visited. This can happen when an
// ancestor of the current clique also contain some marginalizable
// varaibles and it's processed beore the current.
continue;
}
if (needsReelimination(clique, marginalizableKeySet, &cachedSearch)) {
// Add the current clique
additionalCliques.insert(clique);
// Then add the dependent cliques
gatherDependentCliques(clique, marginalizableKeySet, &additionalCliques,
&cachedSearch);
}
}
return additionalCliques;
}
/**
* Gather the cliques containing any of the given keys.
*
* @param[in] bayesTree The Bayes tree
* @param[in] keysOfInterest Set of keys of interest
* @return Set of cliques that contain any of the given keys
*/
static std::unordered_set<const Clique*> getCliquesContainingKeys(
const BayesTree& bayesTree,
const std::unordered_set<Key>& keysOfInterest) {
std::unordered_set<const Clique*> cliques;
for (const Key& key : keysOfInterest) {
cliques.insert(bayesTree[key].get());
}
return cliques;
}
/**
* A struct to cache the results of the below two functions.
*/
struct CachedSearch {
std::unordered_map<const Clique*, bool> wholeMarginalizableCliques;
std::unordered_map<const Clique*, bool> wholeMarginalizableSubtrees;
};
/**
* Check if all variables in the clique are marginalizable.
*
* Note we use a cache map to avoid repeated searches.
*/
static bool isWholeCliqueMarginalizable(
const Clique* clique,
const std::unordered_set<Key>& marginalizableKeys,
CachedSearch* cache) {
auto it = cache->wholeMarginalizableCliques.find(clique);
if (it != cache->wholeMarginalizableCliques.end()) {
return it->second;
} else {
bool ret = true;
for (Key key : clique->conditional()->frontals()) {
if (!marginalizableKeys.count(key)) {
ret = false;
break;
}
}
cache->wholeMarginalizableCliques.insert({clique, ret});
return ret;
}
}
/**
* Check if all variables in the subtree are marginalizable.
*
* Note we use a cache map to avoid repeated searches.
*/
static bool isWholeSubtreeMarginalizable(
const Clique* subtree,
const std::unordered_set<Key>& marginalizableKeys,
CachedSearch* cache) {
auto it = cache->wholeMarginalizableSubtrees.find(subtree);
if (it != cache->wholeMarginalizableSubtrees.end()) {
return it->second;
} else {
bool ret = true;
if (isWholeCliqueMarginalizable(subtree, marginalizableKeys, cache)) {
for (const sharedClique& child : subtree->children) {
if (!isWholeSubtreeMarginalizable(child.get(), marginalizableKeys, cache)) {
ret = false;
break;
}
}
} else {
ret = false;
}
cache->wholeMarginalizableSubtrees.insert({subtree, ret});
return ret;
}
}
/**
* Check if a clique contains variables that need reelimination due to
* elimination ordering conflicts.
*
* @param[in] clique The clique to check
* @param[in] marginalizableKeys Set of keys to be marginalized
* @return true if any variables in the clique need re-elimination
*/
static bool needsReelimination(
const Clique* clique,
const std::unordered_set<Key>& marginalizableKeys,
CachedSearch* cache) {
bool hasNonMarginalizableAhead = false;
// Check each frontal variable in order
for (Key key : clique->conditional()->frontals()) {
if (marginalizableKeys.count(key)) {
// If we've seen non-marginalizable variables before this one,
// we need to reeliminate
if (hasNonMarginalizableAhead) {
return true;
}
// Check if any child depends on this marginalizable key and the
// subtree rooted at that child contains non-marginalizables.
for (const sharedClique& child : clique->children) {
if (hasDependency(child.get(), key) &&
!isWholeSubtreeMarginalizable(child.get(), marginalizableKeys, cache)) {
return true;
}
}
} else {
hasNonMarginalizableAhead = true;
}
}
return false;
}
/**
* Gather all dependent nodes that lie on a path from the root clique
* to a clique containing a non-marginalizable variable at the leaf side.
*
* @param[in] rootClique The root clique
* @param[in] marginalizableKeys Set of keys to be marginalized
*/
static void gatherDependentCliques(
const Clique* rootClique,
const std::unordered_set<Key>& marginalizableKeys,
std::unordered_set<const Clique*>* additionalCliques,
CachedSearch* cache) {
std::vector<const Clique*> dependentChildren;
dependentChildren.reserve(rootClique->children.size());
for (const sharedClique& child : rootClique->children) {
if (additionalCliques->count(child.get())) {
// This child has already been visited. This can happen if the
// child itself contains a marginalizable variable and it's
// processed before the current rootClique.
continue;
}
if (hasDependency(child.get(), marginalizableKeys)) {
dependentChildren.push_back(child.get());
}
}
gatherDependentCliquesFromChildren(
dependentChildren, marginalizableKeys, additionalCliques, cache);
}
/**
* A helper function for the above gatherDependentCliques().
*/
static void gatherDependentCliquesFromChildren(
const std::vector<const Clique*>& dependentChildren,
const std::unordered_set<Key>& marginalizableKeys,
std::unordered_set<const Clique*>* additionalCliques,
CachedSearch* cache) {
std::deque<const Clique*> descendants(
dependentChildren.begin(), dependentChildren.end());
while (!descendants.empty()) {
const Clique* descendant = descendants.front();
descendants.pop_front();
// If the subtree rooted at this descendant contains non-marginalizables,
// it must lie on a path from the root clique to a clique containing
// non-marginalizables at the leaf side.
if (!isWholeSubtreeMarginalizable(descendant, marginalizableKeys, cache)) {
additionalCliques->insert(descendant);
// Add children of the current descendant to the set descendants.
for (const sharedClique& child : descendant->children) {
if (additionalCliques->count(child.get())) {
// This child has already been visited.
continue;
} else {
descendants.push_back(child.get());
}
}
}
}
}
/**
* Add all frontal variables from a clique to a key set.
*
* @param[in] clique Clique to add keys from
* @param[out] additionalKeys Pointer to the output key set
*/
static void addCliqueToKeySet(
const Clique* clique,
std::unordered_set<Key>* additionalKeys) {
for (Key key : clique->conditional()->frontals()) {
additionalKeys->insert(key);
}
}
/**
* Check if the clique depends on the given key.
*
* @param[in] clique Clique to check
* @param[in] key Key to check for dependencies
* @return true if clique depends on the key
*/
static bool hasDependency(
const Clique* clique, Key key) {
auto& conditional = clique->conditional();
if (std::find(conditional->beginParents(),
conditional->endParents(), key)
!= conditional->endParents()) {
return true;
} else {
return false;
}
}
/**
* Check if the clique depends on any of the given keys.
*/
static bool hasDependency(
const Clique* clique, const std::unordered_set<Key>& keys) {
auto& conditional = clique->conditional();
for (auto it = conditional->beginParents();
it != conditional->endParents(); ++it) {
if (keys.count(*it)) {
return true;
}
}
return false;
}
};
// BayesTreeMarginalizationHelper
}/// namespace gtsam

4
gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp → gtsam/nonlinear/IncrementalFixedLagSmoother.cpp
View File

@ -19,8 +19,8 @@
* @date Oct 14, 2012
*/
#include <gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h>
#include <gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h>
#include <gtsam/nonlinear/IncrementalFixedLagSmoother.h>
#include <gtsam/nonlinear/BayesTreeMarginalizationHelper.h>
#include <gtsam/base/debug.h>
namespace gtsam {

159
gtsam/nonlinear/IncrementalFixedLagSmoother.h Normal file
View File

@ -0,0 +1,159 @@
/* ----------------------------------------------------------------------------
* 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 IncrementalFixedLagSmoother.h
* @brief An iSAM2-based fixed-lag smoother.
*
* @author Michael Kaess, Stephen Williams
* @date Oct 14, 2012
*/
// \callgraph
#pragma once
#include <gtsam/nonlinear/FixedLagSmoother.h>
#include <gtsam/nonlinear/ISAM2.h>
#include "gtsam/dllexport.h"
namespace gtsam {
/**
* This is a base class for the various HMF2 implementations. The HMF2 eliminates the factor graph
* such that the active states are placed in/near the root. This base class implements a function
* to calculate the ordering, and an update function to incorporate new factors into the HMF.
*/
class GTSAM_EXPORT IncrementalFixedLagSmoother: public FixedLagSmoother {
public:
/// Typedef for a shared pointer to an Incremental Fixed-Lag Smoother
typedef std::shared_ptr<IncrementalFixedLagSmoother> shared_ptr;
/** default constructor */
IncrementalFixedLagSmoother(double smootherLag = 0.0,
const ISAM2Params& parameters = DefaultISAM2Params()) :
FixedLagSmoother(smootherLag), isam_(parameters) {
}
/** destructor */
~IncrementalFixedLagSmoother() override {
}
/** Print the factor for debugging and testing (implementing Testable) */
void print(const std::string& s = "IncrementalFixedLagSmoother:\n",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
/** Check if two IncrementalFixedLagSmoother Objects are equal */
bool equals(const FixedLagSmoother& rhs, double tol = 1e-9) const override;
/**
* Add new factors, updating the solution and re-linearizing as needed.
* @param newFactors new factors on old and/or new variables
* @param newTheta new values for new variables only
* @param timestamps an (optional) map from keys to real time stamps
* @param factorsToRemove an (optional) list of factors to remove.
*/
Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(),
const Values& newTheta = Values(), //
const KeyTimestampMap& timestamps = KeyTimestampMap(),
const FactorIndices& factorsToRemove = FactorIndices()) override;
/** Compute an estimate from the incomplete linear delta computed during the last update.
* This delta is incomplete because it was not updated below wildfire_threshold. If only
* a single variable is needed, it is faster to call calculateEstimate(const KEY&).
*/
Values calculateEstimate() const override {
return isam_.calculateEstimate();
}
/** Compute an estimate for a single variable using its incomplete linear delta computed
* during the last update. This is faster than calling the no-argument version of
* calculateEstimate, which operates on all variables.
* @param key
* @return
*/
template<class VALUE>
VALUE calculateEstimate(Key key) const {
return isam_.calculateEstimate<VALUE>(key);
}
/** return the current set of iSAM2 parameters */
const ISAM2Params& params() const {
return isam_.params();
}
/** Access the current set of factors */
const NonlinearFactorGraph& getFactors() const {
return isam_.getFactorsUnsafe();
}
/** Access the current linearization point */
const Values& getLinearizationPoint() const {
return isam_.getLinearizationPoint();
}
/** Access the current set of deltas to the linearization point */
const VectorValues& getDelta() const {
return isam_.getDelta();
}
/// Calculate marginal covariance on given variable
Matrix marginalCovariance(Key key) const {
return isam_.marginalCovariance(key);
}
/// Get results of latest isam2 update
const ISAM2Result& getISAM2Result() const{ return isamResult_; }
/// Get the iSAM2 object which is used for the inference internally
const ISAM2& getISAM2() const { return isam_; }
protected:
/** Create default parameters */
static ISAM2Params DefaultISAM2Params() {
ISAM2Params params;
params.findUnusedFactorSlots = true;
return params;
}
/** An iSAM2 object used to perform inference. The smoother lag is controlled
* by what factors are removed each iteration */
ISAM2 isam_;
/** Store results of latest isam2 update */
ISAM2Result isamResult_;
/** Erase any keys associated with timestamps before the provided time */
void eraseKeysBefore(double timestamp);
/** Fill in an iSAM2 ConstrainedKeys structure such that the provided keys are eliminated before all others */
void createOrderingConstraints(const KeyVector& marginalizableKeys,
std::optional<FastMap<Key, int> >& constrainedKeys) const;
private:
/** Private methods for printing debug information */
static void PrintKeySet(const std::set<Key>& keys, const std::string& label =
"Keys:");
static void PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor);
static void PrintSymbolicGraph(const GaussianFactorGraph& graph,
const std::string& label = "Factor Graph:");
static void PrintSymbolicTree(const gtsam::ISAM2& isam,
const std::string& label = "Bayes Tree:");
static void PrintSymbolicTreeHelper(
const gtsam::ISAM2Clique::shared_ptr& clique, const std::string indent =
"");
};
// IncrementalFixedLagSmoother
}/// namespace gtsam

14
gtsam/nonlinear/nonlinear.i
View File

@ -731,6 +731,20 @@ virtual class BatchFixedLagSmoother : gtsam::FixedLagSmoother {
VALUE calculateEstimate(size_t key) const;
};
#include <gtsam/nonlinear/IncrementalFixedLagSmoother.h>
virtual class IncrementalFixedLagSmoother : gtsam::FixedLagSmoother {
IncrementalFixedLagSmoother();
IncrementalFixedLagSmoother(double smootherLag);
IncrementalFixedLagSmoother(double smootherLag, const gtsam::ISAM2Params& params);
void print(string s = "IncrementalFixedLagSmoother:\n") const;
gtsam::ISAM2Params params() const;
gtsam::NonlinearFactorGraph getFactors() const;
gtsam::ISAM2 getISAM2() const;
};
#include <gtsam/nonlinear/ExtendedKalmanFilter.h>
template <T = {gtsam::Point2,
gtsam::Point3,

13
gtsam_unstable/gtsam_unstable.i
View File

@ -532,19 +532,6 @@ virtual class DiscreteEulerPoincareHelicopter : gtsam::NoiseModelFactor {
// nonlinear
//*************************************************************************
#include <gtsam/nonlinear/FixedLagSmoother.h>
#include <gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h>
virtual class IncrementalFixedLagSmoother : gtsam::FixedLagSmoother {
IncrementalFixedLagSmoother();
IncrementalFixedLagSmoother(double smootherLag);
IncrementalFixedLagSmoother(double smootherLag, const gtsam::ISAM2Params& params);
void print(string s = "IncrementalFixedLagSmoother:\n") const;
gtsam::ISAM2Params params() const;
gtsam::NonlinearFactorGraph getFactors() const;
gtsam::ISAM2 getISAM2() const;
};
#include <gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h>
virtual class ConcurrentFilter {

349
gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h
View File

@ -9,350 +9,13 @@
* -------------------------------------------------------------------------- */
/**
* @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 <unordered_map>
#include <unordered_set>
#include <deque>
#include <gtsam/inference/BayesTree.h>
#include <gtsam/inference/BayesTreeCliqueBase.h>
#include <gtsam/base/debug.h>
#include "gtsam_unstable/dllexport.h"
#ifdef _MSC_VER
#pragma message("BayesTreeMarginalizationHelper was moved to the gtsam/nonlinear directory")
#else
#warning "BayesTreeMarginalizationHelper was moved to the gtsam/nonlinear directory"
#endif
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;
/**
* This function identifies variables that need to be re-eliminated before
* performing marginalization.
*
* Re-elimination is necessary for a clique containing marginalizable
* variables if:
*
* 1. Some non-marginalizable variables appear before marginalizable ones
* in that clique;
* 2. Or it has a child node depending on a marginalizable variable AND the
* subtree rooted at that child contains non-marginalizables.
*
* In addition, for any descendant node depending on a marginalizable
* variable, if the subtree rooted at that descendant contains
* non-marginalizable variables (i.e., it lies on a path from one of the
* aforementioned cliques that require re-elimination to a node containing
* non-marginalizable variables at the leaf side), then it also needs to
* be re-eliminated.
*
* @param[in] bayesTree The Bayes tree
* @param[in] marginalizableKeys Keys to be marginalized
* @return Set of additional keys that need to be re-eliminated
*/
static std::unordered_set<Key>
gatherAdditionalKeysToReEliminate(
const BayesTree& bayesTree,
const KeyVector& marginalizableKeys) {
const bool debug = ISDEBUG("BayesTreeMarginalizationHelper");
std::unordered_set<const Clique*> additionalCliques =
gatherAdditionalCliquesToReEliminate(bayesTree, marginalizableKeys);
std::unordered_set<Key> additionalKeys;
for (const Clique* clique : additionalCliques) {
addCliqueToKeySet(clique, &additionalKeys);
}
if (debug) {
std::cout << "BayesTreeMarginalizationHelper: Additional keys to re-eliminate: ";
for (const Key& key : additionalKeys) {
std::cout << DefaultKeyFormatter(key) << " ";
}
std::cout << std::endl;
}
return additionalKeys;
}
protected:
/**
* This function identifies cliques that need to be re-eliminated before
* performing marginalization.
* See the docstring of @ref gatherAdditionalKeysToReEliminate().
*/
static std::unordered_set<const Clique*>
gatherAdditionalCliquesToReEliminate(
const BayesTree& bayesTree,
const KeyVector& marginalizableKeys) {
std::unordered_set<const Clique*> additionalCliques;
std::unordered_set<Key> marginalizableKeySet(
marginalizableKeys.begin(), marginalizableKeys.end());
CachedSearch cachedSearch;
// Check each clique that contains a marginalizable key
for (const Clique* clique :
getCliquesContainingKeys(bayesTree, marginalizableKeySet)) {
if (additionalCliques.count(clique)) {
// The clique has already been visited. This can happen when an
// ancestor of the current clique also contain some marginalizable
// varaibles and it's processed beore the current.
continue;
}
if (needsReelimination(clique, marginalizableKeySet, &cachedSearch)) {
// Add the current clique
additionalCliques.insert(clique);
// Then add the dependent cliques
gatherDependentCliques(clique, marginalizableKeySet, &additionalCliques,
&cachedSearch);
}
}
return additionalCliques;
}
/**
* Gather the cliques containing any of the given keys.
*
* @param[in] bayesTree The Bayes tree
* @param[in] keysOfInterest Set of keys of interest
* @return Set of cliques that contain any of the given keys
*/
static std::unordered_set<const Clique*> getCliquesContainingKeys(
const BayesTree& bayesTree,
const std::unordered_set<Key>& keysOfInterest) {
std::unordered_set<const Clique*> cliques;
for (const Key& key : keysOfInterest) {
cliques.insert(bayesTree[key].get());
}
return cliques;
}
/**
* A struct to cache the results of the below two functions.
*/
struct CachedSearch {
std::unordered_map<const Clique*, bool> wholeMarginalizableCliques;
std::unordered_map<const Clique*, bool> wholeMarginalizableSubtrees;
};
/**
* Check if all variables in the clique are marginalizable.
*
* Note we use a cache map to avoid repeated searches.
*/
static bool isWholeCliqueMarginalizable(
const Clique* clique,
const std::unordered_set<Key>& marginalizableKeys,
CachedSearch* cache) {
auto it = cache->wholeMarginalizableCliques.find(clique);
if (it != cache->wholeMarginalizableCliques.end()) {
return it->second;
} else {
bool ret = true;
for (Key key : clique->conditional()->frontals()) {
if (!marginalizableKeys.count(key)) {
ret = false;
break;
}
}
cache->wholeMarginalizableCliques.insert({clique, ret});
return ret;
}
}
/**
* Check if all variables in the subtree are marginalizable.
*
* Note we use a cache map to avoid repeated searches.
*/
static bool isWholeSubtreeMarginalizable(
const Clique* subtree,
const std::unordered_set<Key>& marginalizableKeys,
CachedSearch* cache) {
auto it = cache->wholeMarginalizableSubtrees.find(subtree);
if (it != cache->wholeMarginalizableSubtrees.end()) {
return it->second;
} else {
bool ret = true;
if (isWholeCliqueMarginalizable(subtree, marginalizableKeys, cache)) {
for (const sharedClique& child : subtree->children) {
if (!isWholeSubtreeMarginalizable(child.get(), marginalizableKeys, cache)) {
ret = false;
break;
}
}
} else {
ret = false;
}
cache->wholeMarginalizableSubtrees.insert({subtree, ret});
return ret;
}
}
/**
* Check if a clique contains variables that need reelimination due to
* elimination ordering conflicts.
*
* @param[in] clique The clique to check
* @param[in] marginalizableKeys Set of keys to be marginalized
* @return true if any variables in the clique need re-elimination
*/
static bool needsReelimination(
const Clique* clique,
const std::unordered_set<Key>& marginalizableKeys,
CachedSearch* cache) {
bool hasNonMarginalizableAhead = false;
// Check each frontal variable in order
for (Key key : clique->conditional()->frontals()) {
if (marginalizableKeys.count(key)) {
// If we've seen non-marginalizable variables before this one,
// we need to reeliminate
if (hasNonMarginalizableAhead) {
return true;
}
// Check if any child depends on this marginalizable key and the
// subtree rooted at that child contains non-marginalizables.
for (const sharedClique& child : clique->children) {
if (hasDependency(child.get(), key) &&
!isWholeSubtreeMarginalizable(child.get(), marginalizableKeys, cache)) {
return true;
}
}
} else {
hasNonMarginalizableAhead = true;
}
}
return false;
}
/**
* Gather all dependent nodes that lie on a path from the root clique
* to a clique containing a non-marginalizable variable at the leaf side.
*
* @param[in] rootClique The root clique
* @param[in] marginalizableKeys Set of keys to be marginalized
*/
static void gatherDependentCliques(
const Clique* rootClique,
const std::unordered_set<Key>& marginalizableKeys,
std::unordered_set<const Clique*>* additionalCliques,
CachedSearch* cache) {
std::vector<const Clique*> dependentChildren;
dependentChildren.reserve(rootClique->children.size());
for (const sharedClique& child : rootClique->children) {
if (additionalCliques->count(child.get())) {
// This child has already been visited. This can happen if the
// child itself contains a marginalizable variable and it's
// processed before the current rootClique.
continue;
}
if (hasDependency(child.get(), marginalizableKeys)) {
dependentChildren.push_back(child.get());
}
}
gatherDependentCliquesFromChildren(
dependentChildren, marginalizableKeys, additionalCliques, cache);
}
/**
* A helper function for the above gatherDependentCliques().
*/
static void gatherDependentCliquesFromChildren(
const std::vector<const Clique*>& dependentChildren,
const std::unordered_set<Key>& marginalizableKeys,
std::unordered_set<const Clique*>* additionalCliques,
CachedSearch* cache) {
std::deque<const Clique*> descendants(
dependentChildren.begin(), dependentChildren.end());
while (!descendants.empty()) {
const Clique* descendant = descendants.front();
descendants.pop_front();
// If the subtree rooted at this descendant contains non-marginalizables,
// it must lie on a path from the root clique to a clique containing
// non-marginalizables at the leaf side.
if (!isWholeSubtreeMarginalizable(descendant, marginalizableKeys, cache)) {
additionalCliques->insert(descendant);
// Add children of the current descendant to the set descendants.
for (const sharedClique& child : descendant->children) {
if (additionalCliques->count(child.get())) {
// This child has already been visited.
continue;
} else {
descendants.push_back(child.get());
}
}
}
}
}
/**
* Add all frontal variables from a clique to a key set.
*
* @param[in] clique Clique to add keys from
* @param[out] additionalKeys Pointer to the output key set
*/
static void addCliqueToKeySet(
const Clique* clique,
std::unordered_set<Key>* additionalKeys) {
for (Key key : clique->conditional()->frontals()) {
additionalKeys->insert(key);
}
}
/**
* Check if the clique depends on the given key.
*
* @param[in] clique Clique to check
* @param[in] key Key to check for dependencies
* @return true if clique depends on the key
*/
static bool hasDependency(
const Clique* clique, Key key) {
auto& conditional = clique->conditional();
if (std::find(conditional->beginParents(),
conditional->endParents(), key)
!= conditional->endParents()) {
return true;
} else {
return false;
}
}
/**
* Check if the clique depends on any of the given keys.
*/
static bool hasDependency(
const Clique* clique, const std::unordered_set<Key>& keys) {
auto& conditional = clique->conditional();
for (auto it = conditional->beginParents();
it != conditional->endParents(); ++it) {
if (keys.count(*it)) {
return true;
}
}
return false;
}
};
// BayesTreeMarginalizationHelper
}/// namespace gtsam
#include <gtsam/nonlinear/BayesTreeMarginalizationHelper.h>

150
gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h
View File

@ -9,151 +9,13 @@
* -------------------------------------------------------------------------- */
/**
* @file IncrementalFixedLagSmoother.h
* @brief An iSAM2-based fixed-lag smoother.
*
* @author Michael Kaess, Stephen Williams
* @date Oct 14, 2012
*/
// \callgraph
#pragma once
#include <gtsam/nonlinear/FixedLagSmoother.h>
#include <gtsam/nonlinear/ISAM2.h>
#include "gtsam_unstable/dllexport.h"
#ifdef _MSC_VER
#pragma message("IncrementalFixedLagSmoother was moved to the gtsam/nonlinear directory")
#else
#warning "IncrementalFixedLagSmoother was moved to the gtsam/nonlinear directory"
#endif
namespace gtsam {
/**
* This is a base class for the various HMF2 implementations. The HMF2 eliminates the factor graph
* such that the active states are placed in/near the root. This base class implements a function
* to calculate the ordering, and an update function to incorporate new factors into the HMF.
*/
class GTSAM_UNSTABLE_EXPORT IncrementalFixedLagSmoother: public FixedLagSmoother {
public:
/// Typedef for a shared pointer to an Incremental Fixed-Lag Smoother
typedef std::shared_ptr<IncrementalFixedLagSmoother> shared_ptr;
/** default constructor */
IncrementalFixedLagSmoother(double smootherLag = 0.0,
const ISAM2Params& parameters = DefaultISAM2Params()) :
FixedLagSmoother(smootherLag), isam_(parameters) {
}
/** destructor */
~IncrementalFixedLagSmoother() override {
}
/** Print the factor for debugging and testing (implementing Testable) */
void print(const std::string& s = "IncrementalFixedLagSmoother:\n",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
/** Check if two IncrementalFixedLagSmoother Objects are equal */
bool equals(const FixedLagSmoother& rhs, double tol = 1e-9) const override;
/**
* Add new factors, updating the solution and re-linearizing as needed.
* @param newFactors new factors on old and/or new variables
* @param newTheta new values for new variables only
* @param timestamps an (optional) map from keys to real time stamps
* @param factorsToRemove an (optional) list of factors to remove.
*/
Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(),
const Values& newTheta = Values(), //
const KeyTimestampMap& timestamps = KeyTimestampMap(),
const FactorIndices& factorsToRemove = FactorIndices()) override;
/** Compute an estimate from the incomplete linear delta computed during the last update.
* This delta is incomplete because it was not updated below wildfire_threshold. If only
* a single variable is needed, it is faster to call calculateEstimate(const KEY&).
*/
Values calculateEstimate() const override {
return isam_.calculateEstimate();
}
/** Compute an estimate for a single variable using its incomplete linear delta computed
* during the last update. This is faster than calling the no-argument version of
* calculateEstimate, which operates on all variables.
* @param key
* @return
*/
template<class VALUE>
VALUE calculateEstimate(Key key) const {
return isam_.calculateEstimate<VALUE>(key);
}
/** return the current set of iSAM2 parameters */
const ISAM2Params& params() const {
return isam_.params();
}
/** Access the current set of factors */
const NonlinearFactorGraph& getFactors() const {
return isam_.getFactorsUnsafe();
}
/** Access the current linearization point */
const Values& getLinearizationPoint() const {
return isam_.getLinearizationPoint();
}
/** Access the current set of deltas to the linearization point */
const VectorValues& getDelta() const {
return isam_.getDelta();
}
/// Calculate marginal covariance on given variable
Matrix marginalCovariance(Key key) const {
return isam_.marginalCovariance(key);
}
/// Get results of latest isam2 update
const ISAM2Result& getISAM2Result() const{ return isamResult_; }
/// Get the iSAM2 object which is used for the inference internally
const ISAM2& getISAM2() const { return isam_; }
protected:
/** Create default parameters */
static ISAM2Params DefaultISAM2Params() {
ISAM2Params params;
params.findUnusedFactorSlots = true;
return params;
}
/** An iSAM2 object used to perform inference. The smoother lag is controlled
* by what factors are removed each iteration */
ISAM2 isam_;
/** Store results of latest isam2 update */
ISAM2Result isamResult_;
/** Erase any keys associated with timestamps before the provided time */
void eraseKeysBefore(double timestamp);
/** Fill in an iSAM2 ConstrainedKeys structure such that the provided keys are eliminated before all others */
void createOrderingConstraints(const KeyVector& marginalizableKeys,
std::optional<FastMap<Key, int> >& constrainedKeys) const;
private:
/** Private methods for printing debug information */
static void PrintKeySet(const std::set<Key>& keys, const std::string& label =
"Keys:");
static void PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor);
static void PrintSymbolicGraph(const GaussianFactorGraph& graph,
const std::string& label = "Factor Graph:");
static void PrintSymbolicTree(const gtsam::ISAM2& isam,
const std::string& label = "Bayes Tree:");
static void PrintSymbolicTreeHelper(
const gtsam::ISAM2Clique::shared_ptr& clique, const std::string indent =
"");
};
// IncrementalFixedLagSmoother
}/// namespace gtsam
#include <gtsam/nonlinear/IncrementalFixedLagSmoother.h>