From 0d6a8e21decea383e6d19531334a99c3cc2edb0a Mon Sep 17 00:00:00 2001 From: p-zach Date: Sun, 6 Apr 2025 15:17:27 -0400 Subject: [PATCH] Move incremental fls --- .../IncrementalFixedLagSmootherExample.cpp | 2 +- .../nonlinear/IncrementalFixedLagSmoother.cpp | 2 +- gtsam/nonlinear/IncrementalFixedLagSmoother.h | 159 ++++++++++++++++++ gtsam/nonlinear/nonlinear.i | 14 ++ .../nonlinear/IncrementalFixedLagSmoother.h | 150 +---------------- 5 files changed, 181 insertions(+), 146 deletions(-) rename {gtsam_unstable/examples => examples}/IncrementalFixedLagSmootherExample.cpp (99%) rename {gtsam_unstable => gtsam}/nonlinear/IncrementalFixedLagSmoother.cpp (99%) create mode 100644 gtsam/nonlinear/IncrementalFixedLagSmoother.h diff --git a/gtsam_unstable/examples/IncrementalFixedLagSmootherExample.cpp b/examples/IncrementalFixedLagSmootherExample.cpp similarity index 99% rename from gtsam_unstable/examples/IncrementalFixedLagSmootherExample.cpp rename to examples/IncrementalFixedLagSmootherExample.cpp index 73beb6a75..fda7ad708 100644 --- a/gtsam_unstable/examples/IncrementalFixedLagSmootherExample.cpp +++ b/examples/IncrementalFixedLagSmootherExample.cpp @@ -59,7 +59,7 @@ #include #include #include -#include +#include #include #include // for writeG2o diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp b/gtsam/nonlinear/IncrementalFixedLagSmoother.cpp similarity index 99% rename from gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp rename to gtsam/nonlinear/IncrementalFixedLagSmoother.cpp index 0cd9ecbac..ed6f2dba3 100644 --- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp +++ b/gtsam/nonlinear/IncrementalFixedLagSmoother.cpp @@ -19,7 +19,7 @@ * @date Oct 14, 2012 */ -#include +#include #include #include diff --git a/gtsam/nonlinear/IncrementalFixedLagSmoother.h b/gtsam/nonlinear/IncrementalFixedLagSmoother.h new file mode 100644 index 000000000..ca70c3b58 --- /dev/null +++ b/gtsam/nonlinear/IncrementalFixedLagSmoother.h @@ -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 +#include +#include "gtsam_unstable/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 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 + VALUE calculateEstimate(Key key) const { + return isam_.calculateEstimate(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 >& constrainedKeys) const; + +private: + /** Private methods for printing debug information */ + static void PrintKeySet(const std::set& 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 diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 5092713b7..ef2a7218d 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -731,6 +731,20 @@ virtual class BatchFixedLagSmoother : gtsam::FixedLagSmoother { VALUE calculateEstimate(size_t key) const; }; +#include +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 template -#include -#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 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 - VALUE calculateEstimate(Key key) const { - return isam_.calculateEstimate(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 >& constrainedKeys) const; - -private: - /** Private methods for printing debug information */ - static void PrintKeySet(const std::set& 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 \ No newline at end of file