#include <IncrementalFixedLagSmoother.h>

Inheritance diagram for gtsam::IncrementalFixedLagSmoother:

Public Types
typedef boost::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 boost::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 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 () const

double &	smootherLag ()

const KeyTimestampMap &	timestamps () const

virtual	~FixedLagSmoother ()

Protected Member Functions
void	createOrderingConstraints (const KeyVector &marginalizableKeys, boost::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 33 of file IncrementalFixedLagSmoother.h.

Member Typedef Documentation

typedef boost::shared_ptr<IncrementalFixedLagSmoother> gtsam::IncrementalFixedLagSmoother::shared_ptr

Typedef for a shared pointer to an Incremental Fixed-Lag Smoother.

Definition at line 38 of file IncrementalFixedLagSmoother.h.

Constructor & Destructor Documentation

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

inline

default constructor

Definition at line 41 of file IncrementalFixedLagSmoother.h.

gtsam::IncrementalFixedLagSmoother::~IncrementalFixedLagSmoother ( )

inlineoverride

destructor

Definition at line 47 of file IncrementalFixedLagSmoother.h.

Member Function Documentation

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 73 of file IncrementalFixedLagSmoother.h.

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 84 of file IncrementalFixedLagSmoother.h.

void gtsam::IncrementalFixedLagSmoother::createOrderingConstraints	(	const KeyVector &	marginalizableKeys,
		boost::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 176 of file IncrementalFixedLagSmoother.cpp.

static ISAM2Params gtsam::IncrementalFixedLagSmoother::DefaultISAM2Params ( )

inlinestaticprotected

Create default parameters

Definition at line 119 of file IncrementalFixedLagSmoother.h.

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

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

protected

Erase any keys associated with timestamps before the provided time

Definition at line 166 of file IncrementalFixedLagSmoother.cpp.

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

inline

Access the current set of deltas to the linearization point

Definition at line 104 of file IncrementalFixedLagSmoother.h.

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

inline

Access the current set of factors

Definition at line 94 of file IncrementalFixedLagSmoother.h.

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

inline

Get results of latest isam2 update.

Definition at line 114 of file IncrementalFixedLagSmoother.h.

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

inline

Access the current linearization point

Definition at line 99 of file IncrementalFixedLagSmoother.h.

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

inline

Calculate marginal covariance on given variable.

Definition at line 109 of file IncrementalFixedLagSmoother.h.

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

inline

return the current set of iSAM2 parameters

Definition at line 89 of file IncrementalFixedLagSmoother.h.

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

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

staticprivate

Private methods for printing debug information

Definition at line 194 of file IncrementalFixedLagSmoother.cpp.

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

staticprivate

Definition at line 204 of file IncrementalFixedLagSmoother.cpp.

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

staticprivate

Definition at line 214 of file IncrementalFixedLagSmoother.cpp.

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

staticprivate

Definition at line 223 of file IncrementalFixedLagSmoother.cpp.

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

staticprivate

Definition at line 235 of file IncrementalFixedLagSmoother.cpp.

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