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

Move incremental fls

release/4.3a0
p-zach 2025-04-06 15:17:27 -04:00
parent f4a79517c1
commit 0d6a8e21de
5 changed files with 181 additions and 146 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

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

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

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_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<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_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/nonlinear/ExtendedKalmanFilter.h>
template <T = {gtsam::Point2,
gtsam::Point3,

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>