gtsam  4.1.0
gtsam
gtsam::ISAM2 Class Reference
+ 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. More...
 
virtual ISAM2Result update (const NonlinearFactorGraph &newFactors, const Values &newTheta, const ISAM2UpdateParams &updateParams)
 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 members for non-typical usage
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
 
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. More...
 
- 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.
 
void insertRoot (const sharedClique &subtree)
 Insert a new subtree with known parent clique. More...
 
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< CliquesharedClique
 Shared pointer to a clique.
 
typedef Clique Node
 Synonym for Clique (TODO: remove)
 
typedef sharedClique sharedNode
 Synonym for sharedClique (TODO: remove)
 
typedef CLIQUE::ConditionalType ConditionalType
 
typedef boost::shared_ptr< ConditionalType > sharedConditional
 
typedef CLIQUE::BayesNetType BayesNetType
 
typedef boost::shared_ptr< BayesNetType > sharedBayesNet
 
typedef CLIQUE::FactorType FactorType
 
typedef boost::shared_ptr< FactorType > sharedFactor
 
typedef CLIQUE::FactorGraphType FactorGraphType
 
typedef boost::shared_ptr< FactorGraphType > sharedFactorGraph
 
typedef FactorGraphType::Eliminate Eliminate
 
typedef CLIQUE::EliminationTraitsType EliminationTraitsType
 
typedef FastList< sharedCliqueCliques
 A convenience class for a list of shared cliques.
 
typedef ConcurrentMap< Key, sharedCliqueNodes
 Map from keys to Clique.
 
typedef FastVector< sharedCliqueRoots
 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)
 
void addVariables (const Values &newTheta, ISAM2Result::DetailedResults *detail=0)
 Add new variables to the ISAM2 system. More...
 
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 >
Thisoperator= (const This &other)
 Assignment operator.
 
 BayesTree ()
 Create an empty Bayes Tree.
 
 BayesTree (const This &other)
 Copy constructor.
 
void getCliqueData (sharedClique clique, BayesTreeCliqueData *stats) 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_
 
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(). 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.
 
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.
 

Additional Inherited Members

- Protected Types inherited from gtsam::BayesTree< ISAM2Clique >
typedef BayesTree< ISAM2CliqueThis
 
typedef boost::shared_ptr< Thisshared_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
newThetaInitial values for new variables
variableStatusoptional 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]

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

◆ 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

◆ 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
newFactorsThe new factors to be added to the system
newThetaInitialization 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.
updateParamsAdditional 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
newFactorsThe new factors to be added to the system
newThetaInitialization 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.
removeFactorIndicesIndices of factors to remove from system
force_relinearizeRelinearize any variables whose delta magnitude is sufficiently large (Params::relinearizeThreshold), regardless of the relinearization interval (Params::relinearizeSkip).
constrainedKeysis an optional map of keys to group labels, such that a variable can be constrained to a particular grouping in the BayesTree
noRelinKeysis an optional set of nonlinear keys that iSAM2 will hold at a constant linearization point, regardless of the size of the linear delta
extraReelimKeysis 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: