#include <ConcurrentBatchFilter.h>
Classes | |
struct | Result |
Public Types | |
typedef ConcurrentFilter | Base |
typedef for base class More... | |
typedef std::shared_ptr< ConcurrentBatchFilter > | 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 |
ConcurrentBatchFilter (const LevenbergMarquardtParams ¶meters=LevenbergMarquardtParams()) | |
bool | equals (const ConcurrentFilter &rhs, double tol=1e-9) const override |
const VectorValues & | getDelta () const |
const NonlinearFactorGraph & | getFactors () const |
const Values & | getLinearizationPoint () const |
const Ordering & | getOrdering () 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 Batch Filter:\n", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override |
void | synchronize (const NonlinearFactorGraph &smootherSummarization, const Values &smootherSummarizationValues) override |
virtual Result | update (const NonlinearFactorGraph &newFactors=NonlinearFactorGraph(), const Values &newTheta=Values(), const std::optional< FastList< Key > > &keysToMove={}, const std::optional< std::vector< size_t > > &removeFactorIndices={}) |
~ConcurrentBatchFilter () override=default | |
Public Member Functions inherited from gtsam::ConcurrentFilter | |
ConcurrentFilter ()=default | |
virtual | ~ConcurrentFilter ()=default |
Protected Attributes | |
std::queue< size_t > | availableSlots_ |
The set of available factor graph slots caused by deleting factors. More... | |
VectorValues | delta_ |
The current set of linear deltas from the linearization point. More... | |
NonlinearFactorGraph | factors_ |
The set of all factors currently in the filter. More... | |
NonlinearFactorGraph | filterSummarization_ |
A temporary holding place for calculated filter summarization factors to be sent to the smoother. More... | |
Ordering | ordering_ |
The current ordering used to calculate the linear deltas. More... | |
LevenbergMarquardtParams | parameters_ |
LM parameters. More... | |
std::vector< size_t > | separatorSummarizationSlots_ |
The slots in factor graph that correspond to the current smoother summarization on the current separator. More... | |
Values | separatorValues_ |
The linearization points of the separator variables. These should not be updated during optimization. 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... | |
NonlinearFactorGraph | smootherSummarization_ |
The smoother summarization on the old separator sent by the smoother during the last synchronization. More... | |
Values | smootherValues_ |
A temporary holding place for the linearization points of all keys being sent to the smoother. More... | |
Values | theta_ |
Current linearization point of all variables in the filter. More... | |
Private Member Functions | |
std::vector< size_t > | insertFactors (const NonlinearFactorGraph &factors) |
void | moveSeparator (const FastList< Key > &keysToMove) |
void | removeFactors (const std::vector< size_t > &slots) |
void | reorder (const std::optional< FastList< Key > > &keysToMove={}) |
Static Private Member Functions | |
static void | optimize (const NonlinearFactorGraph &factors, Values &theta, const Ordering &ordering, VectorValues &delta, const Values &linearValues, const LevenbergMarquardtParams ¶meters, Result &result) |
template<class Container > | |
static void | PrintKeys (const Container &keys, const std::string &indent, const std::string &title, const KeyFormatter &keyFormatter=DefaultKeyFormatter) |
Implementation of PrintKeys. More... | |
static void | PrintLinearFactor (const GaussianFactor::shared_ptr &factor, const std::string &indent="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) |
static void | PrintLinearFactorGraph (const GaussianFactorGraph &factors, const std::string &indent="", const std::string &title="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) |
static void | PrintNonlinearFactor (const NonlinearFactor::shared_ptr &factor, const std::string &indent="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) |
static void | PrintNonlinearFactorGraph (const NonlinearFactorGraph &factors, const std::string &indent="", const std::string &title="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) |
static void | PrintNonlinearFactorGraph (const NonlinearFactorGraph &factors, const std::vector< size_t > &slots, const std::string &indent="", const std::string &title="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) |
A Levenberg-Marquardt Batch Filter that implements the Concurrent Filtering and Smoother interface.
Definition at line 31 of file ConcurrentBatchFilter.h.
typedef for base class
Definition at line 35 of file ConcurrentBatchFilter.h.
typedef std::shared_ptr<ConcurrentBatchFilter> gtsam::ConcurrentBatchFilter::shared_ptr |
Definition at line 34 of file ConcurrentBatchFilter.h.
|
inline |
Default constructor
Definition at line 64 of file ConcurrentBatchFilter.h.
|
overridedefault |
Default destructor
|
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 ConcurrentBatchFilter.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 ConcurrentBatchFilter.h.
|
overridevirtual |
Check if two Concurrent Filters are equal
Implements gtsam::ConcurrentFilter.
Definition at line 105 of file ConcurrentBatchFilter.cpp.
|
inline |
Access the current set of deltas to the linearization point
Definition at line 91 of file ConcurrentBatchFilter.h.
|
inline |
Access the current set of factors
Definition at line 76 of file ConcurrentBatchFilter.h.
|
inline |
Access the current linearization point
Definition at line 81 of file ConcurrentBatchFilter.h.
|
inline |
Access the current ordering
Definition at line 86 of file ConcurrentBatchFilter.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 288 of file ConcurrentBatchFilter.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 276 of file ConcurrentBatchFilter.cpp.
|
private |
Augment the graph with new factors
factors | The factors to add to the graph |
Definition at line 312 of file ConcurrentBatchFilter.cpp.
Marginalize out the set of requested variables from the filter, caching them for the smoother This effectively moves the separator.
keysToMove | The set of keys to move from the filter to the smoother |
Definition at line 510 of file ConcurrentBatchFilter.cpp.
|
staticprivate |
Use a modified version of L-M to update the linearization point and delta
Definition at line 370 of file ConcurrentBatchFilter.cpp.
|
overridevirtual |
Perform any required operations after the synchronization process finishes. Called by 'synchronize'
Reimplemented from gtsam::ConcurrentFilter.
Definition at line 300 of file ConcurrentBatchFilter.cpp.
|
overridevirtual |
Perform any required operations before the synchronization process starts. Called by 'synchronize'
Reimplemented from gtsam::ConcurrentFilter.
Definition at line 192 of file ConcurrentBatchFilter.cpp.
|
overridevirtual |
Implement a GTSAM standard 'print' function
Implements gtsam::ConcurrentFilter.
Definition at line 96 of file ConcurrentBatchFilter.cpp.
|
staticprivate |
Implementation of PrintKeys.
Print just the nonlinear keys contained inside a container
Definition at line 245 of file ConcurrentBatchFilter.h.
|
staticprivate |
Print just the nonlinear keys in a linear factor
Definition at line 64 of file ConcurrentBatchFilter.cpp.
|
staticprivate |
Print just the nonlinear keys in each linear factor for a whole Gaussian Factor Graph
Definition at line 87 of file ConcurrentBatchFilter.cpp.
|
staticprivate |
Print just the nonlinear keys in a nonlinear factor
Definition at line 27 of file ConcurrentBatchFilter.cpp.
|
staticprivate |
Print just the nonlinear keys in each factor for a whole Nonlinear Factor Graph
Definition at line 46 of file ConcurrentBatchFilter.cpp.
|
staticprivate |
Print just the nonlinear keys of specific factors in a Nonlinear Factor Graph
Definition at line 55 of file ConcurrentBatchFilter.cpp.
|
private |
Remove factors from the graph by slot index
slots | The slots in the factor graph that should be deleted |
Definition at line 340 of file ConcurrentBatchFilter.cpp.
|
private |
Use colamd to update into an efficient ordering
Definition at line 358 of file ConcurrentBatchFilter.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 200 of file ConcurrentBatchFilter.cpp.
|
virtual |
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 121 of file ConcurrentBatchFilter.cpp.
|
protected |
The set of available factor graph slots caused by deleting factors.
Definition at line 174 of file ConcurrentBatchFilter.h.
|
protected |
The current set of linear deltas from the linearization point.
Definition at line 173 of file ConcurrentBatchFilter.h.
|
protected |
The set of all factors currently in the filter.
Definition at line 170 of file ConcurrentBatchFilter.h.
|
protected |
A temporary holding place for calculated filter summarization factors to be sent to the smoother.
Definition at line 183 of file ConcurrentBatchFilter.h.
|
protected |
The current ordering used to calculate the linear deltas.
Definition at line 172 of file ConcurrentBatchFilter.h.
|
protected |
LM parameters.
Definition at line 169 of file ConcurrentBatchFilter.h.
|
protected |
The slots in factor graph that correspond to the current smoother summarization on the current separator.
Definition at line 176 of file ConcurrentBatchFilter.h.
|
protected |
The linearization points of the separator variables. These should not be updated during optimization.
Definition at line 175 of file ConcurrentBatchFilter.h.
|
protected |
A temporary holding place for the set of full nonlinear factors being sent to the smoother.
Definition at line 184 of file ConcurrentBatchFilter.h.
|
protected |
A set of conditional factors from the old separator to the current separator (recursively calculated during each filter update)
Definition at line 180 of file ConcurrentBatchFilter.h.
|
protected |
The smoother summarization on the old separator sent by the smoother during the last synchronization.
Definition at line 179 of file ConcurrentBatchFilter.h.
|
protected |
A temporary holding place for the linearization points of all keys being sent to the smoother.
Definition at line 185 of file ConcurrentBatchFilter.h.
|
protected |
Current linearization point of all variables in the filter.
Definition at line 171 of file ConcurrentBatchFilter.h.