gtsam  4.0.0 gtsam
gtsam::ISAM2 Class Reference
Inheritance diagram for gtsam::ISAM2:

## Public members for non-typical usage

size_t lastAffectedVariableCount

size_t lastAffectedFactorCount

size_t lastAffectedCliqueCount

size_t lastAffectedMarkedCount

size_t lastBacksubVariableCount

size_t lastNnzTop

Values calculateBestEstimate () const
Compute an estimate using a complete delta computed by a full back-substitution.

const VectorValuesgetDelta () const
Access the current delta, computed during the last call to update.

double error (const VectorValues &x) const
Compute the linear error.

const NonlinearFactorGraphgetFactorsUnsafe () const
Access the set of nonlinear factors.

const VariableIndexgetVariableIndex () const
Access the nonlinear variable index.

const KeySetgetFixedVariables () const
Access the nonlinear variable index.

const ISAM2Paramsparams () const

void printStats () const
prints out clique statistics

Compute the gradient of the energy function, $$\nabla_{x=0} \left\Vert \Sigma^{-1} R x - d \right\Vert^2$$, centered around zero. More...

## 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 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)
Add new factors, updating the solution and relinearizing as needed. More...

void marginalizeLeaves (const FastList< Key > &leafKeys, boost::optional< FactorIndices & > marginalFactorsIndices=boost::none, boost::optional< FactorIndices & > deletedFactorsIndices=boost::none)
Marginalize out variables listed in leafKeys. More...

const ValuesgetLinearizationPoint () 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. More...

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

const ValuecalculateEstimate (Key key) const
Compute an estimate for a single variable using its incomplete linear delta computed during the last update. More...

Matrix marginalCovariance (Key key) const
Return marginal on any variable as a covariance matrix.

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

bool empty () const
Check if there are any cliques in the tree.

const Nodesnodes () const
return nodes

const sharedNode operator[] (Key j) const
Access node by variable.

const Rootsroots () const
return root cliques

const sharedCliqueclique (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. More...

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

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

void saveGraph (const std::string &s, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
Read only with side effects. More...

Key findParentClique (const CONTAINER &parents) const
Find parent clique of a conditional. More...

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

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

Cliques removeSubtree (const sharedClique &subtree)
Remove the requested subtree. More...

void insertRoot (const sharedClique &subtree)
Insert a new subtree with known parent clique. More...

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

void addFactorsToGraph (FactorGraph< FactorType > &graph) const
Add all cliques in this BayesTree to the specified factor graph.

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

## Static Public Member Functions

static void AddFactorsStep1 (const NonlinearFactorGraph &newFactors, bool useUnusedSlots, NonlinearFactorGraph *nonlinearFactors, FactorIndices *newFactorIndices)
Perform the first part of the bookkeeping updates for adding new factors. More...

static KeySet CheckRelinearizationFull (const VectorValues &delta, const ISAM2Params::RelinearizationThreshold &relinearizeThreshold)
Find the set of variables to be relinearized according to relinearizeThreshold. More...

static KeySet CheckRelinearizationPartial (const ISAM2::Roots &roots, const VectorValues &delta, const ISAM2Params::RelinearizationThreshold &relinearizeThreshold)
Find the set of variables to be relinearized according to relinearizeThreshold. More...

static size_t UpdateGaussNewtonDelta (const ISAM2::Roots &roots, const KeySet &replacedKeys, double wildfireThreshold, VectorValues *delta)
Update the Newton's method step point, using wildfire.

static size_t UpdateRgProd (const ISAM2::Roots &roots, const KeySet &replacedKeys, const VectorValues &gradAtZero, VectorValues *RgProd)
Update the RgProd (R*g) incrementally taking into account which variables have been recalculated in replacedKeys. More...

## Public Types

typedef ISAM2 This
This class.

typedef BayesTree< ISAM2CliqueBase
The BayesTree base class.

typedef Base::Clique Clique
A clique.

typedef Base::sharedClique sharedClique
Shared pointer to a clique.

typedef Base::Cliques Cliques
List of Clique typedef from base class.

Public Types inherited from gtsam::BayesTree< ISAM2Clique >
typedef ISAM2Clique Clique
The clique type, normally BayesTreeClique.

typedef boost::shared_ptr< CliquesharedClique
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 boost::shared_ptr< ConditionalType > sharedConditional

typedef ISAM2Clique ::BayesNetType BayesNetType

typedef boost::shared_ptr< BayesNetType > sharedBayesNet

typedef ISAM2Clique ::FactorType FactorType

typedef boost::shared_ptr< FactorType > sharedFactor

typedef ISAM2Clique ::FactorGraphType FactorGraphType

typedef boost::shared_ptr< FactorGraphType > sharedFactorGraph

typedef FactorGraphType::Eliminate Eliminate

typedef ISAM2Clique ::EliminationTraitsType EliminationTraitsType

typedef FastList< sharedCliqueCliques
A convenience class for a list of shared cliques.

typedef ConcurrentMap< Key, sharedCliqueNodes
Map from keys to Clique.

## Classes

struct  PartialSolveResult

struct  ReorderingMode

## Protected Member Functions

Add new variables to the ISAM2 system. More...

void removeVariables (const KeySet &unusedKeys)
Remove variables from the ISAM2 system.

Apply expmap to the given values, but only for indices appearing in mask. More...

FactorIndexSet getAffectedFactors (const FastList< Key > &keys) const

GaussianFactorGraph::shared_ptr relinearizeAffectedFactors (const FastList< Key > &affectedKeys, const KeySet &relinKeys) const

GaussianFactorGraph getCachedBoundaryFactors (const Cliques &orphans)

virtual boost::shared_ptr< KeySetrecalculate (const KeySet &markedKeys, const KeySet &relinKeys, const KeyVector &observedKeys, const KeySet &unusedIndices, const boost::optional< FastMap< Key, int > > &constrainKeys, ISAM2Result *result)

void updateDelta (bool forceFullSolve=false) const

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

BayesTree ()
Create an empty Bayes Tree.

BayesTree (const This &other)
Copy constructor.

void getCliqueData (BayesTreeCliqueData &stats, sharedClique clique) const
Gather data on a single clique.

void saveGraph (std::ostream &s, sharedClique clique, const KeyFormatter &keyFormatter, int parentnum=0) const
private helper method for saving the Tree to a text file in GraphViz format

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.

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

## 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. More...

VectorValues deltaNewton_

VectorValues RgProd_

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(). More...

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.

boost::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. More...

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

Protected Attributes inherited from gtsam::BayesTree< ISAM2Clique >
Nodes nodes_
Map from indices to Clique.

Roots roots_
Root cliques.

Protected Types inherited from gtsam::BayesTree< ISAM2Clique >
typedef BayesTree< ISAM2CliqueThis

typedef boost::shared_ptr< Thisshared_ptr

typedef FastVector< sharedCliqueRoots
Root cliques.

## Member Function Documentation

 static void gtsam::ISAM2::AddFactorsStep1 ( const NonlinearFactorGraph & newFactors, bool useUnusedSlots, NonlinearFactorGraph * nonlinearFactors, FactorIndices * newFactorIndices )
static

Perform the first part of the bookkeeping updates for adding new factors.

Adds them to the complete list of nonlinear factors, and populates the list of new factor indices, both optionally finding and reusing empty factor slots.

 void gtsam::ISAM2::addVariables ( const Values & newTheta )
protected

Add new variables to the ISAM2 system.

Parameters
 newTheta Initial values for new variables theta Current solution to be augmented with new initialization delta Current linear delta to be augmented with zeros deltaNewton RgProd keyFormatter Formatter for printing nonlinear keys during debugging

## ◆ 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]

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

## ◆ calculateEstimate() [3/3]

 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

## ◆ CheckRelinearizationFull()

 static KeySet gtsam::ISAM2::CheckRelinearizationFull ( const VectorValues & delta, const ISAM2Params::RelinearizationThreshold & relinearizeThreshold )
static

Find the set of variables to be relinearized according to relinearizeThreshold.

Any variables in the VectorValues delta whose vector magnitude is greater than or equal to relinearizeThreshold are returned.

Parameters
 delta The linear delta to check against the threshold keyFormatter Formatter for printing nonlinear keys during debugging
Returns
The set of variable indices in delta whose magnitude is greater than or equal to relinearizeThreshold

## ◆ CheckRelinearizationPartial()

 static KeySet gtsam::ISAM2::CheckRelinearizationPartial ( const ISAM2::Roots & roots, const VectorValues & delta, const ISAM2Params::RelinearizationThreshold & relinearizeThreshold )
static

Find the set of variables to be relinearized according to relinearizeThreshold.

This check is performed recursively, starting at the top of the tree. Once a variable in the tree does not need to be relinearized, no further checks in that branch are performed. This is an approximation of the Full version, designed to save time at the expense of accuracy.

Parameters
 delta The linear delta to check against the threshold keyFormatter Formatter for printing nonlinear keys during debugging
Returns
The set of variable indices in delta whose magnitude is greater than or equal to relinearizeThreshold

 static VectorValues gtsam::ISAM2::ComputeGradientSearch ( const VectorValues & gradAtZero, const VectorValues & RgProd )
static

Only used in Dogleg.

protected

Apply expmap to the given values, but only for indices appearing in mask.

Values are expmapped in-place.

Parameters
 mask Mask on linear indices, only true entries are expmapped

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

## ◆ marginalizeLeaves()

 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

## ◆ update()

 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

## ◆ UpdateRgProd()

 static size_t gtsam::ISAM2::UpdateRgProd ( const ISAM2::Roots & roots, const KeySet & replacedKeys, const VectorValues & gradAtZero, VectorValues * RgProd )
static

Update the RgProd (R*g) incrementally taking into account which variables have been recalculated in replacedKeys.

Only used in Dogleg.

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

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.

## ◆ fixedVariables_

 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.

The documentation for this class was generated from the following files:
• /Users/dellaert/git/gtsam/gtsam/nonlinear/ISAM2-impl.h
• /Users/dellaert/git/gtsam/gtsam/nonlinear/ISAM2.h
• /Users/dellaert/git/gtsam/gtsam/nonlinear/ISAM2.cpp