#include <ConcurrentIncrementalFilter.h>
Classes | |
struct | Result |
Public Types | |
typedef ConcurrentFilter | Base |
typedef for base class More... | |
typedef std::shared_ptr< ConcurrentIncrementalFilter > | shared_ptr |
Public Types inherited from gtsam::ConcurrentFilter | |
typedef std::shared_ptr< ConcurrentFilter > | shared_ptr |
Public Member Functions | |
Values | calculateEstimate () const |
template<class VALUE > | |
VALUE | calculateEstimate (Key key) const |
ConcurrentIncrementalFilter (const ISAM2Params ¶meters=ISAM2Params()) | |
bool | equals (const ConcurrentFilter &rhs, double tol=1e-9) const override |
const VectorValues & | getDelta () const |
const NonlinearFactorGraph & | getFactors () const |
const ISAM2 & | getISAM2 () const |
const Values & | getLinearizationPoint () const |
void | getSmootherFactors (NonlinearFactorGraph &smootherFactors, Values &smootherValues) override |
void | getSummarizedFactors (NonlinearFactorGraph &filterSummarization, Values &filterSummarizationValues) override |
void | postsync () override |
void | presync () override |
void | print (const std::string &s="Concurrent Incremental Filter:\n", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override |
void | synchronize (const NonlinearFactorGraph &smootherSummarization, const Values &smootherSummarizationValues) override |
Result | update (const NonlinearFactorGraph &newFactors=NonlinearFactorGraph(), const Values &newTheta=Values(), const std::optional< FastList< Key > > &keysToMove={}, const std::optional< FactorIndices > &removeFactorIndices={}) |
~ConcurrentIncrementalFilter () override | |
Public Member Functions inherited from gtsam::ConcurrentFilter | |
ConcurrentFilter ()=default | |
virtual | ~ConcurrentFilter ()=default |
Protected Attributes | |
FactorIndices | currentSmootherSummarizationSlots_ |
The slots in factor graph that correspond to the current smoother summarization on the current separator. More... | |
ISAM2 | isam2_ |
The iSAM2 inference engine. More... | |
NonlinearFactorGraph | previousSmootherSummarization_ |
The smoother summarization on the old separator sent by the smoother during the last synchronization. More... | |
NonlinearFactorGraph | smootherFactors_ |
A temporary holding place for the set of full nonlinear factors being sent to the smoother. More... | |
NonlinearFactorGraph | smootherShortcut_ |
A set of conditional factors from the old separator to the current separator (recursively calculated during each filter update) More... | |
Values | smootherValues_ |
A temporary holding place for the linearization points of all keys being sent to the smoother. More... | |
Private Member Functions | |
NonlinearFactorGraph | calculateFilterSummarization () const |
void | updateShortcut (const NonlinearFactorGraph &removedFactors) |
Static Private Member Functions | |
static FactorIndices | FindAdjacentFactors (const ISAM2 &isam2, const FastList< Key > &keys, const FactorIndices &factorsToIgnore) |
static void | RecursiveMarkAffectedKeys (const Key &key, const ISAM2Clique::shared_ptr &clique, std::set< Key > &additionalKeys) |
An iSAM2-based Batch Filter that implements the Concurrent Filtering and Smoother interface.
Definition at line 30 of file ConcurrentIncrementalFilter.h.
typedef for base class
Definition at line 35 of file ConcurrentIncrementalFilter.h.
typedef std::shared_ptr<ConcurrentIncrementalFilter> gtsam::ConcurrentIncrementalFilter::shared_ptr |
Definition at line 34 of file ConcurrentIncrementalFilter.h.
|
inline |
Default constructor
Definition at line 64 of file ConcurrentIncrementalFilter.h.
|
inlineoverride |
Default destructor
Definition at line 67 of file ConcurrentIncrementalFilter.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 98 of file ConcurrentIncrementalFilter.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 108 of file ConcurrentIncrementalFilter.h.
|
private |
Calculate marginal factors on the current separator variables using just the information in the filter
Definition at line 340 of file ConcurrentIncrementalFilter.cpp.
|
overridevirtual |
Check if two Concurrent Filters are equal
Implements gtsam::ConcurrentFilter.
Definition at line 32 of file ConcurrentIncrementalFilter.cpp.
|
staticprivate |
Find the set of iSAM2 factors adjacent to 'keys'
Definition at line 288 of file ConcurrentIncrementalFilter.cpp.
|
inline |
Access the current set of deltas to the linearization point
Definition at line 91 of file ConcurrentIncrementalFilter.h.
|
inline |
Access the current set of factors
Definition at line 76 of file ConcurrentIncrementalFilter.h.
|
inline |
Access the current linearization point
Definition at line 81 of file ConcurrentIncrementalFilter.h.
|
inline |
Access the current linearization point
Definition at line 86 of file ConcurrentIncrementalFilter.h.
|
overridevirtual |
Populate the provided containers with factors being sent to the smoother from the filter. These may be original nonlinear factors, or factors encoding a summarization of the filter information. The specifics will be implementation-specific for a given filter.
smootherFactors | The new factors to be added to the smoother |
smootherValues | The linearization points of any new variables |
Implements gtsam::ConcurrentFilter.
Definition at line 243 of file ConcurrentIncrementalFilter.cpp.
|
overridevirtual |
Populate the provided containers with factors that constitute the filter branch summarization needed by the smoother. Also, linearization points for the new root clique must be provided.
summarizedFactors | The summarized factors for the filter branch |
rootValues | The linearization points of the root clique variables |
Implements gtsam::ConcurrentFilter.
Definition at line 227 of file ConcurrentIncrementalFilter.cpp.
|
overridevirtual |
Perform any required operations after the synchronization process finishes. Called by 'synchronize'
Reimplemented from gtsam::ConcurrentFilter.
Definition at line 255 of file ConcurrentIncrementalFilter.cpp.
|
overridevirtual |
Perform any required operations before the synchronization process starts. Called by 'synchronize'
Reimplemented from gtsam::ConcurrentFilter.
Definition at line 167 of file ConcurrentIncrementalFilter.cpp.
|
overridevirtual |
Implement a GTSAM standard 'print' function
Implements gtsam::ConcurrentFilter.
Definition at line 26 of file ConcurrentIncrementalFilter.cpp.
|
staticprivate |
Traverse the iSAM2 Bayes Tree, inserting all descendants of the provided index/key into 'additionalKeys'
Definition at line 268 of file ConcurrentIncrementalFilter.cpp.
|
overridevirtual |
Apply the updated version of the smoother branch summarized factors.
summarizedFactors | An updated version of the smoother branch summarized factors |
Implements gtsam::ConcurrentFilter.
Definition at line 175 of file ConcurrentIncrementalFilter.cpp.
ConcurrentIncrementalFilter::Result gtsam::ConcurrentIncrementalFilter::update | ( | const NonlinearFactorGraph & | newFactors = NonlinearFactorGraph() , |
const Values & | newTheta = Values() , |
||
const std::optional< FastList< Key > > & | keysToMove = {} , |
||
const std::optional< FactorIndices > & | removeFactorIndices = {} |
||
) |
Add new factors and variables to the filter.
Add new measurements, and optionally new variables, to the filter. This runs a full update step of the derived filter algorithm
newFactors | The new factors to be added to the smoother |
newTheta | Initialization points for new variables to be added to the filter You must include here all new variables occurring in newFactors that were not already in the filter. |
keysToMove | An optional set of keys to move from the filter to the smoother |
removeFactorIndices | An optional set of indices corresponding to the factors you want to remove from the graph |
Definition at line 45 of file ConcurrentIncrementalFilter.cpp.
|
private |
Update the shortcut marginal between the current separator keys and the previous separator keys
Definition at line 312 of file ConcurrentIncrementalFilter.cpp.
|
protected |
The slots in factor graph that correspond to the current smoother summarization on the current separator.
Definition at line 173 of file ConcurrentIncrementalFilter.h.
|
protected |
The iSAM2 inference engine.
Definition at line 169 of file ConcurrentIncrementalFilter.h.
|
protected |
The smoother summarization on the old separator sent by the smoother during the last synchronization.
Definition at line 172 of file ConcurrentIncrementalFilter.h.
|
protected |
A temporary holding place for the set of full nonlinear factors being sent to the smoother.
Definition at line 177 of file ConcurrentIncrementalFilter.h.
|
protected |
A set of conditional factors from the old separator to the current separator (recursively calculated during each filter update)
Definition at line 174 of file ConcurrentIncrementalFilter.h.
|
protected |
A temporary holding place for the linearization points of all keys being sent to the smoother.
Definition at line 178 of file ConcurrentIncrementalFilter.h.