#include <ISAM2.h>

Inheritance diagram for gtsam::ISAM2:

Public Types
using	Base = BayesTree< ISAM2Clique >
	The BayesTree base class. More...

using	Clique = Base::Clique
	A clique. More...

using	Cliques = Base::Cliques
	List of Cliques. More...

using	sharedClique = Base::sharedClique
	Shared pointer to a clique. More...

using	This = ISAM2
	This class. More...

Public Types inherited from gtsam::BayesTree< ISAM2Clique >
typedef ISAM2Clique::BayesNetType	BayesNetType

typedef ISAM2Clique	Clique
	The clique type, normally BayesTreeClique. More...

typedef FastList< sharedClique >	Cliques

typedef ISAM2Clique::ConditionalType	ConditionalType

typedef FactorGraphType::Eliminate	Eliminate

typedef ISAM2Clique::EliminationTraitsType	EliminationTraitsType

typedef ISAM2Clique::FactorGraphType	FactorGraphType

typedef ISAM2Clique::FactorType	FactorType

typedef Clique	Node
	Synonym for Clique (TODO: remove) More...

typedef ConcurrentMap< Key, sharedClique >	Nodes

typedef FastVector< sharedClique >	Roots

typedef boost::shared_ptr< BayesNetType >	sharedBayesNet

typedef boost::shared_ptr< Clique >	sharedClique
	Shared pointer to a clique. More...

typedef boost::shared_ptr< ConditionalType >	sharedConditional

typedef boost::shared_ptr< FactorType >	sharedFactor

typedef boost::shared_ptr< FactorGraphType >	sharedFactorGraph

typedef sharedClique	sharedNode
	Synonym for sharedClique (TODO: remove) More...

Public Member Functions
Values	calculateEstimate () const

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

const Value &	calculateEstimate (Key key) const

virtual bool	equals (const ISAM2 &other, double tol=1e-9) const

const Values &	getLinearizationPoint () const
	Access the current linearization point. More...

	ISAM2 (const ISAM2Params &params)

	ISAM2 ()

Matrix	marginalCovariance (Key key) const

void	marginalizeLeaves (const FastList< Key > &leafKeys, boost::optional< FactorIndices & > marginalFactorsIndices=boost::none, boost::optional< FactorIndices & > deletedFactorsIndices=boost::none)

virtual ISAM2Result	update (const NonlinearFactorGraph &newFactors=NonlinearFactorGraph(), const Values &newTheta=Values(), const FactorIndices &removeFactorIndices=FactorIndices(), const boost::optional< FastMap< Key, int > > &constrainedKeys=boost::none, const boost::optional< FastList< Key > > &noRelinKeys=boost::none, const boost::optional< FastList< Key > > &extraReelimKeys=boost::none, bool force_relinearize=false)

virtual ISAM2Result	update (const NonlinearFactorGraph &newFactors, const Values &newTheta, const ISAM2UpdateParams &updateParams)

bool	valueExists (Key key) const
	Check whether variable with given key exists in linearization point. More...

virtual	~ISAM2 ()

Public members for non-typical usage
Values	calculateBestEstimate () const

const VectorValues &	getDelta () const

double	error (const VectorValues &x) const

const NonlinearFactorGraph &	getFactorsUnsafe () const

const VariableIndex &	getVariableIndex () const

const KeySet &	getFixedVariables () const

const ISAM2Params &	params () const

void	printStats () const

VectorValues	gradientAtZero () const

Public Member Functions inherited from gtsam::BayesTree< ISAM2Clique >
size_t	size () const

bool	empty () const

const Nodes &	nodes () const

const sharedNode	operator[] (Key j) const

const Roots &	roots () const

const sharedClique &	clique (Key j) const

BayesTreeCliqueData	getCliqueData () const

size_t	numCachedSeparatorMarginals () const

sharedConditional	marginalFactor (Key j, const Eliminate &function=EliminationTraitsType::DefaultEliminate) const

sharedFactorGraph	joint (Key j1, Key j2, const Eliminate &function=EliminationTraitsType::DefaultEliminate) const

sharedBayesNet	jointBayesNet (Key j1, Key j2, const Eliminate &function=EliminationTraitsType::DefaultEliminate) const

void	saveGraph (const std::string &s, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const

Key	findParentClique (const CONTAINER &parents) const

void	clear ()

void	deleteCachedShortcuts ()

void	removePath (sharedClique clique, BayesNetType bn, Cliques orphans)

void	removeTop (const KeyVector &keys, BayesNetType bn, Cliques orphans)

Cliques	removeSubtree (const sharedClique &subtree)

void	insertRoot (const sharedClique &subtree)

void	addClique (const sharedClique &clique, const sharedClique &parent_clique=sharedClique())

void	addFactorsToGraph (FactorGraph< FactorType > *graph) const

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

Protected Member Functions
void	addVariables (const Values &newTheta, ISAM2Result::DetailedResults *detail=0)

void	recalculate (const ISAM2UpdateParams &updateParams, const KeySet &relinKeys, ISAM2Result *result)
	Remove marked top and either recalculate in batch or incrementally. More...

void	recalculateBatch (const ISAM2UpdateParams &updateParams, KeySet affectedKeysSet, ISAM2Result result)

void	recalculateIncremental (const ISAM2UpdateParams &updateParams, const KeySet &relinKeys, const FastList< Key > &affectedKeys, KeySet affectedKeysSet, Cliques orphans, ISAM2Result *result)

GaussianFactorGraph	relinearizeAffectedFactors (const ISAM2UpdateParams &updateParams, const FastList< Key > &affectedKeys, const KeySet &relinKeys)

void	removeVariables (const KeySet &unusedKeys)

void	updateDelta (bool forceFullSolve=false) const

Protected Member Functions inherited from gtsam::BayesTree< ISAM2Clique >
This &	operator= (const This &other)

	BayesTree ()

	BayesTree (const This &other)

void	getCliqueData (sharedClique clique, BayesTreeCliqueData *stats) const

void	saveGraph (std::ostream &s, sharedClique clique, const KeyFormatter &keyFormatter, int parentnum=0) const

void	removeClique (sharedClique clique)

void	fillNodesIndex (const sharedClique &subtree)

bool	equals (const This &other, double tol=1e-9) const

Protected Attributes
VectorValues	delta_

VectorValues	deltaNewton_

KeySet	deltaReplacedMask_

boost::optional< double >	doglegDelta_

KeySet	fixedVariables_

GaussianFactorGraph	linearFactors_

NonlinearFactorGraph	nonlinearFactors_

ISAM2Params	params_

VectorValues	RgProd_

Values	theta_

int	update_count_

VariableIndex	variableIndex_

Protected Attributes inherited from gtsam::BayesTree< ISAM2Clique >
Nodes	nodes_

Roots	roots_

Additional Inherited Members
Protected Types inherited from gtsam::BayesTree< ISAM2Clique >
typedef boost::shared_ptr< This >	shared_ptr

typedef BayesTree< ISAM2Clique >	This

Detailed Description

Definition at line 45 of file ISAM2.h.

Member Typedef Documentation

using gtsam::ISAM2::Base = BayesTree<ISAM2Clique>

The BayesTree base class.

Definition at line 101 of file ISAM2.h.

using gtsam::ISAM2::Clique = Base::Clique

A clique.

Definition at line 102 of file ISAM2.h.

using gtsam::ISAM2::Cliques = Base::Cliques

List of Cliques.

Definition at line 104 of file ISAM2.h.

using gtsam::ISAM2::sharedClique = Base::sharedClique

Shared pointer to a clique.

Definition at line 103 of file ISAM2.h.

using gtsam::ISAM2::This = ISAM2

This class.

Definition at line 100 of file ISAM2.h.

Constructor & Destructor Documentation

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

explicit

Create an empty ISAM2 instance

Definition at line 40 of file ISAM2.cpp.

gtsam::ISAM2::ISAM2 ( )

Create an empty ISAM2 instance using the default set of parameters (see ISAM2Params)

Definition at line 47 of file ISAM2.cpp.

virtual gtsam::ISAM2::~ISAM2 ( )

inlinevirtual

default virtual destructor

Definition at line 114 of file ISAM2.h.

Member Function Documentation

void gtsam::ISAM2::addVariables	(	const Values &	newTheta,
		ISAM2Result::DetailedResults *	detail = `0`
	)

protected

Add new variables to the ISAM2 system.

Parameters

newTheta	Initial values for new variables
variableStatus	optional detailed result structure

Definition at line 360 of file ISAM2.cpp.

Values gtsam::ISAM2::calculateBestEstimate ( ) const

Compute an estimate using a complete delta computed by a full back-substitution.

Definition at line 778 of file ISAM2.cpp.

Values gtsam::ISAM2::calculateEstimate ( ) const

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&).

Definition at line 763 of file ISAM2.cpp.

template<class VALUE >

VALUE gtsam::ISAM2::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 224 of file ISAM2.h.

const Value & gtsam::ISAM2::calculateEstimate ( Key key ) const

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. This is a non-templated version that returns a Value base class for use with the MATLAB wrapper.

Parameters

key

Returns

Definition at line 772 of file ISAM2.cpp.

bool gtsam::ISAM2::equals	(	const ISAM2 &	other,
		double	tol = `1e-9`
	)		const

virtual

Compare equality

Definition at line 54 of file ISAM2.cpp.

double gtsam::ISAM2::error ( const VectorValues & x ) const

Compute the linear error

Definition at line 797 of file ISAM2.cpp.

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

Access the current delta, computed during the last call to update

Definition at line 791 of file ISAM2.cpp.

const NonlinearFactorGraph& gtsam::ISAM2::getFactorsUnsafe ( ) const

inline

Access the set of nonlinear factors

Definition at line 257 of file ISAM2.h.

const KeySet& gtsam::ISAM2::getFixedVariables ( ) const

inline

Access the nonlinear variable index

Definition at line 265 of file ISAM2.h.

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

inline

Access the current linearization point.

Definition at line 205 of file ISAM2.h.

const VariableIndex& gtsam::ISAM2::getVariableIndex ( ) const

inline

Access the nonlinear variable index

Definition at line 262 of file ISAM2.h.

VectorValues gtsam::ISAM2::gradientAtZero ( ) const

Compute the gradient of the energy function, $\nabla_{x=0} \left\Vert \Sigma^{-1} R x - d \right\Vert^2$ , centered around zero. The gradient about zero is $ -R^T d $ . See also gradient(const GaussianBayesNet&, const VectorValues&).

Returns: A VectorValues storing the gradient.

Definition at line 802 of file ISAM2.cpp.

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

Return marginal on any variable as a covariance matrix

Definition at line 784 of file ISAM2.cpp.

void gtsam::ISAM2::marginalizeLeaves	(	const FastList< Key > &	leafKeys,
		boost::optional< FactorIndices & >	marginalFactorsIndices = `boost::none`,
		boost::optional< FactorIndices & >	deletedFactorsIndices = `boost::none`
	)

Marginalize out variables listed in leafKeys. These keys must be leaves in the BayesTree. Throws MarginalizeNonleafException if non-leaves are requested to be marginalized. Marginalization leaves a linear approximation of the marginal in the system, and the linearization points of any variables involved in this linear marginal become fixed. The set fixed variables will include any key involved with the marginalized variables in the original factors, and possibly additional ones due to fill-in.

If provided, 'marginalFactorsIndices' will be augmented with the factor graph indices of the marginal factors added during the 'marginalizeLeaves' call

If provided, 'deletedFactorsIndices' will be augmented with the factor graph indices of any factor that was removed during the 'marginalizeLeaves' call

Definition at line 479 of file ISAM2.cpp.

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

inline

Definition at line 267 of file ISAM2.h.

void gtsam::ISAM2::printStats ( ) const

inline

prints out clique statistics

Definition at line 270 of file ISAM2.h.

void gtsam::ISAM2::recalculate	(	const ISAM2UpdateParams &	updateParams,
		const KeySet &	relinKeys,
		ISAM2Result *	result
	)

protected

Remove marked top and either recalculate in batch or incrementally.

Definition at line 113 of file ISAM2.cpp.

void gtsam::ISAM2::recalculateBatch	(	const ISAM2UpdateParams &	updateParams,
		KeySet *	affectedKeysSet,
		ISAM2Result *	result
	)

protected

Definition at line 174 of file ISAM2.cpp.

void gtsam::ISAM2::recalculateIncremental	(	const ISAM2UpdateParams &	updateParams,
		const KeySet &	relinKeys,
		const FastList< Key > &	affectedKeys,
		KeySet *	affectedKeysSet,
		Cliques *	orphans,
		ISAM2Result *	result
	)

protected

Definition at line 244 of file ISAM2.cpp.

GaussianFactorGraph gtsam::ISAM2::relinearizeAffectedFactors	(	const ISAM2UpdateParams &	updateParams,
		const FastList< Key > &	affectedKeys,
		const KeySet &	relinKeys
	)

protected

Definition at line 62 of file ISAM2.cpp.

void gtsam::ISAM2::removeVariables ( const KeySet & unusedKeys )

protected

Remove variables from the ISAM2 system.

Definition at line 380 of file ISAM2.cpp.

ISAM2Result gtsam::ISAM2::update	(	const NonlinearFactorGraph &	newFactors = `NonlinearFactorGraph()`,
		const Values &	newTheta = `Values()`,
		const FactorIndices &	removeFactorIndices = `FactorIndices()`,
		const boost::optional< FastMap< Key, int > > &	constrainedKeys = `boost::none`,
		const boost::optional< FastList< Key > > &	noRelinKeys = `boost::none`,
		const boost::optional< FastList< Key > > &	extraReelimKeys = `boost::none`,
		bool	force_relinearize = `false`
	)

virtual

Add new factors, updating the solution and relinearizing as needed.

Optionally, this function remove existing factors from the system to enable behaviors such as swapping existing factors with new ones.

Add new measurements, and optionally new variables, to the current system. This runs a full step of the ISAM2 algorithm, relinearizing and updating the solution as needed, according to the wildfire and relinearize thresholds.

Parameters

newFactors	The new factors to be added to the system
newTheta	Initialization points for new variables to be added to the system. You must include here all new variables occuring in newFactors (which were not already in the system). There must not be any variables here that do not occur in newFactors, and additionally, variables that were already in the system must not be included here.
removeFactorIndices	Indices of factors to remove from system
force_relinearize	Relinearize any variables whose delta magnitude is sufficiently large (Params::relinearizeThreshold), regardless of the relinearization interval (Params::relinearizeSkip).
constrainedKeys	is an optional map of keys to group labels, such that a variable can be constrained to a particular grouping in the BayesTree
noRelinKeys	is an optional set of nonlinear keys that iSAM2 will hold at a constant linearization point, regardless of the size of the linear delta
extraReelimKeys	is an optional set of nonlinear keys that iSAM2 will re-eliminate, regardless of the size of the linear delta. This allows the provided keys to be reordered.

Returns: An ISAM2Result struct containing information about the update

Definition at line 396 of file ISAM2.cpp.

ISAM2Result gtsam::ISAM2::update	(	const NonlinearFactorGraph &	newFactors,
		const Values &	newTheta,
		const ISAM2UpdateParams &	updateParams
	)

virtual

Add new factors, updating the solution and relinearizing as needed.

Alternative signature of update() (see its documentation above), with all additional parameters in one structure. This form makes easier to keep future API/ABI compatibility if parameters change.

Parameters

newFactors	The new factors to be added to the system
newTheta	Initialization points for new variables to be added to the system. You must include here all new variables occuring in newFactors (which were not already in the system). There must not be any variables here that do not occur in newFactors, and additionally, variables that were already in the system must not be included here.
updateParams	Additional parameters to control relinearization, constrained keys, etc.

Returns: An ISAM2Result struct containing information about the update

Note: No default parameters to avoid ambiguous call errors.

Definition at line 414 of file ISAM2.cpp.

void gtsam::ISAM2::updateDelta ( bool forceFullSolve = false ) const

protected

Definition at line 701 of file ISAM2.cpp.

bool gtsam::ISAM2::valueExists ( Key key ) const

inline

Check whether variable with given key exists in linearization point.

Definition at line 208 of file ISAM2.h.

Member Data Documentation

VectorValues gtsam::ISAM2::delta_

mutableprotected

The linear delta from the last linear solution, an update to the estimate in theta

This is mutable because it is a "cached" variable - it is not updated until either requested with getDelta() or calculateEstimate(), or needed during update() to evaluate whether to relinearize variables.

Definition at line 61 of file ISAM2.h.

VectorValues gtsam::ISAM2::deltaNewton_

mutableprotected

Definition at line 63 of file ISAM2.h.

KeySet gtsam::ISAM2::deltaReplacedMask_

mutableprotected

A cumulative mask for the variables that were replaced and have not yet been updated in the linear solution delta_, this is only used internally, delta will always be updated if necessary when requested with getDelta() or calculateEstimate().

This is mutable because it is used internally to not update delta_ until it is needed.

Definition at line 76 of file ISAM2.h.

boost::optional<double> gtsam::ISAM2::doglegDelta_

mutableprotected

The current Dogleg Delta (trust region radius)

Definition at line 90 of file ISAM2.h.

KeySet gtsam::ISAM2::fixedVariables_

protected

Set of variables that are involved with linear factors from marginalized variables and thus cannot have their linearization points changed.

Definition at line 94 of file ISAM2.h.

GaussianFactorGraph gtsam::ISAM2::linearFactors_

mutableprotected

The current linear factors, which are only updated as needed

Definition at line 84 of file ISAM2.h.

NonlinearFactorGraph gtsam::ISAM2::nonlinearFactors_

protected

All original nonlinear factors are stored here to use during relinearization

Definition at line 81 of file ISAM2.h.

ISAM2Params gtsam::ISAM2::params_

protected

The current parameters

Definition at line 87 of file ISAM2.h.

VectorValues gtsam::ISAM2::RgProd_

mutableprotected

Definition at line 65 of file ISAM2.h.

Values gtsam::ISAM2::theta_

protected

The current linearization point

Definition at line 48 of file ISAM2.h.

int gtsam::ISAM2::update_count_

protected

Counter incremented every update(), used to determine periodic relinearization

Definition at line 96 of file ISAM2.h.

VariableIndex gtsam::ISAM2::variableIndex_

protected

VariableIndex lets us look up factors by involved variable and keeps track of dimensions

Definition at line 52 of file ISAM2.h.

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

Public Types

Public Member Functions

Protected Member Functions

Protected Attributes

Additional Inherited Members

Detailed Description

Member Typedef Documentation

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation