#include <IncrementalFixedLagSmoother.h>

Inheritance diagram for gtsam::IncrementalFixedLagSmoother:

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 &parameters=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 &timestamps=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="")

Detailed Description

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 nonlinear/IncrementalFixedLagSmoother.h.

Member Typedef Documentation

◆ shared_ptr

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 nonlinear/IncrementalFixedLagSmoother.h.

Constructor & Destructor Documentation

◆ IncrementalFixedLagSmoother()

gtsam::IncrementalFixedLagSmoother::IncrementalFixedLagSmoother	(	double	smootherLag = `0.0`,
		const ISAM2Params &	parameters = `DefaultISAM2Params()`
	)

inline

default constructor

Definition at line 42 of file nonlinear/IncrementalFixedLagSmoother.h.

◆ ~IncrementalFixedLagSmoother()

gtsam::IncrementalFixedLagSmoother::~IncrementalFixedLagSmoother ( )

inlineoverride

destructor

Definition at line 48 of file nonlinear/IncrementalFixedLagSmoother.h.

Member Function Documentation

◆ calculateEstimate() [1/2]

Values gtsam::IncrementalFixedLagSmoother::calculateEstimate ( ) const

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 nonlinear/IncrementalFixedLagSmoother.h.

◆ calculateEstimate() [2/2]

template<class VALUE >

VALUE gtsam::IncrementalFixedLagSmoother::calculateEstimate ( Key key ) const

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.

Parameters

key

Returns

Definition at line 85 of file nonlinear/IncrementalFixedLagSmoother.h.

◆ createOrderingConstraints()

void gtsam::IncrementalFixedLagSmoother::createOrderingConstraints	(	const KeyVector &	marginalizableKeys,
		std::optional< FastMap< Key, int > > &	constrainedKeys
	)		const

protected

Fill in an iSAM2 ConstrainedKeys structure such that the provided keys are eliminated before all others

Definition at line 150 of file IncrementalFixedLagSmoother.cpp.

◆ DefaultISAM2Params()

static ISAM2Params gtsam::IncrementalFixedLagSmoother::DefaultISAM2Params ( )

inlinestaticprotected

Create default parameters

Definition at line 123 of file nonlinear/IncrementalFixedLagSmoother.h.

◆ equals()

bool gtsam::IncrementalFixedLagSmoother::equals	(	const FixedLagSmoother &	rhs,
		double	tol = `1e-9`
	)		const

overridevirtual

Check if two IncrementalFixedLagSmoother Objects are equal

Reimplemented from gtsam::FixedLagSmoother.

Definition at line 36 of file IncrementalFixedLagSmoother.cpp.

◆ eraseKeysBefore()

void gtsam::IncrementalFixedLagSmoother::eraseKeysBefore ( double timestamp )

protected

Erase any keys associated with timestamps before the provided time

Definition at line 140 of file IncrementalFixedLagSmoother.cpp.

◆ getDelta()

const VectorValues& gtsam::IncrementalFixedLagSmoother::getDelta ( ) const

inline

Access the current set of deltas to the linearization point

Definition at line 105 of file nonlinear/IncrementalFixedLagSmoother.h.

◆ getFactors()

const NonlinearFactorGraph& gtsam::IncrementalFixedLagSmoother::getFactors ( ) const

inline

Access the current set of factors

Definition at line 95 of file nonlinear/IncrementalFixedLagSmoother.h.

◆ getISAM2()

const ISAM2& gtsam::IncrementalFixedLagSmoother::getISAM2 ( ) const

inline

Get the iSAM2 object which is used for the inference internally.

Definition at line 118 of file nonlinear/IncrementalFixedLagSmoother.h.

◆ getISAM2Result()

const ISAM2Result& gtsam::IncrementalFixedLagSmoother::getISAM2Result ( ) const

inline

Get results of latest isam2 update.

Definition at line 115 of file nonlinear/IncrementalFixedLagSmoother.h.

◆ getLinearizationPoint()

const Values& gtsam::IncrementalFixedLagSmoother::getLinearizationPoint ( ) const

inline

Access the current linearization point

Definition at line 100 of file nonlinear/IncrementalFixedLagSmoother.h.

◆ marginalCovariance()

Matrix gtsam::IncrementalFixedLagSmoother::marginalCovariance ( Key key ) const

inline

Calculate marginal covariance on given variable.

Definition at line 110 of file nonlinear/IncrementalFixedLagSmoother.h.

◆ params()

const ISAM2Params& gtsam::IncrementalFixedLagSmoother::params ( ) const

inline

return the current set of iSAM2 parameters

Definition at line 90 of file nonlinear/IncrementalFixedLagSmoother.h.

◆ print()

void gtsam::IncrementalFixedLagSmoother::print	(	const std::string &	s = `"IncrementalFixedLagSmoother:\n"`,
		const KeyFormatter &	keyFormatter = `DefaultKeyFormatter`
	)		const

overridevirtual

Print the factor for debugging and testing (implementing Testable)

Reimplemented from gtsam::FixedLagSmoother.

Definition at line 29 of file IncrementalFixedLagSmoother.cpp.

◆ PrintKeySet()

void gtsam::IncrementalFixedLagSmoother::PrintKeySet	(	const std::set< Key > &	keys,
		const std::string &	label = `"Keys:"`
	)

staticprivate

Private methods for printing debug information

Definition at line 168 of file IncrementalFixedLagSmoother.cpp.

◆ PrintSymbolicFactor()

void gtsam::IncrementalFixedLagSmoother::PrintSymbolicFactor ( const GaussianFactor::shared_ptr & factor )

staticprivate

Definition at line 178 of file IncrementalFixedLagSmoother.cpp.

◆ PrintSymbolicGraph()

void gtsam::IncrementalFixedLagSmoother::PrintSymbolicGraph	(	const GaussianFactorGraph &	graph,
		const std::string &	label = `"Factor Graph:"`
	)

staticprivate

Definition at line 188 of file IncrementalFixedLagSmoother.cpp.

◆ PrintSymbolicTree()

void gtsam::IncrementalFixedLagSmoother::PrintSymbolicTree	(	const gtsam::ISAM2 &	isam,
		const std::string &	label = `"Bayes Tree:"`
	)

staticprivate

Definition at line 197 of file IncrementalFixedLagSmoother.cpp.

◆ PrintSymbolicTreeHelper()

void gtsam::IncrementalFixedLagSmoother::PrintSymbolicTreeHelper	(	const gtsam::ISAM2Clique::shared_ptr &	clique,
		const std::string	indent = `""`
	)

staticprivate

Definition at line 209 of file IncrementalFixedLagSmoother.cpp.

◆ update()

FixedLagSmoother::Result gtsam::IncrementalFixedLagSmoother::update	(	const NonlinearFactorGraph &	newFactors = `NonlinearFactorGraph()`,
		const Values &	newTheta = `Values()`,
		const KeyTimestampMap &	timestamps = `KeyTimestampMap()`,
		const FactorIndices &	factorsToRemove = `FactorIndices()`
	)

overridevirtual

Add new factors, updating the solution and re-linearizing as needed.

Parameters

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 45 of file IncrementalFixedLagSmoother.cpp.

Member Data Documentation

◆ isam_

ISAM2 gtsam::IncrementalFixedLagSmoother::isam_

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 nonlinear/IncrementalFixedLagSmoother.h.

◆ isamResult_

ISAM2Result gtsam::IncrementalFixedLagSmoother::isamResult_

protected

Store results of latest isam2 update

Definition at line 134 of file nonlinear/IncrementalFixedLagSmoother.h.

The documentation for this class was generated from the following files:

Public Types

Public Member Functions

Protected Member Functions

Static Protected Member Functions

Protected Attributes

Static Private Member Functions

Detailed Description

Member Typedef Documentation

◆ shared_ptr

Constructor & Destructor Documentation

◆ IncrementalFixedLagSmoother()

◆ ~IncrementalFixedLagSmoother()

Member Function Documentation

◆ calculateEstimate() [1/2]

◆ calculateEstimate() [2/2]

◆ createOrderingConstraints()

◆ DefaultISAM2Params()

◆ equals()

◆ eraseKeysBefore()

◆ getDelta()

◆ getFactors()

◆ getISAM2()

◆ getISAM2Result()

◆ getLinearizationPoint()

◆ marginalCovariance()

◆ params()

◆ print()

◆ PrintKeySet()

◆ PrintSymbolicFactor()

◆ PrintSymbolicGraph()

◆ PrintSymbolicTree()

◆ PrintSymbolicTreeHelper()

◆ update()

Member Data Documentation

◆ isam_

◆ isamResult_