Public Types | Public Member Functions | Protected Member Functions | Protected Attributes | List of all members
gtsam::ISAM2 Class Reference

#include <ISAM2.h>

Inheritance diagram for gtsam::ISAM2:
Inheritance graph
[legend]

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< sharedCliqueCliques
 
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, sharedCliqueNodes
 
typedef FastVector< sharedCliqueRoots
 
typedef std::shared_ptr< BayesNetTypesharedBayesNet
 
typedef std::shared_ptr< CliquesharedClique
 Shared pointer to a clique. More...
 
typedef std::shared_ptr< ConditionalTypesharedConditional
 
typedef std::shared_ptr< FactorTypesharedFactor
 
typedef std::shared_ptr< FactorGraphTypesharedFactorGraph
 
typedef sharedClique sharedNode
 Synonym for sharedClique (TODO: remove) More...
 

Public Member Functions

Values calculateEstimate () const
 
template<class VALUE >
VALUE calculateEstimate (Key key) const
 
const ValuecalculateEstimate (Key key) const
 
virtual bool equals (const ISAM2 &other, double tol=1e-9) const
 
const ValuesgetLinearizationPoint () const
 Access the current linearization point. More...
 
 ISAM2 ()
 
 ISAM2 (const ISAM2Params &params)
 
Matrix marginalCovariance (Key key) const
 
void marginalizeLeaves (const FastList< Key > &leafKeys, FactorIndices *marginalFactorsIndices=nullptr, FactorIndices *deletedFactorsIndices=nullptr)
 
template<class... OptArgs>
void marginalizeLeaves (const FastList< Key > &leafKeys, OptArgs &&... optArgs)
 
virtual ISAM2Result update (const NonlinearFactorGraph &newFactors, const Values &newTheta, const ISAM2UpdateParams &updateParams)
 
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)
 
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 VectorValuesgetDelta () const
 
double error (const VectorValues &x) const
 
const NonlinearFactorGraphgetFactorsUnsafe () const
 
const VariableIndexgetVariableIndex () const
 
const KeySetgetFixedVariables () const
 
const ISAM2Paramsparams () const
 
void printStats () const
 
VectorValues gradientAtZero () const
 
- Public Member Functions inherited from gtsam::BayesTree< ISAM2Clique >
size_t size () const
 
bool empty () const
 
const Nodesnodes () const
 
sharedClique operator[] (Key j) const
 
const Rootsroots () const
 
const sharedCliqueclique (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 dot (std::ostream &os, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
 Output to graphviz format, stream version. More...
 
std::string dot (const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
 Output to graphviz format string. More...
 
void saveGraph (const std::string &filename, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
 output to file with graphviz format. More...
 
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)
 Perform an incremental update of the factor graph to return a new Bayes Tree with affected keys. More...
 
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 >
Thisoperator= (const This &other)
 
 ~BayesTree ()
 
 BayesTree ()
 
 BayesTree (const This &other)
 
void getCliqueData (sharedClique clique, BayesTreeCliqueData *stats) const
 
void dot (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_
 
std::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 std::shared_ptr< Thisshared_ptr
 
typedef BayesTree< ISAM2CliqueThis
 

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.

Definition at line 45 of file ISAM2.h.

Member Typedef Documentation

◆ Base

The BayesTree base class.

Definition at line 101 of file ISAM2.h.

◆ Clique

A clique.

Definition at line 102 of file ISAM2.h.

◆ Cliques

List of Cliques.

Definition at line 104 of file ISAM2.h.

◆ sharedClique

Shared pointer to a clique.

Definition at line 103 of file ISAM2.h.

◆ This

This class.

Definition at line 100 of file ISAM2.h.

Constructor & Destructor Documentation

◆ ISAM2() [1/2]

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

Create an empty ISAM2 instance

Definition at line 42 of file ISAM2.cpp.

◆ ISAM2() [2/2]

gtsam::ISAM2::ISAM2 ( )

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

Definition at line 50 of file ISAM2.cpp.

◆ ~ISAM2()

virtual gtsam::ISAM2::~ISAM2 ( )
inlinevirtual

default virtual destructor

Definition at line 114 of file ISAM2.h.

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

Definition at line 365 of file ISAM2.cpp.

◆ calculateBestEstimate()

Values gtsam::ISAM2::calculateBestEstimate ( ) const

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

Definition at line 801 of file ISAM2.cpp.

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

Definition at line 786 of file ISAM2.cpp.

◆ 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

Definition at line 236 of file ISAM2.h.

◆ 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

Definition at line 795 of file ISAM2.cpp.

◆ equals()

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

Compare equality

Definition at line 58 of file ISAM2.cpp.

◆ error()

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

Compute the linear error

Definition at line 820 of file ISAM2.cpp.

◆ getDelta()

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

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

Definition at line 814 of file ISAM2.cpp.

◆ getFactorsUnsafe()

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

Access the set of nonlinear factors

Definition at line 269 of file ISAM2.h.

◆ getFixedVariables()

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

Access the nonlinear variable index

Definition at line 277 of file ISAM2.h.

◆ getLinearizationPoint()

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

Access the current linearization point.

Definition at line 217 of file ISAM2.h.

◆ getVariableIndex()

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

Access the nonlinear variable index

Definition at line 274 of file ISAM2.h.

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

Definition at line 825 of file ISAM2.cpp.

◆ marginalCovariance()

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

Return marginal on any variable as a covariance matrix

Definition at line 807 of file ISAM2.cpp.

◆ marginalizeLeaves() [1/2]

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

Definition at line 487 of file ISAM2.cpp.

◆ marginalizeLeaves() [2/2]

template<class... OptArgs>
void gtsam::ISAM2::marginalizeLeaves ( const FastList< Key > &  leafKeys,
OptArgs &&...  optArgs 
)
inline

An overload of marginalizeLeaves that takes references to vectors instead of pointers to vectors and passes it to the pointer version of the function.

Definition at line 209 of file ISAM2.h.

◆ params()

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

Definition at line 279 of file ISAM2.h.

◆ printStats()

void gtsam::ISAM2::printStats ( ) const
inline

prints out clique statistics

Definition at line 282 of file ISAM2.h.

◆ recalculate()

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 117 of file ISAM2.cpp.

◆ recalculateBatch()

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

Definition at line 178 of file ISAM2.cpp.

◆ 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
updateParamsParameters for the ISAM2 update.
relinKeysKeys of variables to relinearize.
affectedKeysThe 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.

Definition at line 250 of file ISAM2.cpp.

◆ relinearizeAffectedFactors()

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

Definition at line 66 of file ISAM2.cpp.

◆ removeVariables()

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

Remove variables from the ISAM2 system.

Definition at line 385 of file ISAM2.cpp.

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

Definition at line 419 of file ISAM2.cpp.

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

Definition at line 401 of file ISAM2.cpp.

◆ updateDelta()

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

Definition at line 725 of file ISAM2.cpp.

◆ valueExists()

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

Check whether variable with given key exists in linearization point.

Definition at line 220 of file ISAM2.h.

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.

Definition at line 61 of file ISAM2.h.

◆ deltaNewton_

VectorValues gtsam::ISAM2::deltaNewton_
mutableprotected

Definition at line 63 of file ISAM2.h.

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

Definition at line 76 of file ISAM2.h.

◆ doglegDelta_

std::optional<double> gtsam::ISAM2::doglegDelta_
mutableprotected

The current Dogleg Delta (trust region radius)

Definition at line 90 of file ISAM2.h.

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

Definition at line 94 of file ISAM2.h.

◆ linearFactors_

GaussianFactorGraph gtsam::ISAM2::linearFactors_
mutableprotected

The current linear factors, which are only updated as needed

Definition at line 84 of file ISAM2.h.

◆ nonlinearFactors_

NonlinearFactorGraph gtsam::ISAM2::nonlinearFactors_
protected

All original nonlinear factors are stored here to use during relinearization

Definition at line 81 of file ISAM2.h.

◆ params_

ISAM2Params gtsam::ISAM2::params_
protected

The current parameters

Definition at line 87 of file ISAM2.h.

◆ RgProd_

VectorValues gtsam::ISAM2::RgProd_
mutableprotected

Definition at line 65 of file ISAM2.h.

◆ theta_

Values gtsam::ISAM2::theta_
protected

The current linearization point

Definition at line 48 of file ISAM2.h.

◆ update_count_

int gtsam::ISAM2::update_count_
protected

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

Definition at line 96 of file ISAM2.h.

◆ variableIndex_

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:


gtsam
Author(s):
autogenerated on Sun Dec 22 2024 04:23:55