#include <BatchFixedLagSmoother.h>
Public Types | |
typedef std::shared_ptr< BatchFixedLagSmoother > | shared_ptr |
Typedef for a shared pointer to an Incremental Fixed-Lag Smoother. More... | |
Public Types inherited from gtsam::FixedLagSmoother | |
typedef std::map< Key, double > | KeyTimestampMap |
Typedef for a Key-Timestamp map/database. More... | |
typedef std::shared_ptr< FixedLagSmoother > | shared_ptr |
Typedef for a shared pointer to an Incremental Fixed-Lag Smoother. More... | |
typedef std::multimap< double, Key > | TimestampKeyMap |
Public Member Functions | |
BatchFixedLagSmoother (double smootherLag=0.0, const LevenbergMarquardtParams ¶meters=LevenbergMarquardtParams(), bool enforceConsistency=true) | |
Values | calculateEstimate () const override |
template<class VALUE > | |
VALUE | calculateEstimate (Key key) const |
bool | equals (const FixedLagSmoother &rhs, double tol=1e-9) const override |
const VectorValues & | getDelta () const |
const NonlinearFactorGraph & | getFactors () const |
const Values & | getLinearizationPoint () const |
const Ordering & | getOrdering () const |
Matrix | marginalCovariance (Key key) const |
Calculate marginal covariance on given variable. More... | |
LevenbergMarquardtParams & | params () |
const LevenbergMarquardtParams & | params () const |
void | print (const std::string &s="BatchFixedLagSmoother:\n", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override |
Result | update (const NonlinearFactorGraph &newFactors=NonlinearFactorGraph(), const Values &newTheta=Values(), const KeyTimestampMap ×tamps=KeyTimestampMap(), const FactorIndices &factorsToRemove=FactorIndices()) override |
~BatchFixedLagSmoother () override | |
Public Member Functions inherited from gtsam::FixedLagSmoother | |
FixedLagSmoother (double smootherLag=0.0) | |
double & | smootherLag () |
double | smootherLag () const |
const KeyTimestampMap & | timestamps () const |
virtual | ~FixedLagSmoother () |
Static Public Member Functions | |
static GaussianFactorGraph | CalculateMarginalFactors (const GaussianFactorGraph &graph, const KeyVector &keys, const GaussianFactorGraph::Eliminate &eliminateFunction=EliminatePreferCholesky) |
static NonlinearFactorGraph | CalculateMarginalFactors (const NonlinearFactorGraph &graph, const Values &theta, const KeyVector &keys, const GaussianFactorGraph::Eliminate &eliminateFunction=EliminatePreferCholesky) |
Marginalize specific keys from a nonlinear graph, wrap in LinearContainers. More... | |
Protected Types | |
typedef std::map< Key, std::set< Key > > | FactorIndex |
Protected Member Functions | |
void | eraseKeys (const KeyVector &keys) |
void | insertFactors (const NonlinearFactorGraph &newFactors) |
void | marginalize (const KeyVector &marginalizableKeys) |
Result | optimize () |
void | removeFactors (const std::set< size_t > &deleteFactors) |
void | reorder (const KeyVector &marginalizeKeys=KeyVector()) |
Protected Member Functions inherited from gtsam::FixedLagSmoother | |
void | eraseKeyTimestampMap (const KeyVector &keys) |
KeyVector | findKeysAfter (double timestamp) const |
KeyVector | findKeysBefore (double timestamp) const |
double | getCurrentTimestamp () const |
void | updateKeyTimestampMap (const KeyTimestampMap &newTimestamps) |
Protected Attributes | |
std::queue< size_t > | availableSlots_ |
VectorValues | delta_ |
bool | enforceConsistency_ |
FactorIndex | factorIndex_ |
NonlinearFactorGraph | factors_ |
Values | linearValues_ |
Ordering | ordering_ |
LevenbergMarquardtParams | parameters_ |
Values | theta_ |
Protected Attributes inherited from gtsam::FixedLagSmoother | |
KeyTimestampMap | keyTimestampMap_ |
double | smootherLag_ |
TimestampKeyMap | timestampKeyMap_ |
Static Private Member Functions | |
static void | PrintKeySet (const gtsam::KeySet &keys, const std::string &label) |
static void | PrintKeySet (const std::set< Key > &keys, const std::string &label) |
static void | PrintSymbolicFactor (const GaussianFactor::shared_ptr &factor) |
static void | PrintSymbolicFactor (const NonlinearFactor::shared_ptr &factor) |
static void | PrintSymbolicGraph (const GaussianFactorGraph &graph, const std::string &label) |
static void | PrintSymbolicGraph (const NonlinearFactorGraph &graph, const std::string &label) |
Definition at line 29 of file nonlinear/BatchFixedLagSmoother.h.
|
protected |
A typedef defining an Key-Factor mapping
Definition at line 123 of file nonlinear/BatchFixedLagSmoother.h.
typedef std::shared_ptr<BatchFixedLagSmoother> gtsam::BatchFixedLagSmoother::shared_ptr |
Typedef for a shared pointer to an Incremental Fixed-Lag Smoother.
Definition at line 34 of file nonlinear/BatchFixedLagSmoother.h.
|
inline |
default constructor
Definition at line 37 of file nonlinear/BatchFixedLagSmoother.h.
|
inlineoverride |
destructor
Definition at line 41 of file nonlinear/BatchFixedLagSmoother.h.
|
inlineoverridevirtual |
Compute an estimate from the incomplete linear delta computed during the last update. This delta is incomplete because it was not updated below wildfire_threshold. If only a single variable is needed, it is faster to call calculateEstimate(const KEY&).
Implements gtsam::FixedLagSmoother.
Definition at line 59 of file nonlinear/BatchFixedLagSmoother.h.
|
inline |
Compute an estimate for a single variable using its incomplete linear delta computed during the last update. This is faster than calling the no-argument version of calculateEstimate, which operates on all variables.
key |
Definition at line 70 of file nonlinear/BatchFixedLagSmoother.h.
|
static |
Marginalize specific keys from a linear graph. Does not check whether keys actually exist in graph. In that case will fail somewhere deep within elimination
Definition at line 410 of file BatchFixedLagSmoother.cpp.
|
static |
Marginalize specific keys from a nonlinear graph, wrap in LinearContainers.
Definition at line 423 of file BatchFixedLagSmoother.cpp.
|
overridevirtual |
Check if two IncrementalFixedLagSmoother Objects are equal
Reimplemented from gtsam::FixedLagSmoother.
Definition at line 38 of file BatchFixedLagSmoother.cpp.
|
protected |
Erase any keys associated with timestamps before the provided time
Definition at line 154 of file BatchFixedLagSmoother.cpp.
|
inline |
Access the current set of deltas to the linearization point
Definition at line 101 of file nonlinear/BatchFixedLagSmoother.h.
|
inline |
Access the current set of factors
Definition at line 86 of file nonlinear/BatchFixedLagSmoother.h.
|
inline |
Access the current linearization point
Definition at line 91 of file nonlinear/BatchFixedLagSmoother.h.
|
inline |
Access the current ordering
Definition at line 96 of file nonlinear/BatchFixedLagSmoother.h.
|
protected |
Augment the list of factors with a set of new factors
Definition at line 112 of file BatchFixedLagSmoother.cpp.
Calculate marginal covariance on given variable.
Definition at line 47 of file BatchFixedLagSmoother.cpp.
|
protected |
Marginalize out selected variables
Definition at line 310 of file BatchFixedLagSmoother.cpp.
|
protected |
Optimize the current graph using a modified version of L-M
Definition at line 185 of file BatchFixedLagSmoother.cpp.
|
inline |
update the current set of optimizer parameters
Definition at line 81 of file nonlinear/BatchFixedLagSmoother.h.
|
inline |
read the current set of optimizer parameters
Definition at line 76 of file nonlinear/BatchFixedLagSmoother.h.
|
overridevirtual |
Print the factor for debugging and testing (implementing Testable)
Reimplemented from gtsam::FixedLagSmoother.
Definition at line 31 of file BatchFixedLagSmoother.cpp.
|
staticprivate |
Definition at line 358 of file BatchFixedLagSmoother.cpp.
|
staticprivate |
Private methods for printing debug information
Definition at line 348 of file BatchFixedLagSmoother.cpp.
|
staticprivate |
|
staticprivate |
Definition at line 368 of file BatchFixedLagSmoother.cpp.
|
staticprivate |
Definition at line 401 of file BatchFixedLagSmoother.cpp.
|
staticprivate |
Definition at line 392 of file BatchFixedLagSmoother.cpp.
|
protected |
Remove factors from the list of factors by slot index
Definition at line 133 of file BatchFixedLagSmoother.cpp.
|
protected |
Use colamd to update into an efficient ordering
Definition at line 179 of file BatchFixedLagSmoother.cpp.
|
overridevirtual |
Add new factors, updating the solution and relinearizing as needed.
Implements gtsam::FixedLagSmoother.
Definition at line 53 of file BatchFixedLagSmoother.cpp.
|
protected |
The set of available factor graph slots. These occur because we are constantly deleting factors, leaving holes.
Definition at line 149 of file nonlinear/BatchFixedLagSmoother.h.
|
protected |
The current set of linear deltas
Definition at line 146 of file nonlinear/BatchFixedLagSmoother.h.
|
protected |
A flag indicating if the optimizer should enforce probabilistic consistency by maintaining the linearization point of all variables involved in linearized/marginal factors at the edge of the smoothing window. This idea is from ??? TODO: Look up paper reference
Definition at line 131 of file nonlinear/BatchFixedLagSmoother.h.
|
protected |
A cross-reference structure to allow efficient factor lookups by key
Definition at line 152 of file nonlinear/BatchFixedLagSmoother.h.
|
protected |
The nonlinear factors
Definition at line 134 of file nonlinear/BatchFixedLagSmoother.h.
|
protected |
The set of values involved in current linear factors.
Definition at line 140 of file nonlinear/BatchFixedLagSmoother.h.
|
protected |
The current ordering
Definition at line 143 of file nonlinear/BatchFixedLagSmoother.h.
|
protected |
The L-M optimization parameters
Definition at line 126 of file nonlinear/BatchFixedLagSmoother.h.
|
protected |
The current linearization point
Definition at line 137 of file nonlinear/BatchFixedLagSmoother.h.