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.

Inheritance diagram for gtsam::IncrementalFixedLagSmoother:

Public Member Functions
	IncrementalFixedLagSmoother (double smootherLag=0.0, const ISAM2Params &parameters=DefaultISAM2Params())
	default constructor
	~IncrementalFixedLagSmoother () override
	destructor
void	print (const std::string &s="IncrementalFixedLagSmoother:\n", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
	Print the factor for debugging and testing (implementing Testable).
bool	equals (const FixedLagSmoother &rhs, double tol=1e-9) const override
	Check if two IncrementalFixedLagSmoother Objects are equal.
Result	update (const NonlinearFactorGraph &newFactors=NonlinearFactorGraph(), const Values &newTheta=Values(), const KeyTimestampMap &timestamps=KeyTimestampMap(), const FactorIndices &factorsToRemove=FactorIndices()) override
	Add new factors, updating the solution and re-linearizing as needed.
Values	calculateEstimate () const override
	Compute an estimate from the incomplete linear delta computed during the last update.
Values	calculateEstimate (const KeyVector &keys) const override
	Compute estimates for a set of variables only, one retract per key.
template<class VALUE>
VALUE	calculateEstimate (Key key) const
	Compute an estimate for a single variable using its incomplete linear delta computed during the last update.
const ISAM2Params &	params () const
	return the current set of iSAM2 parameters
const NonlinearFactorGraph &	getFactors () const
	Access the current set of factors.
const Values &	getLinearizationPoint () const
	Access the current linearization point.
const VectorValues &	getDelta () const
	Access the current set of deltas to the linearization point.
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.
Public Member Functions inherited from gtsam::FixedLagSmoother
	FixedLagSmoother (double smootherLag=0.0)
	default constructor
virtual	~FixedLagSmoother ()
	destructor
double	smootherLag () const
	read the current smoother lag
double &	smootherLag ()
	write to the current smoother lag
void	setSmootherLag (double smootherLag)
	Write to the current smoother lag.
const KeyTimestampMap &	timestamps () const
	Access the current set of timestamps associated with each variable.

Public Types
typedef std::shared_ptr< IncrementalFixedLagSmoother >	shared_ptr
	Typedef for a shared pointer to an Incremental Fixed-Lag Smoother.
Public Types inherited from gtsam::FixedLagSmoother
typedef std::shared_ptr< FixedLagSmoother >	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

Protected Member Functions
void	eraseKeysBefore (double timestamp)
	Erase any keys associated with timestamps before the provided time.
void	createOrderingConstraints (const KeyVector &marginalizableKeys, const std::function< bool(Key)> &isActive, std::optional< FastMap< Key, int > > &constrainedKeys) const
	Fill in an iSAM2 ConstrainedKeys structure such that the provided keys are eliminated before all others.
Protected Member Functions inherited from gtsam::FixedLagSmoother
void	updateKeyTimestampMap (const KeyTimestampMap &newTimestamps)
	Update the Timestamps associated with the keys.
void	eraseKeyTimestampMap (const KeyVector &keys)
	Erase keys from the Key-Timestamps database.
double	getCurrentTimestamp () const
	Find the most recent timestamp of the system.
KeyVector	findKeysBefore (double timestamp) const
	Find all of the keys associated with timestamps before the provided time.
KeyVector	findKeysAfter (double timestamp) const
	Find all of the keys associated with timestamps before the provided time.

Static Protected Member Functions
static ISAM2Params	DefaultISAM2Params ()
	Create default parameters.

Protected Attributes
ISAM2	isam_
	An iSAM2 object used to perform inference.
ISAM2Result	isamResult_
	Store results of latest isam2 update.
Protected Attributes inherited from gtsam::FixedLagSmoother
double	smootherLag_
	The length of the smoother lag.
TimestampKeyMap	timestampKeyMap_
	The current timestamp associated with each tracked key.
KeyTimestampMap	keyTimestampMap_

Member Function Documentation

◆ calculateEstimate() [1/3]

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/3]

Values gtsam::IncrementalFixedLagSmoother::calculateEstimate ( const KeyVector & keys ) const

inlineoverridevirtual

Compute estimates for a set of variables only, one retract per key.

Reimplemented from gtsam::FixedLagSmoother.

◆ calculateEstimate() [3/3]

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

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

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

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

Every key in timestamps must name a value the smoother already holds or one supplied in newTheta in this update; otherwise the update throws std::invalid_argument identifying the key, before any state is mutated, so a rejected update leaves the smoother unchanged. Timestamp-before-value is not supported. A timestamp for an existing value is accepted whether or not a factor references the key yet, and participates in the smoother clock normally.

If this validation fails, no timestamp is retained and no part of the update is applied. A caller that later supplies the value must supply its timestamp again; until timestamped, that value is not eligible for fixed-lag expiration.

Removal indices must be within the factor graph at update entry, including empty slots; adding factors cannot make an invalid index valid. Removal indices are checked before timestamp keys. Either validation failure leaves the smoother unchanged, with no part of the update applied.

Exceptions

std::out_of_range	identifying a removal index outside the graph.
std::invalid_argument	if a key in `timestamps` has no value in the smoother or in `newTheta`.

Implements gtsam::FixedLagSmoother.

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

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

/tmp/gtsam-4.3.0-doxygen.rsXPUS/source/gtsam/nonlinear/IncrementalFixedLagSmoother.h
/tmp/gtsam-4.3.0-doxygen.rsXPUS/source/gtsam/nonlinear/IncrementalFixedLagSmoother.cpp

Detailed Description

Public Member Functions

Public Types

Protected Member Functions

Static Protected Member Functions

Protected Attributes

Member Function Documentation

◆ calculateEstimate() [1/3]

◆ calculateEstimate() [2/3]

◆ calculateEstimate() [3/3]

◆ equals()

◆ print()

◆ update()

Member Data Documentation

◆ isam_