Detailed Description

Implementation of the full ISAM2 algorithm for incremental nonlinear optimization.

The typical cycle of using this class to create an instance by providing ISAM2Params to the constructor, then add measurements and variables as they arrive using the update() method. At any time, calculateEstimate() may be called to obtain the current estimate of all variables.

Inheritance diagram for gtsam::ISAM2:

Public Member Functions
	ISAM2 (const ISAM2Params &params)
	Create an empty ISAM2 instance.
	ISAM2 ()
	Create an empty ISAM2 instance using the default set of parameters (see ISAM2Params).
virtual	~ISAM2 ()
	default virtual destructor
virtual bool	equals (const ISAM2 &other, double tol=1e-9) const
	Compare equality.
virtual ISAM2Result	update (const NonlinearFactorGraph &newFactors=NonlinearFactorGraph(), const Values &newTheta=Values(), const FactorIndices &removeFactorIndices=FactorIndices(), const std::optional< FastMap< Key, int > > &constrainedKeys={}, const std::optional< FastList< Key > > &noRelinKeys={}, const std::optional< FastList< Key > > &extraReelimKeys={}, bool force_relinearize=false)
	Add new factors, updating the solution and relinearizing as needed.
virtual ISAM2Result	update (const NonlinearFactorGraph &newFactors, const Values &newTheta, const ISAM2UpdateParams &updateParams)
	Add new factors, updating the solution and relinearizing as needed.
void	marginalizeLeaves (const FastList< Key > &leafKeys, FactorIndices marginalFactorsIndices=nullptr, FactorIndices deletedFactorsIndices=nullptr)
	Marginalize out variables listed in leafKeys.
template<class... OptArgs>
void	marginalizeLeaves (const FastList< Key > &leafKeys, OptArgs &&... optArgs)
	An overload of marginalizeLeaves that takes references to vectors instead of pointers to vectors and passes it to the pointer version of the function.
std::pair< FactorIndices, FactorIndices >	marginalizeLeavesWithIndices (const FastList< Key > &leafKeys)
	An added function specifically to return the marginalFactorIndices and deletedFactorIndices.
const Values &	getLinearizationPoint () const
	Access the current linearization point.
bool	valueExists (Key key) const
	Check whether variable with given key exists in linearization point.
Values	calculateEstimate () const
	Compute an estimate from the incomplete linear delta computed during the last update.
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.
Values	calculateEstimate (const KeyVector &keys) const
	Compute estimates for a set of variables from the incomplete linear delta computed during the last update, as a Values holding only those keys, whatever their types.
Matrix	marginalInformation (Key key) const
	Return the marginal information matrix on any variable.
Matrix	marginalCovariance (Key key) const
	Return the marginal covariance matrix on any variable.
JointMarginal	jointMarginalCovariance (const KeyVector &queryKeys) const
	Return joint marginal covariance with blocks in queryKeys order.
JointMarginal	jointMarginalInformation (const KeyVector &queryKeys) const
	Return joint marginal information with blocks in queryKeys order.
Public members for non-typical usage
Values	calculateBestEstimate () const
	Compute an estimate using a complete delta computed by a full back-substitution.
const VectorValues &	getDelta () const
	Access the current delta, computed during the last call to update.
double	error (const VectorValues &x) const
	Compute the linear error.
const NonlinearFactorGraph &	getFactorsUnsafe () const
	Access the set of nonlinear factors.
const VariableIndex &	getVariableIndex () const
	Access the nonlinear variable index.
const KeySet &	getFixedVariables () const
	Access the nonlinear variable index.
const ISAM2Params &	params () const
size_t	treeNnz () const
	Compute the total number of nonzeros by traversing the entire Bayes tree.
void	printStats () const
	prints out clique statistics
VectorValues	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.
std::pair< KeySet, bool >	predictUpdateInfo (const NonlinearFactorGraph &newFactors, const Values &newTheta, const ISAM2UpdateParams &updateParams) const
	Predicts the updated variables for a hypothetical update.
Public Member Functions inherited from gtsam::BayesTree< ISAM2Clique >
bool	equals (const This &other, double tol=1e-9) const
	check equality
void	print (const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
	print
size_t	size () const
	number of cliques
bool	empty () const
	Check if there are any cliques in the tree.
const Nodes &	nodes () const
	Return nodes.
sharedClique	operator[] (Key j) const
	Access node by variable.
const Roots &	roots () const
	return root cliques
const sharedClique &	clique (Key j) const
	alternate syntax for matlab: find the clique that contains the variable with Key j
BayesTreeCliqueData	getCliqueData () const
	Gather data on all cliques.
size_t	numCachedSeparatorMarginals () const
	Collect number of cliques with cached separator marginals.
sharedConditional	marginalFactor (Key j, const Eliminate &function=EliminationTraitsType::DefaultEliminate) const
	Return marginal on any variable.
sharedFactorGraph	joint (Key j1, Key j2, const Eliminate &function=EliminationTraitsType::DefaultEliminate) const
	return joint on two variables Limitation: can only calculate joint if cliques are disjoint or one of them is root
sharedFactorGraph	joint (const KeyVector &keys, const Eliminate &function=EliminationTraitsType::DefaultEliminate) const
	Return a joint factor graph on an arbitrary set of variables.
sharedBayesNet	jointBayesNet (Key j1, Key j2, const Eliminate &function=EliminationTraitsType::DefaultEliminate) const
	return joint on two variables as a BayesNet Limitation: can only calculate joint if cliques are disjoint or one of them is root
sharedBayesNet	jointBayesNet (const KeyVector &keys, const Eliminate &function=EliminationTraitsType::DefaultEliminate) const
	Return a joint marginal Bayes net whose elimination order follows the first occurrence of each key.
void	dot (std::ostream &os, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
	Output to graphviz format, stream version.
std::string	dot (const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
	Output to graphviz format string.
void	saveGraph (const std::string &filename, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
	output to file with graphviz format.
Key	findParentClique (const CONTAINER &parents) const
	Find parent clique of a conditional.
void	clear ()
	Remove all nodes.
void	deleteCachedShortcuts ()
	Clear all shortcut caches - use before timing on marginal calculation to avoid residual cache data.
void	removePath (sharedClique clique, BayesNetType bn, Cliques orphans)
	Remove path from clique to root and return that path as factors plus a list of orphaned subtree roots.
void	removeTop (const KeyVector &keys, BayesNetType bn, Cliques orphans)
	Given a list of indices, turn "contaminated" part of the tree back into a factor graph.
Cliques	removeSubtree (const sharedClique &subtree)
	Remove the requested subtree.
void	insertRoot (const sharedClique &subtree)
	Insert a new subtree with known parent clique.
void	addClique (const sharedClique &clique, const sharedClique &parent_clique=sharedClique())
	add a clique (top down)
void	addFactorsToGraph (FactorGraph< FactorType > *graph) const
	Add all cliques in this BayesTree to the specified factor graph.
gtsam::KeySet	collectAffectedKeys (const gtsam::KeyVector &keys) const
	Returns the set of keys from the tree that are affected by a update to 'keys'.

Public Types
using	This = ISAM2
	This class.
using	Base = BayesTree<ISAM2Clique>
	The BayesTree base class.
using	Clique = Base::Clique
	A clique.
using	sharedClique = Base::sharedClique
	Shared pointer to a clique.
using	Cliques = Base::Cliques
	List of Cliques.
Public Types inherited from gtsam::BayesTree< ISAM2Clique >
typedef ISAM2Clique	Clique
	The clique type, normally BayesTreeClique.
typedef std::shared_ptr< Clique >	sharedClique
	Shared pointer to a clique.
typedef Clique	Node
	Synonym for Clique (TODO: remove).
typedef sharedClique	sharedNode
	Synonym for sharedClique (TODO: remove).
typedef ISAM2Clique::ConditionalType	ConditionalType
typedef std::shared_ptr< ConditionalType >	sharedConditional
typedef ISAM2Clique::BayesNetType	BayesNetType
typedef std::shared_ptr< BayesNetType >	sharedBayesNet
typedef ISAM2Clique::FactorType	FactorType
typedef std::shared_ptr< FactorType >	sharedFactor
typedef ISAM2Clique::FactorGraphType	FactorGraphType
typedef std::shared_ptr< FactorGraphType >	sharedFactorGraph
typedef FactorGraphType::Eliminate	Eliminate
typedef ISAM2Clique::EliminationTraitsType	EliminationTraitsType
typedef FastList< sharedClique >	Cliques
	A convenience class for a list of shared cliques.
typedef ConcurrentMap< Key, sharedClique >	Nodes
	Map from keys to Clique.
typedef FastVector< sharedClique >	Roots
	Root cliques.

Protected Member Functions
void	recalculate (const ISAM2UpdateParams &updateParams, const KeySet &relinKeys, ISAM2Result *result)
	Remove marked top and either recalculate in batch or incrementally.
void	recalculateBatch (const ISAM2UpdateParams &updateParams, KeySet affectedKeysSet, ISAM2Result result)
GaussianFactorGraph	relinearizeAffectedFactors (const ISAM2UpdateParams &updateParams, const FastList< Key > &affectedKeys, const KeySet &relinKeys)
void	recalculateIncremental (const ISAM2UpdateParams &updateParams, const KeySet &relinKeys, const FastList< Key > &affectedKeys, KeySet affectedKeysSet, Cliques orphans, ISAM2Result *result)
	Perform an incremental update of the factor graph to return a new Bayes Tree with affected keys.
void	addVariables (const Values &newTheta, ISAM2Result::DetailedResults *detail=0)
	Add new variables to the ISAM2 system.
void	removeVariables (const KeySet &unusedKeys)
	Remove variables from the ISAM2 system.
void	updateDelta (bool forceFullSolve=false) const
Protected Member Functions inherited from gtsam::BayesTree< ISAM2Clique >
	~BayesTree ()
	Destructor.
This &	operator= (const This &other)
	Assignment operator.
	BayesTree ()
	Create an empty Bayes Tree.
	BayesTree (const This &other)
	Copy constructor.
void	dot (std::ostream &s, sharedClique clique, const KeyFormatter &keyFormatter, size_t parentnum=0) const
	private helper method for saving the Tree to a text file in GraphViz format
void	getCliqueData (sharedClique clique, BayesTreeCliqueData *stats) const
	Gather data on a single clique.
void	removeClique (sharedClique clique)
	remove a clique: warning, can result in a forest
void	fillNodesIndex (const sharedClique &subtree)
	Fill the nodes index for a subtree.
void	collectAffectedPathKeys (gtsam::KeySet &traversedKeys, const sharedClique &clique) const
	Helper for collectAffectedKeys that recursively aggregates affected keys from a path from 'clique' to the root of tree.

Protected Attributes
Values	theta_
	The current linearization point.
VariableIndex	variableIndex_
	VariableIndex lets us look up factors by involved variable and keeps track of dimensions.
VectorValues	delta_
	The linear delta from the last linear solution, an update to the estimate in theta.
VectorValues	deltaNewton_
VectorValues	RgProd_
KeySet	deltaReplacedMask_
	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().
NonlinearFactorGraph	nonlinearFactors_
	All original nonlinear factors are stored here to use during relinearization.
GaussianFactorGraph	linearFactors_
	The current linear factors, which are only updated as needed.
ISAM2Params	params_
	The current parameters.
std::optional< double >	doglegDelta_
	The current Dogleg Delta (trust region radius).
KeySet	fixedVariables_
	Set of variables that are involved with linear factors from marginalized variables and thus cannot have their linearization points changed.
int	update_count_
	Counter incremented every update(), used to determine periodic relinearization.
size_t	nnzAfterLastReorder_
	Bayes tree nnz recorded after the most recent full batch reorder.
Protected Attributes inherited from gtsam::BayesTree< ISAM2Clique >
Nodes	nodes_
	Map from indices to Clique.
Roots	roots_
	Root cliques.

Friends
class	IncrementalFixedLagSmoother

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

Member Function Documentation

◆ addVariables()

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

◆ calculateEstimate() [1/3]

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

◆ calculateEstimate() [2/3]

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

Compute estimates for a set of variables from the incomplete linear delta computed during the last update, as a Values holding only those keys, whatever their types.

Costs one retract per requested key, unlike the no-argument calculateEstimate(), which retracts every variable.

Parameters

keys	The keys to estimate; must be unique.

Exceptions

ValuesKeyDoesNotExist if a key is not in the linearization point.

◆ calculateEstimate() [3/3]

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

◆ gradientAtZero()

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

Components associated with hard constraints are undefined and are represented as zero in the returned gradient.

Returns: A VectorValues storing the gradient.

◆ marginalizeLeaves()

void gtsam::ISAM2::marginalizeLeaves	(	const FastList< Key > &	leafKeys,
		FactorIndices *	marginalFactorsIndices = nullptr,
		FactorIndices *	deletedFactorsIndices = nullptr )

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

◆ marginalizeLeavesWithIndices()

std::pair< FactorIndices, FactorIndices > gtsam::ISAM2::marginalizeLeavesWithIndices ( const FastList< Key > & leafKeys )

An added function specifically to return the marginalFactorIndices and deletedFactorIndices.

Made for the python wrapping of marginalizeLeaves

◆ predictUpdateInfo()

std::pair< KeySet, bool > gtsam::ISAM2::predictUpdateInfo	(	const NonlinearFactorGraph &	newFactors,
		const Values &	newTheta,
		const ISAM2UpdateParams &	updateParams ) const

Predicts the updated variables for a hypothetical update.

Parameters

newFactors	The factors for the hypothetical update
newTheta	The estimates for new variables in the hypothetical update
updateParams	The update params for the hypothetical update

Returns: The set of all affected keys, and a flag indicating if this would be a batch update

NOTE: Update may mutate the mutable field delta_

◆ recalculateIncremental()

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

protected

Perform an incremental update of the factor graph to return a new Bayes Tree with affected keys.

Parameters

updateParams	Parameters for the ISAM2 update.
relinKeys	Keys of variables to relinearize.
affectedKeys	The set of keys which are affected in the update.
affectedKeysSet	[output] Affected and contaminated keys.
orphans	[output] List of orphanes cliques after elimination.
result	[output] The result of the incremental update step.

◆ treeNnz()

size_t gtsam::ISAM2::treeNnz ( ) const

Compute the total number of nonzeros by traversing the entire Bayes tree.

Available regardless of whether update() computes ISAM2Result::treeNnz.

◆ update() [1/2]

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.

◆ update() [2/2]

ISAM2Result gtsam::ISAM2::update	(	const NonlinearFactorGraph &	newFactors = NonlinearFactorGraph(),
		const Values &	newTheta = Values(),
		const FactorIndices &	removeFactorIndices = FactorIndices(),
		const std::optional< FastMap< Key, int > > &	constrainedKeys = {},
		const std::optional< FastList< Key > > &	noRelinKeys = {},
		const std::optional< FastList< Key > > &	extraReelimKeys = {},
		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

Member Data Documentation

◆ delta_

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.

◆ deltaReplacedMask_

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.

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

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

Detailed Description

Public Member Functions

Public Types

Protected Member Functions

Protected Attributes

Friends

Additional Inherited Members

Member Function Documentation

◆ addVariables()

◆ calculateEstimate() [1/3]

◆ calculateEstimate() [2/3]

◆ calculateEstimate() [3/3]

◆ gradientAtZero()

◆ marginalizeLeaves()

◆ marginalizeLeavesWithIndices()

◆ predictUpdateInfo()

◆ recalculateIncremental()

◆ treeNnz()

◆ update() [1/2]

◆ update() [2/2]

Member Data Documentation

◆ delta_

◆ deltaReplacedMask_