#include <ConcurrentIncrementalSmoother.h>
Classes | |
struct | Result |
Public Types | |
typedef ConcurrentSmoother | Base |
typedef for base class More... | |
typedef std::shared_ptr< ConcurrentIncrementalSmoother > | shared_ptr |
Public Types inherited from gtsam::ConcurrentSmoother | |
typedef std::shared_ptr< ConcurrentSmoother > | shared_ptr |
Public Member Functions | |
Values | calculateEstimate () const |
template<class VALUE > | |
VALUE | calculateEstimate (Key key) const |
ConcurrentIncrementalSmoother (const ISAM2Params ¶meters=ISAM2Params()) | |
bool | equals (const ConcurrentSmoother &rhs, double tol=1e-9) const override |
const VectorValues & | getDelta () const |
const NonlinearFactorGraph & | getFactors () const |
const Values & | getLinearizationPoint () const |
void | getSummarizedFactors (NonlinearFactorGraph &summarizedFactors, Values &separatorValues) override |
void | postsync () override |
void | presync () override |
void | print (const std::string &s="Concurrent Incremental Smoother:\n", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override |
void | synchronize (const NonlinearFactorGraph &smootherFactors, const Values &smootherValues, const NonlinearFactorGraph &summarizedFactors, const Values &separatorValues) override |
Result | update (const NonlinearFactorGraph &newFactors=NonlinearFactorGraph(), const Values &newTheta=Values(), const std::optional< FactorIndices > &removeFactorIndices={}) |
~ConcurrentIncrementalSmoother () override | |
Public Member Functions inherited from gtsam::ConcurrentSmoother | |
ConcurrentSmoother () | |
virtual | ~ConcurrentSmoother ()=default |
Protected Attributes | |
NonlinearFactorGraph | filterSummarizationFactors_ |
New filter summarization factors to replace the existing filter summarization during the next update. More... | |
FactorIndices | filterSummarizationSlots_ |
The slots in factor graph that correspond to the current filter summarization factors. More... | |
ISAM2 | isam2_ |
iSAM2 inference engine More... | |
Values | separatorValues_ |
The linearization points of the separator variables. These should not be changed during optimization. More... | |
NonlinearFactorGraph | smootherFactors_ |
New factors to be added to the smoother during the next update. More... | |
NonlinearFactorGraph | smootherSummarization_ |
A temporary holding place for calculated smoother summarization. More... | |
Values | smootherValues_ |
New variables to be added to the smoother during the next update. More... | |
bool | synchronizationUpdatesAvailable_ |
Flag indicating the currently stored synchronization updates have not been applied yet. More... | |
Private Member Functions | |
void | updateSmootherSummarization () |
A Levenberg-Marquardt Batch Smoother that implements the Concurrent Filtering and Smoother interface.
Definition at line 30 of file ConcurrentIncrementalSmoother.h.
typedef for base class
Definition at line 34 of file ConcurrentIncrementalSmoother.h.
typedef std::shared_ptr<ConcurrentIncrementalSmoother> gtsam::ConcurrentIncrementalSmoother::shared_ptr |
Definition at line 33 of file ConcurrentIncrementalSmoother.h.
|
inline |
Default constructor
Definition at line 54 of file ConcurrentIncrementalSmoother.h.
|
inlineoverride |
Default destructor
Definition at line 57 of file ConcurrentIncrementalSmoother.h.
|
inline |
Compute the current best estimate of all variables and return a full Values structure. If only a single variable is needed, it may be faster to call calculateEstimate(const KEY&).
Definition at line 83 of file ConcurrentIncrementalSmoother.h.
|
inline |
Compute the current best estimate of a single variable. This is generally faster than calling the no-argument version of calculateEstimate if only specific variables are needed.
key |
Definition at line 93 of file ConcurrentIncrementalSmoother.h.
|
overridevirtual |
Check if two Concurrent Smoothers are equal
Implements gtsam::ConcurrentSmoother.
Definition at line 33 of file ConcurrentIncrementalSmoother.cpp.
|
inline |
Access the current set of deltas to the linearization point
Definition at line 76 of file ConcurrentIncrementalSmoother.h.
|
inline |
Access the current set of factors
Definition at line 66 of file ConcurrentIncrementalSmoother.h.
|
inline |
Access the current linearization point
Definition at line 71 of file ConcurrentIncrementalSmoother.h.
|
overridevirtual |
Populate the provided containers with factors that constitute the smoother branch summarization needed by the filter.
summarizedFactors | The summarized factors for the filter branch |
Implements gtsam::ConcurrentSmoother.
Definition at line 140 of file ConcurrentIncrementalSmoother.cpp.
|
overridevirtual |
Perform any required operations after the synchronization process finishes. Called by 'synchronize'
Reimplemented from gtsam::ConcurrentSmoother.
Definition at line 174 of file ConcurrentIncrementalSmoother.cpp.
|
overridevirtual |
Perform any required operations before the synchronization process starts. Called by 'synchronize'
Reimplemented from gtsam::ConcurrentSmoother.
Definition at line 132 of file ConcurrentIncrementalSmoother.cpp.
|
overridevirtual |
Implement a GTSAM standard 'print' function
Implements gtsam::ConcurrentSmoother.
Definition at line 27 of file ConcurrentIncrementalSmoother.cpp.
|
overridevirtual |
Apply the new smoother factors sent by the filter, and the updated version of the filter branch summarized factors.
smootherFactors | A set of new factors added to the smoother from the filter |
smootherValues | Linearization points for any new variables |
summarizedFactors | An updated version of the filter branch summarized factors |
rootValues | The linearization point of the root variables |
Implements gtsam::ConcurrentSmoother.
Definition at line 154 of file ConcurrentIncrementalSmoother.cpp.
ConcurrentIncrementalSmoother::Result gtsam::ConcurrentIncrementalSmoother::update | ( | const NonlinearFactorGraph & | newFactors = NonlinearFactorGraph() , |
const Values & | newTheta = Values() , |
||
const std::optional< FactorIndices > & | removeFactorIndices = {} |
||
) |
Add new factors and variables to the smoother.
Add new measurements, and optionally new variables, to the smoother. This runs a full step of the ISAM2 algorithm, relinearizing and updating the solution as needed, according to the wildfire and relinearize thresholds.
newFactors | The new factors to be added to the smoother |
newTheta | Initialization points for new variables to be added to the smoother You must include here all new variables occuring in newFactors (which were not already in the smoother). 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. |
Definition at line 47 of file ConcurrentIncrementalSmoother.cpp.
|
private |
Calculate the smoother marginal factors on the separator variables
Definition at line 182 of file ConcurrentIncrementalSmoother.cpp.
|
protected |
New filter summarization factors to replace the existing filter summarization during the next update.
Definition at line 153 of file ConcurrentIncrementalSmoother.h.
|
protected |
The slots in factor graph that correspond to the current filter summarization factors.
Definition at line 155 of file ConcurrentIncrementalSmoother.h.
|
protected |
iSAM2 inference engine
Definition at line 148 of file ConcurrentIncrementalSmoother.h.
|
protected |
The linearization points of the separator variables. These should not be changed during optimization.
Definition at line 154 of file ConcurrentIncrementalSmoother.h.
|
protected |
New factors to be added to the smoother during the next update.
Definition at line 151 of file ConcurrentIncrementalSmoother.h.
|
protected |
A temporary holding place for calculated smoother summarization.
Definition at line 159 of file ConcurrentIncrementalSmoother.h.
|
protected |
New variables to be added to the smoother during the next update.
Definition at line 152 of file ConcurrentIncrementalSmoother.h.
|
protected |
Flag indicating the currently stored synchronization updates have not been applied yet.
Definition at line 156 of file ConcurrentIncrementalSmoother.h.