|
Values | calculateEstimate () const |
|
template<class VALUE > |
VALUE | calculateEstimate (Key key) const |
|
const Value & | calculateEstimate (Key key) const |
|
virtual bool | equals (const ISAM2 &other, double tol=1e-9) const |
|
const Values & | getLinearizationPoint () const |
| Access the current linearization point. More...
|
|
| ISAM2 () |
|
| ISAM2 (const ISAM2Params ¶ms) |
|
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 () |
|
|
Values | calculateBestEstimate () const |
|
const VectorValues & | getDelta () const |
|
double | error (const VectorValues &x) const |
|
const NonlinearFactorGraph & | getFactorsUnsafe () const |
|
const VariableIndex & | getVariableIndex () const |
|
const KeySet & | getFixedVariables () const |
|
const ISAM2Params & | params () const |
|
void | printStats () const |
|
VectorValues | gradientAtZero () const |
|
size_t | size () const |
|
bool | empty () const |
|
const Nodes & | nodes () const |
|
sharedClique | operator[] (Key j) const |
|
const Roots & | roots () const |
|
const sharedClique & | clique (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 |
|
|
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 |
|
This & | operator= (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 |
|
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.
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 486 of file ISAM2.cpp.
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
Definition at line 400 of file ISAM2.cpp.