#include <IncrementalFixedLagSmoother.h>

Inheritance diagram for gtsam::IncrementalFixedLagSmoother:

Collaboration diagram for gtsam::IncrementalFixedLagSmoother:

[legend]

Public Types
typedef std::shared_ptr< IncrementalFixedLagSmoother >	shared_ptr
	Typedef for a shared pointer to an Incremental Fixed-Lag Smoother.

typedef std::map< Key, double >	KeyTimestampMap
	Typedef for a Key-Timestamp map/database.

typedef std::multimap< double, Key >	TimestampKeyMap

Public Member Functions
	IncrementalFixedLagSmoother (double smootherLag=0.0, const ISAM2Params &parameters=DefaultISAM2Params())

	~IncrementalFixedLagSmoother () override

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

bool	equals (const FixedLagSmoother &rhs, double tol=1e-9) const override

Result	update (const NonlinearFactorGraph &newFactors=NonlinearFactorGraph(), const Values &newTheta=Values(), const KeyTimestampMap &timestamps=KeyTimestampMap(), const FactorIndices &factorsToRemove=FactorIndices()) override

Values	calculateEstimate () const override

template<class VALUE >
VALUE	calculateEstimate (Key key) const

const ISAM2Params &	params () const

const NonlinearFactorGraph &	getFactors () const

const Values &	getLinearizationPoint () const

const VectorValues &	getDelta () const

Matrix	marginalCovariance (Key key) const
	Calculate marginal covariance on given variable.

const ISAM2Result &	getISAM2Result () const
	Get results of latest isam2 update.

const ISAM2 &	getISAM2 () const
	Get the iSAM2 object which is used for the inference internally.

double	smootherLag () const

double &	smootherLag ()

const KeyTimestampMap &	timestamps () const

Protected Member Functions
void	eraseKeysBefore (double timestamp)

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

void	updateKeyTimestampMap (const KeyTimestampMap &newTimestamps)

void	eraseKeyTimestampMap (const KeyVector &keys)

double	getCurrentTimestamp () const

KeyVector	findKeysBefore (double timestamp) const

KeyVector	findKeysAfter (double timestamp) const

Static Protected Member Functions
static ISAM2Params	DefaultISAM2Params ()

Protected Attributes
ISAM2	isam_

ISAM2Result	isamResult_

double	smootherLag_

TimestampKeyMap	timestampKeyMap_

KeyTimestampMap	keyTimestampMap_

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.

Constructor & Destructor Documentation

◆ IncrementalFixedLagSmoother()

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

inline

default constructor

◆ ~IncrementalFixedLagSmoother()

gtsam::IncrementalFixedLagSmoother::~IncrementalFixedLagSmoother ( )

inlineoverride

destructor

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.

◆ 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

◆ 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

◆ DefaultISAM2Params()

static ISAM2Params gtsam::IncrementalFixedLagSmoother::DefaultISAM2Params ( )

inlinestaticprotected

Create default parameters

◆ 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.

◆ eraseKeysBefore()

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

protected

Erase any keys associated with timestamps before the provided time

◆ eraseKeyTimestampMap()

void gtsam::FixedLagSmoother::eraseKeyTimestampMap ( const KeyVector & keys )

protectedinherited

Erase keys from the Key-Timestamps database

◆ findKeysAfter()

KeyVector gtsam::FixedLagSmoother::findKeysAfter ( double timestamp ) const

protectedinherited

Find all of the keys associated with timestamps before the provided time

◆ findKeysBefore()

KeyVector gtsam::FixedLagSmoother::findKeysBefore ( double timestamp ) const

protectedinherited

Find all of the keys associated with timestamps before the provided time

◆ getCurrentTimestamp()

double gtsam::FixedLagSmoother::getCurrentTimestamp ( ) const

protectedinherited

Find the most recent timestamp of the system

◆ getDelta()

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

inline

Access the current set of deltas to the linearization point

◆ getFactors()

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

inline

Access the current set of factors

◆ getLinearizationPoint()

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

inline

Access the current linearization point

◆ params()

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

inline

return the current set of iSAM2 parameters

◆ print()

void gtsam::IncrementalFixedLagSmoother::print ( )

overridevirtual

Print the factor for debugging and testing (implementing Testable)

Reimplemented from gtsam::FixedLagSmoother.

◆ smootherLag() [1/2]

double gtsam::FixedLagSmoother::smootherLag ( ) const

inlineinherited

read the current smoother lag

◆ smootherLag() [2/2]

double& gtsam::FixedLagSmoother::smootherLag ( )

inlineinherited

write to the current smoother lag

◆ timestamps()

const KeyTimestampMap& gtsam::FixedLagSmoother::timestamps ( ) const

inlineinherited

Access the current set of timestamps associated with each variable

◆ update()

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.

◆ updateKeyTimestampMap()

void gtsam::FixedLagSmoother::updateKeyTimestampMap ( const KeyTimestampMap & newTimestamps )

protectedinherited

Update the Timestamps associated with the keys

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

◆ isamResult_

ISAM2Result gtsam::IncrementalFixedLagSmoother::isamResult_

protected

Store results of latest isam2 update

◆ smootherLag_

double gtsam::FixedLagSmoother::smootherLag_

protectedinherited

The length of the smoother lag. Any variable older than this amount will be marginalized out.

◆ timestampKeyMap_

TimestampKeyMap gtsam::FixedLagSmoother::timestampKeyMap_

protectedinherited

The current timestamp associated with each tracked key

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

/home/docs/checkouts/readthedocs.org/user_builds/gtsam-jlblanco-docs/checkouts/latest/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h

Public Types

Public Member Functions

Protected Member Functions

Static Protected Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

◆ IncrementalFixedLagSmoother()

◆ ~IncrementalFixedLagSmoother()

Member Function Documentation

◆ calculateEstimate() [1/2]

◆ calculateEstimate() [2/2]

◆ createOrderingConstraints()

◆ DefaultISAM2Params()

◆ equals()

◆ eraseKeysBefore()

◆ eraseKeyTimestampMap()

◆ findKeysAfter()

◆ findKeysBefore()

◆ getCurrentTimestamp()

◆ getDelta()

◆ getFactors()

◆ getLinearizationPoint()

◆ params()

◆ print()

◆ smootherLag() [1/2]

◆ smootherLag() [2/2]

◆ timestamps()

◆ update()

◆ updateKeyTimestampMap()

Member Data Documentation

◆ isam_

◆ isamResult_

◆ smootherLag_

◆ timestampKeyMap_