Loading...
Searching...
No Matches
gtsam::ISAM2 Class Reference
ISAM2

## 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 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.
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, boost::optional< FactorIndices & > marginalFactorsIndices=boost::none, boost::optional< FactorIndices & > deletedFactorsIndices=boost::none)
| Marginalize out variables listed in leafKeys.
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.
const Value & | calculateEstimate (Key key) const
| Compute an estimate for a single variable using its incomplete linear delta computed during the last update.
Matrix | marginalCovariance (Key key) const
| Return marginal on any variable as a covariance matrix.
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
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.
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 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
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 | 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.
void | print (const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
| print

## 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 boost::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 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< 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 >
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, int 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.
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.
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.
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.
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.

## Friends

class | boost::serialization::access
| Serialization function.

## Additional Inherited Members

Protected Types inherited from gtsam::BayesTree< ISAM2Clique >
typedef BayesTree< ISAM2Clique > | This
typedef boost::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]

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

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

Returns
  A VectorValues storing the gradient.

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

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

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

## Member Data Documentation

## delta_

VectorValues gtsam::ISAM2::delta_

mutable protected

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_

mutable protected

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:

- /build/gtsam-RkN8wG/gtsam-4.2.2+dfsg/gtsam/nonlinear/ISAM2.h
- /build/gtsam-RkN8wG/gtsam-4.2.2+dfsg/gtsam/nonlinear/ISAM2.cpp