#include <IncrementalFixedLagSmoother.h>
Public Types | |
typedef std::shared_ptr< IncrementalFixedLagSmoother > | shared_ptr |
Typedef for a shared pointer to an Incremental Fixed-Lag Smoother. More... | |
Public Types inherited from gtsam::FixedLagSmoother | |
typedef std::map< Key, double > | KeyTimestampMap |
Typedef for a Key-Timestamp map/database. More... | |
typedef std::shared_ptr< FixedLagSmoother > | shared_ptr |
Typedef for a shared pointer to an Incremental Fixed-Lag Smoother. More... | |
typedef std::multimap< double, Key > | TimestampKeyMap |
Public Member Functions | |
Values | calculateEstimate () const override |
template<class VALUE > | |
VALUE | calculateEstimate (Key key) const |
bool | equals (const FixedLagSmoother &rhs, double tol=1e-9) const override |
const VectorValues & | getDelta () const |
const NonlinearFactorGraph & | getFactors () const |
const ISAM2 & | getISAM2 () const |
Get the iSAM2 object which is used for the inference internally. More... | |
const ISAM2Result & | getISAM2Result () const |
Get results of latest isam2 update. More... | |
const Values & | getLinearizationPoint () const |
IncrementalFixedLagSmoother (double smootherLag=0.0, const ISAM2Params ¶meters=DefaultISAM2Params()) | |
Matrix | marginalCovariance (Key key) const |
Calculate marginal covariance on given variable. More... | |
const ISAM2Params & | params () const |
void | print (const std::string &s="IncrementalFixedLagSmoother:\n", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override |
Result | update (const NonlinearFactorGraph &newFactors=NonlinearFactorGraph(), const Values &newTheta=Values(), const KeyTimestampMap ×tamps=KeyTimestampMap(), const FactorIndices &factorsToRemove=FactorIndices()) override |
~IncrementalFixedLagSmoother () override | |
Public Member Functions inherited from gtsam::FixedLagSmoother | |
FixedLagSmoother (double smootherLag=0.0) | |
double & | smootherLag () |
double | smootherLag () const |
const KeyTimestampMap & | timestamps () const |
virtual | ~FixedLagSmoother () |
Protected Member Functions | |
void | createOrderingConstraints (const KeyVector &marginalizableKeys, std::optional< FastMap< Key, int > > &constrainedKeys) const |
void | eraseKeysBefore (double timestamp) |
Protected Member Functions inherited from gtsam::FixedLagSmoother | |
void | eraseKeyTimestampMap (const KeyVector &keys) |
KeyVector | findKeysAfter (double timestamp) const |
KeyVector | findKeysBefore (double timestamp) const |
double | getCurrentTimestamp () const |
void | updateKeyTimestampMap (const KeyTimestampMap &newTimestamps) |
Static Protected Member Functions | |
static ISAM2Params | DefaultISAM2Params () |
Protected Attributes | |
ISAM2 | isam_ |
ISAM2Result | isamResult_ |
Protected Attributes inherited from gtsam::FixedLagSmoother | |
KeyTimestampMap | keyTimestampMap_ |
double | smootherLag_ |
TimestampKeyMap | timestampKeyMap_ |
Static Private Member Functions | |
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="") |
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.
Definition at line 34 of file IncrementalFixedLagSmoother.h.
typedef std::shared_ptr<IncrementalFixedLagSmoother> gtsam::IncrementalFixedLagSmoother::shared_ptr |
Typedef for a shared pointer to an Incremental Fixed-Lag Smoother.
Definition at line 39 of file IncrementalFixedLagSmoother.h.
|
inline |
default constructor
Definition at line 42 of file IncrementalFixedLagSmoother.h.
|
inlineoverride |
destructor
Definition at line 48 of file IncrementalFixedLagSmoother.h.
|
inlineoverridevirtual |
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&).
Implements gtsam::FixedLagSmoother.
Definition at line 74 of file IncrementalFixedLagSmoother.h.
|
inline |
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.
key |
Definition at line 85 of file IncrementalFixedLagSmoother.h.
|
protected |
Fill in an iSAM2 ConstrainedKeys structure such that the provided keys are eliminated before all others
Definition at line 176 of file IncrementalFixedLagSmoother.cpp.
|
inlinestaticprotected |
Create default parameters
Definition at line 123 of file IncrementalFixedLagSmoother.h.
|
overridevirtual |
Check if two IncrementalFixedLagSmoother Objects are equal
Reimplemented from gtsam::FixedLagSmoother.
Definition at line 57 of file IncrementalFixedLagSmoother.cpp.
|
protected |
Erase any keys associated with timestamps before the provided time
Definition at line 166 of file IncrementalFixedLagSmoother.cpp.
|
inline |
Access the current set of deltas to the linearization point
Definition at line 105 of file IncrementalFixedLagSmoother.h.
|
inline |
Access the current set of factors
Definition at line 95 of file IncrementalFixedLagSmoother.h.
|
inline |
Get the iSAM2 object which is used for the inference internally.
Definition at line 118 of file IncrementalFixedLagSmoother.h.
|
inline |
Get results of latest isam2 update.
Definition at line 115 of file IncrementalFixedLagSmoother.h.
|
inline |
Access the current linearization point
Definition at line 100 of file IncrementalFixedLagSmoother.h.
Calculate marginal covariance on given variable.
Definition at line 110 of file IncrementalFixedLagSmoother.h.
|
inline |
return the current set of iSAM2 parameters
Definition at line 90 of file IncrementalFixedLagSmoother.h.
|
overridevirtual |
Print the factor for debugging and testing (implementing Testable)
Reimplemented from gtsam::FixedLagSmoother.
Definition at line 50 of file IncrementalFixedLagSmoother.cpp.
|
staticprivate |
Private methods for printing debug information
Definition at line 194 of file IncrementalFixedLagSmoother.cpp.
|
staticprivate |
Definition at line 204 of file IncrementalFixedLagSmoother.cpp.
|
staticprivate |
Definition at line 214 of file IncrementalFixedLagSmoother.cpp.
|
staticprivate |
Definition at line 223 of file IncrementalFixedLagSmoother.cpp.
|
staticprivate |
Definition at line 235 of file IncrementalFixedLagSmoother.cpp.
|
overridevirtual |
Add new factors, updating the solution and re-linearizing as needed.
newFactors | new factors on old and/or new variables |
newTheta | new values for new variables only |
timestamps | an (optional) map from keys to real time stamps |
factorsToRemove | an (optional) list of factors to remove. |
Implements gtsam::FixedLagSmoother.
Definition at line 66 of file IncrementalFixedLagSmoother.cpp.
|
protected |
An iSAM2 object used to perform inference. The smoother lag is controlled by what factors are removed each iteration
Definition at line 131 of file IncrementalFixedLagSmoother.h.
|
protected |
Store results of latest isam2 update
Definition at line 134 of file IncrementalFixedLagSmoother.h.