Public Member Functions | Static Public Member Functions | Public Attributes | List of all members
gtsam::UpdateImpl Struct Reference

#include <ISAM2-impl.h>

Public Member Functions

void augmentVariableIndex (const NonlinearFactorGraph &newFactors, const FactorIndices &newFactorsIndices, VariableIndex *variableIndex) const
 
void computeUnusedKeys (const NonlinearFactorGraph &newFactors, const VariableIndex &variableIndex, const KeySet &keysWithRemovedFactors, KeySet *unusedKeys) const
 
void error (const NonlinearFactorGraph &nonlinearFactors, const Values &estimate, boost::optional< double > *result) const
 
void findFluid (const ISAM2::Roots &roots, const KeySet &relinKeys, KeySet *markedKeys, ISAM2Result::DetailedResults *detail) const
 
void gatherInvolvedKeys (const NonlinearFactorGraph &newFactors, const NonlinearFactorGraph &nonlinearFactors, const KeySet &keysWithRemovedFactors, KeySet *markedKeys) const
 
KeySet gatherRelinearizeKeys (const ISAM2::Roots &roots, const VectorValues &delta, const KeySet &fixedVariables, KeySet *markedKeys) const
 
void linearizeNewFactors (const NonlinearFactorGraph &newFactors, const Values &theta, size_t numNonlinearFactors, const FactorIndices &newFactorsIndices, GaussianFactorGraph *linearFactors) const
 
void pushBackFactors (const NonlinearFactorGraph &newFactors, NonlinearFactorGraph *nonlinearFactors, GaussianFactorGraph *linearFactors, VariableIndex *variableIndex, FactorIndices *newFactorsIndices, KeySet *keysWithRemovedFactors) const
 
void recordRelinearizeDetail (const KeySet &relinKeys, ISAM2Result::DetailedResults *detail) const
 
bool relinarizationNeeded (size_t update_count) const
 
 UpdateImpl (const ISAM2Params &params, const ISAM2UpdateParams &updateParams)
 
void updateKeys (const KeySet &markedKeys, ISAM2Result *result) const
 

Static Public Member Functions

static KeySet CheckRelinearizationFull (const VectorValues &delta, const ISAM2Params::RelinearizationThreshold &relinearizeThreshold)
 
static KeySet CheckRelinearizationPartial (const ISAM2::Roots &roots, const VectorValues &delta, const ISAM2Params::RelinearizationThreshold &relinearizeThreshold)
 
static void CheckRelinearizationRecursiveDouble (double threshold, const VectorValues &delta, const ISAM2::sharedClique &clique, KeySet *relinKeys)
 
static void CheckRelinearizationRecursiveMap (const FastMap< char, Vector > &thresholds, const VectorValues &delta, const ISAM2::sharedClique &clique, KeySet *relinKeys)
 
static void ExpmapMasked (const VectorValues &delta, const KeySet &mask, Values *theta)
 
static FactorIndexSet GetAffectedFactors (const KeyList &keys, const VariableIndex &variableIndex)
 
static GaussianFactorGraph GetCachedBoundaryFactors (const ISAM2::Cliques &orphans)
 
static void LogRecalculateKeys (const ISAM2Result &result)
 
static void LogStartingUpdate (const NonlinearFactorGraph &newFactors, const ISAM2 &isam2)
 

Public Attributes

const ISAM2Paramsparams_
 
const ISAM2UpdateParamsupdateParams_
 

Detailed Description

Implementation functions for update method All of the methods below have clear inputs and outputs, even if not functional: iSAM2 is inherintly imperative.

Definition at line 114 of file ISAM2-impl.h.

Constructor & Destructor Documentation

gtsam::UpdateImpl::UpdateImpl ( const ISAM2Params params,
const ISAM2UpdateParams updateParams 
)
inline

Definition at line 117 of file ISAM2-impl.h.

Member Function Documentation

void gtsam::UpdateImpl::augmentVariableIndex ( const NonlinearFactorGraph newFactors,
const FactorIndices newFactorsIndices,
VariableIndex variableIndex 
) const
inline

Definition at line 486 of file ISAM2-impl.h.

static KeySet gtsam::UpdateImpl::CheckRelinearizationFull ( const VectorValues delta,
const ISAM2Params::RelinearizationThreshold relinearizeThreshold 
)
inlinestatic

Find the set of variables to be relinearized according to relinearizeThreshold. Any variables in the VectorValues delta whose vector magnitude is greater than or equal to relinearizeThreshold are returned.

Parameters
deltaThe linear delta to check against the threshold
keyFormatterFormatter for printing nonlinear keys during debugging
Returns
The set of variable indices in delta whose magnitude is greater than or equal to relinearizeThreshold

Definition at line 345 of file ISAM2-impl.h.

static KeySet gtsam::UpdateImpl::CheckRelinearizationPartial ( const ISAM2::Roots roots,
const VectorValues delta,
const ISAM2Params::RelinearizationThreshold relinearizeThreshold 
)
inlinestatic

Find the set of variables to be relinearized according to relinearizeThreshold. This check is performed recursively, starting at the top of the tree. Once a variable in the tree does not need to be relinearized, no further checks in that branch are performed. This is an approximation of the Full version, designed to save time at the expense of accuracy.

Parameters
deltaThe linear delta to check against the threshold
keyFormatterFormatter for printing nonlinear keys during debugging
Returns
The set of variable indices in delta whose magnitude is greater than or equal to relinearizeThreshold

Definition at line 318 of file ISAM2-impl.h.

static void gtsam::UpdateImpl::CheckRelinearizationRecursiveDouble ( double  threshold,
const VectorValues delta,
const ISAM2::sharedClique clique,
KeySet relinKeys 
)
inlinestatic

Definition at line 284 of file ISAM2-impl.h.

static void gtsam::UpdateImpl::CheckRelinearizationRecursiveMap ( const FastMap< char, Vector > &  thresholds,
const VectorValues delta,
const ISAM2::sharedClique clique,
KeySet relinKeys 
)
inlinestatic

Definition at line 250 of file ISAM2-impl.h.

void gtsam::UpdateImpl::computeUnusedKeys ( const NonlinearFactorGraph newFactors,
const VariableIndex variableIndex,
const KeySet keysWithRemovedFactors,
KeySet unusedKeys 
) const
inline

Definition at line 180 of file ISAM2-impl.h.

void gtsam::UpdateImpl::error ( const NonlinearFactorGraph nonlinearFactors,
const Values estimate,
boost::optional< double > *  result 
) const
inline

Definition at line 197 of file ISAM2-impl.h.

static void gtsam::UpdateImpl::ExpmapMasked ( const VectorValues delta,
const KeySet mask,
Values theta 
)
inlinestatic

Apply expmap to the given values, but only for indices appearing in mask. Values are expmapped in-place.

Parameters
maskMask on linear indices, only true entries are expmapped

Definition at line 444 of file ISAM2-impl.h.

void gtsam::UpdateImpl::findFluid ( const ISAM2::Roots roots,
const KeySet relinKeys,
KeySet markedKeys,
ISAM2Result::DetailedResults detail 
) const
inline

Definition at line 417 of file ISAM2-impl.h.

void gtsam::UpdateImpl::gatherInvolvedKeys ( const NonlinearFactorGraph newFactors,
const NonlinearFactorGraph nonlinearFactors,
const KeySet keysWithRemovedFactors,
KeySet markedKeys 
) const
inline

Definition at line 204 of file ISAM2-impl.h.

KeySet gtsam::UpdateImpl::gatherRelinearizeKeys ( const ISAM2::Roots roots,
const VectorValues delta,
const KeySet fixedVariables,
KeySet markedKeys 
) const
inline

Definition at line 375 of file ISAM2-impl.h.

static FactorIndexSet gtsam::UpdateImpl::GetAffectedFactors ( const KeyList keys,
const VariableIndex variableIndex 
)
inlinestatic

Definition at line 522 of file ISAM2-impl.h.

static GaussianFactorGraph gtsam::UpdateImpl::GetCachedBoundaryFactors ( const ISAM2::Cliques orphans)
inlinestatic

Definition at line 535 of file ISAM2-impl.h.

void gtsam::UpdateImpl::linearizeNewFactors ( const NonlinearFactorGraph newFactors,
const Values theta,
size_t  numNonlinearFactors,
const FactorIndices newFactorsIndices,
GaussianFactorGraph linearFactors 
) const
inline

Definition at line 470 of file ISAM2-impl.h.

static void gtsam::UpdateImpl::LogRecalculateKeys ( const ISAM2Result result)
inlinestatic

Definition at line 505 of file ISAM2-impl.h.

static void gtsam::UpdateImpl::LogStartingUpdate ( const NonlinearFactorGraph newFactors,
const ISAM2 isam2 
)
inlinestatic

Definition at line 121 of file ISAM2-impl.h.

void gtsam::UpdateImpl::pushBackFactors ( const NonlinearFactorGraph newFactors,
NonlinearFactorGraph nonlinearFactors,
GaussianFactorGraph linearFactors,
VariableIndex variableIndex,
FactorIndices newFactorsIndices,
KeySet keysWithRemovedFactors 
) const
inline

Definition at line 146 of file ISAM2-impl.h.

void gtsam::UpdateImpl::recordRelinearizeDetail ( const KeySet relinKeys,
ISAM2Result::DetailedResults detail 
) const
inline

Definition at line 405 of file ISAM2-impl.h.

bool gtsam::UpdateImpl::relinarizationNeeded ( size_t  update_count) const
inline

Definition at line 139 of file ISAM2-impl.h.

void gtsam::UpdateImpl::updateKeys ( const KeySet markedKeys,
ISAM2Result result 
) const
inline

Definition at line 233 of file ISAM2-impl.h.

Member Data Documentation

const ISAM2Params& gtsam::UpdateImpl::params_

Definition at line 115 of file ISAM2-impl.h.

const ISAM2UpdateParams& gtsam::UpdateImpl::updateParams_

Definition at line 116 of file ISAM2-impl.h.


The documentation for this struct was generated from the following file:


gtsam
Author(s):
autogenerated on Sat May 8 2021 02:58:37