#include <BatchFixedLagSmoother.h>

Inheritance diagram for gtsam::BatchFixedLagSmoother:

Public Types
typedef std::shared_ptr< BatchFixedLagSmoother >	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
	BatchFixedLagSmoother (double smootherLag=0.0, const LevenbergMarquardtParams &parameters=LevenbergMarquardtParams(), bool enforceConsistency=true)

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 Values &	getLinearizationPoint () const

const Ordering &	getOrdering () const

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

const LevenbergMarquardtParams &	params () const

LevenbergMarquardtParams &	params ()

void	print (const std::string &s="BatchFixedLagSmoother:\, 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

	~BatchFixedLagSmoother () override

Public Member Functions inherited from gtsam::FixedLagSmoother
	FixedLagSmoother (double smootherLag=0.0)

double	smootherLag () const

double &	smootherLag ()

const KeyTimestampMap &	timestamps () const

virtual	~FixedLagSmoother ()

Static Public Member Functions
static GaussianFactorGraph	CalculateMarginalFactors (const GaussianFactorGraph &graph, const KeyVector &keys, const GaussianFactorGraph::Eliminate &eliminateFunction=EliminatePreferCholesky)

static NonlinearFactorGraph	CalculateMarginalFactors (const NonlinearFactorGraph &graph, const Values &theta, const KeyVector &keys, const GaussianFactorGraph::Eliminate &eliminateFunction=EliminatePreferCholesky)
	Marginalize specific keys from a nonlinear graph, wrap in LinearContainers. More...

Protected Types
typedef std::map< Key, std::set< Key > >	FactorIndex

Protected Member Functions
void	eraseKeys (const KeyVector &keys)

void	insertFactors (const NonlinearFactorGraph &newFactors)

void	marginalize (const KeyVector &marginalizableKeys)

Result	optimize ()

void	removeFactors (const std::set< size_t > &deleteFactors)

void	reorder (const KeyVector &marginalizeKeys=KeyVector())

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)

Protected Attributes
std::queue< size_t >	availableSlots_

VectorValues	delta_

bool	enforceConsistency_

FactorIndex	factorIndex_

NonlinearFactorGraph	factors_

Values	linearValues_

Ordering	ordering_

LevenbergMarquardtParams	parameters_

Values	theta_

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)

static void	PrintKeySet (const gtsam::KeySet &keys, const std::string &label)

static void	PrintSymbolicFactor (const NonlinearFactor::shared_ptr &factor)

static void	PrintSymbolicFactor (const GaussianFactor::shared_ptr &factor)

static void	PrintSymbolicGraph (const NonlinearFactorGraph &graph, const std::string &label)

static void	PrintSymbolicGraph (const GaussianFactorGraph &graph, const std::string &label)

Detailed Description

Definition at line 29 of file nonlinear/BatchFixedLagSmoother.h.

Member Typedef Documentation

◆ FactorIndex

typedef std::map<Key, std::set<Key> > gtsam::BatchFixedLagSmoother::FactorIndex

protected

A typedef defining an Key-Factor mapping

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

◆ shared_ptr

typedef std::shared_ptr<BatchFixedLagSmoother> gtsam::BatchFixedLagSmoother::shared_ptr

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

Definition at line 34 of file nonlinear/BatchFixedLagSmoother.h.

Constructor & Destructor Documentation

◆ BatchFixedLagSmoother()

gtsam::BatchFixedLagSmoother::BatchFixedLagSmoother	(	double	smootherLag = `0.0`,
		const LevenbergMarquardtParams &	parameters = `LevenbergMarquardtParams()`,
		bool	enforceConsistency = `true`
	)

inline

default constructor

Definition at line 37 of file nonlinear/BatchFixedLagSmoother.h.

◆ ~BatchFixedLagSmoother()

gtsam::BatchFixedLagSmoother::~BatchFixedLagSmoother ( )

inlineoverride

destructor

Definition at line 41 of file nonlinear/BatchFixedLagSmoother.h.

Member Function Documentation

◆ calculateEstimate() [1/2]

Values gtsam::BatchFixedLagSmoother::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 59 of file nonlinear/BatchFixedLagSmoother.h.

◆ calculateEstimate() [2/2]

template<class VALUE >

VALUE gtsam::BatchFixedLagSmoother::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 70 of file nonlinear/BatchFixedLagSmoother.h.

◆ CalculateMarginalFactors() [1/2]

GaussianFactorGraph gtsam::BatchFixedLagSmoother::CalculateMarginalFactors	(	const GaussianFactorGraph &	graph,
		const KeyVector &	keys,
		const GaussianFactorGraph::Eliminate &	eliminateFunction = `EliminatePreferCholesky`
	)

static

Marginalize specific keys from a linear graph. Does not check whether keys actually exist in graph. In that case will fail somewhere deep within elimination

Definition at line 410 of file BatchFixedLagSmoother.cpp.

◆ CalculateMarginalFactors() [2/2]

NonlinearFactorGraph gtsam::BatchFixedLagSmoother::CalculateMarginalFactors	(	const NonlinearFactorGraph &	graph,
		const Values &	theta,
		const KeyVector &	keys,
		const GaussianFactorGraph::Eliminate &	eliminateFunction = `EliminatePreferCholesky`
	)

static

Marginalize specific keys from a nonlinear graph, wrap in LinearContainers.

Definition at line 423 of file BatchFixedLagSmoother.cpp.

◆ equals()

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

overridevirtual

Check if two IncrementalFixedLagSmoother Objects are equal

Reimplemented from gtsam::FixedLagSmoother.

Definition at line 38 of file BatchFixedLagSmoother.cpp.

◆ eraseKeys()

void gtsam::BatchFixedLagSmoother::eraseKeys ( const KeyVector & keys )

protected

Erase any keys associated with timestamps before the provided time

Definition at line 154 of file BatchFixedLagSmoother.cpp.

◆ getDelta()

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

inline

Access the current set of deltas to the linearization point

Definition at line 101 of file nonlinear/BatchFixedLagSmoother.h.

◆ getFactors()

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

inline

Access the current set of factors

Definition at line 86 of file nonlinear/BatchFixedLagSmoother.h.

◆ getLinearizationPoint()

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

inline

Access the current linearization point

Definition at line 91 of file nonlinear/BatchFixedLagSmoother.h.

◆ getOrdering()

const Ordering& gtsam::BatchFixedLagSmoother::getOrdering ( ) const

inline

Access the current ordering

Definition at line 96 of file nonlinear/BatchFixedLagSmoother.h.

◆ insertFactors()

void gtsam::BatchFixedLagSmoother::insertFactors ( const NonlinearFactorGraph & newFactors )

protected

Augment the list of factors with a set of new factors

Definition at line 112 of file BatchFixedLagSmoother.cpp.

◆ marginalCovariance()

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

Calculate marginal covariance on given variable.

Definition at line 47 of file BatchFixedLagSmoother.cpp.

◆ marginalize()

void gtsam::BatchFixedLagSmoother::marginalize ( const KeyVector & marginalizableKeys )

protected

Marginalize out selected variables

Definition at line 310 of file BatchFixedLagSmoother.cpp.

◆ optimize()

FixedLagSmoother::Result gtsam::BatchFixedLagSmoother::optimize ( )

protected

Optimize the current graph using a modified version of L-M

Definition at line 185 of file BatchFixedLagSmoother.cpp.

◆ params() [1/2]

const LevenbergMarquardtParams& gtsam::BatchFixedLagSmoother::params ( ) const

inline

read the current set of optimizer parameters

Definition at line 76 of file nonlinear/BatchFixedLagSmoother.h.

◆ params() [2/2]

LevenbergMarquardtParams& gtsam::BatchFixedLagSmoother::params ( )

inline

update the current set of optimizer parameters

Definition at line 81 of file nonlinear/BatchFixedLagSmoother.h.

◆ print()

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

overridevirtual

Print the factor for debugging and testing (implementing Testable)

Reimplemented from gtsam::FixedLagSmoother.

Definition at line 31 of file BatchFixedLagSmoother.cpp.

◆ PrintKeySet() [1/2]

void gtsam::BatchFixedLagSmoother::PrintKeySet	(	const std::set< Key > &	keys,
		const std::string &	label
	)

staticprivate

Private methods for printing debug information

Definition at line 348 of file BatchFixedLagSmoother.cpp.

◆ PrintKeySet() [2/2]

void gtsam::BatchFixedLagSmoother::PrintKeySet	(	const gtsam::KeySet &	keys,
		const std::string &	label
	)

staticprivate

Definition at line 358 of file BatchFixedLagSmoother.cpp.

◆ PrintSymbolicFactor() [1/2]

void gtsam::BatchFixedLagSmoother::PrintSymbolicFactor ( const NonlinearFactor::shared_ptr & factor )

staticprivate

Definition at line 368 of file BatchFixedLagSmoother.cpp.

◆ PrintSymbolicFactor() [2/2]

static void gtsam::BatchFixedLagSmoother::PrintSymbolicFactor ( const GaussianFactor::shared_ptr & factor )

staticprivate

◆ PrintSymbolicGraph() [1/2]

void gtsam::BatchFixedLagSmoother::PrintSymbolicGraph	(	const NonlinearFactorGraph &	graph,
		const std::string &	label
	)

staticprivate

Definition at line 392 of file BatchFixedLagSmoother.cpp.

◆ PrintSymbolicGraph() [2/2]

void gtsam::BatchFixedLagSmoother::PrintSymbolicGraph	(	const GaussianFactorGraph &	graph,
		const std::string &	label
	)

staticprivate

Definition at line 401 of file BatchFixedLagSmoother.cpp.

◆ removeFactors()

void gtsam::BatchFixedLagSmoother::removeFactors ( const std::set< size_t > & deleteFactors )

protected

Remove factors from the list of factors by slot index

Definition at line 133 of file BatchFixedLagSmoother.cpp.

◆ reorder()

void gtsam::BatchFixedLagSmoother::reorder ( const KeyVector & marginalizeKeys = KeyVector() )

protected

Use colamd to update into an efficient ordering

Definition at line 179 of file BatchFixedLagSmoother.cpp.

◆ update()

FixedLagSmoother::Result gtsam::BatchFixedLagSmoother::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 relinearizing as needed.

Implements gtsam::FixedLagSmoother.

Definition at line 53 of file BatchFixedLagSmoother.cpp.

Member Data Documentation

◆ availableSlots_

std::queue<size_t> gtsam::BatchFixedLagSmoother::availableSlots_

protected

The set of available factor graph slots. These occur because we are constantly deleting factors, leaving holes.

Definition at line 149 of file nonlinear/BatchFixedLagSmoother.h.

◆ delta_

VectorValues gtsam::BatchFixedLagSmoother::delta_

protected

The current set of linear deltas

Definition at line 146 of file nonlinear/BatchFixedLagSmoother.h.

◆ enforceConsistency_

bool gtsam::BatchFixedLagSmoother::enforceConsistency_

protected

A flag indicating if the optimizer should enforce probabilistic consistency by maintaining the linearization point of all variables involved in linearized/marginal factors at the edge of the smoothing window. This idea is from ??? TODO: Look up paper reference

Definition at line 131 of file nonlinear/BatchFixedLagSmoother.h.

◆ factorIndex_

FactorIndex gtsam::BatchFixedLagSmoother::factorIndex_

protected

A cross-reference structure to allow efficient factor lookups by key

Definition at line 152 of file nonlinear/BatchFixedLagSmoother.h.

◆ factors_

NonlinearFactorGraph gtsam::BatchFixedLagSmoother::factors_

protected

The nonlinear factors

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

◆ linearValues_

Values gtsam::BatchFixedLagSmoother::linearValues_

protected

The set of values involved in current linear factors.

Definition at line 140 of file nonlinear/BatchFixedLagSmoother.h.

◆ ordering_

Ordering gtsam::BatchFixedLagSmoother::ordering_

protected

The current ordering

Definition at line 143 of file nonlinear/BatchFixedLagSmoother.h.

◆ parameters_

LevenbergMarquardtParams gtsam::BatchFixedLagSmoother::parameters_

protected

The L-M optimization parameters

Definition at line 126 of file nonlinear/BatchFixedLagSmoother.h.

◆ theta_

Values gtsam::BatchFixedLagSmoother::theta_

protected

The current linearization point

Definition at line 137 of file nonlinear/BatchFixedLagSmoother.h.

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

Public Types

Public Member Functions

Static Public Member Functions

Protected Types

Protected Member Functions

Protected Attributes

Static Private Member Functions

Detailed Description

Member Typedef Documentation

◆ FactorIndex

◆ shared_ptr

Constructor & Destructor Documentation

◆ BatchFixedLagSmoother()

◆ ~BatchFixedLagSmoother()

Member Function Documentation

◆ calculateEstimate() [1/2]

◆ calculateEstimate() [2/2]

◆ CalculateMarginalFactors() [1/2]

◆ CalculateMarginalFactors() [2/2]

◆ equals()

◆ eraseKeys()

◆ getDelta()

◆ getFactors()

◆ getLinearizationPoint()

◆ getOrdering()

◆ insertFactors()

◆ marginalCovariance()

◆ marginalize()

◆ optimize()

◆ params() [1/2]

◆ params() [2/2]

◆ print()

◆ PrintKeySet() [1/2]

◆ PrintKeySet() [2/2]

◆ PrintSymbolicFactor() [1/2]

◆ PrintSymbolicFactor() [2/2]

◆ PrintSymbolicGraph() [1/2]

◆ PrintSymbolicGraph() [2/2]

◆ removeFactors()

◆ reorder()

◆ update()

Member Data Documentation

◆ availableSlots_

◆ delta_

◆ enforceConsistency_

◆ factorIndex_

◆ factors_

◆ linearValues_

◆ ordering_

◆ parameters_

◆ theta_