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.
|
| ISAM2 (const ISAM2Params ¶ms) |
| 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.
|
|
|
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.
|
|
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
|
|
|
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.
|
|
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 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< 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.
|
|
|
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 |
|
This & | operator= (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 | 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 | 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
|
|