gtsam  4.1.0
gtsam
gtsam::BatchFixedLagSmoother Class Reference
+ Inheritance diagram for gtsam::BatchFixedLagSmoother:

Public Member Functions

 BatchFixedLagSmoother (double smootherLag=0.0, const LevenbergMarquardtParams &parameters=LevenbergMarquardtParams(), bool enforceConsistency=true)
 default constructor
 
virtual ~BatchFixedLagSmoother ()
 destructor
 
void print (const std::string &s="BatchFixedLagSmoother:\n", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
 Print the factor for debugging and testing (implementing Testable)
 
bool equals (const FixedLagSmoother &rhs, double tol=1e-9) const override
 Check if two IncrementalFixedLagSmoother Objects are equal.
 
Result update (const NonlinearFactorGraph &newFactors=NonlinearFactorGraph(), const Values &newTheta=Values(), const KeyTimestampMap &timestamps=KeyTimestampMap(), const FactorIndices &factorsToRemove=FactorIndices()) override
 Add new factors, updating the solution and relinearizing as needed.
 
Values calculateEstimate () const override
 Compute an estimate from the incomplete linear delta computed during the last update. More...
 
template<class VALUE >
VALUE calculateEstimate (Key key) const
 Compute an estimate for a single variable using its incomplete linear delta computed during the last update. More...
 
const LevenbergMarquardtParamsparams () const
 read the current set of optimizer parameters
 
LevenbergMarquardtParamsparams ()
 update the current set of optimizer parameters
 
const NonlinearFactorGraphgetFactors () const
 Access the current set of factors.
 
const ValuesgetLinearizationPoint () const
 Access the current linearization point.
 
const OrderinggetOrdering () const
 Access the current ordering.
 
const VectorValuesgetDelta () const
 Access the current set of deltas to the linearization point.
 
Matrix marginalCovariance (Key key) const
 Calculate marginal covariance on given variable.
 
- Public Member Functions inherited from gtsam::FixedLagSmoother
 FixedLagSmoother (double smootherLag=0.0)
 default constructor
 
virtual ~FixedLagSmoother ()
 destructor
 
double smootherLag () const
 read the current smoother lag
 
double & smootherLag ()
 write to the current smoother lag
 
const KeyTimestampMaptimestamps () const
 Access the current set of timestamps associated with each variable.
 

Static Public Member Functions

static GaussianFactorGraph CalculateMarginalFactors (const GaussianFactorGraph &graph, const KeyVector &keys, const GaussianFactorGraph::Eliminate &eliminateFunction=EliminatePreferCholesky)
 Marginalize specific keys from a linear graph. More...
 
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.
 

Public Types

typedef boost::shared_ptr< BatchFixedLagSmoothershared_ptr
 Typedef for a shared pointer to an Incremental Fixed-Lag Smoother.
 
- Public Types inherited from gtsam::FixedLagSmoother
typedef boost::shared_ptr< FixedLagSmoothershared_ptr
 Typedef for a shared pointer to an Incremental Fixed-Lag Smoother.
 
typedef std::map< Key, double > KeyTimestampMap
 Typedef for a Key-Timestamp map/database.
 
typedef std::multimap< double, KeyTimestampKeyMap
 

Protected Types

typedef std::map< Key, std::set< Key > > FactorIndex
 A typedef defining an Key-Factor mapping.
 

Protected Member Functions

void insertFactors (const NonlinearFactorGraph &newFactors)
 Augment the list of factors with a set of new factors.
 
void removeFactors (const std::set< size_t > &deleteFactors)
 Remove factors from the list of factors by slot index.
 
void eraseKeys (const KeyVector &keys)
 Erase any keys associated with timestamps before the provided time.
 
void reorder (const KeyVector &marginalizeKeys=KeyVector())
 Use colamd to update into an efficient ordering.
 
Result optimize ()
 Optimize the current graph using a modified version of L-M.
 
void marginalize (const KeyVector &marginalizableKeys)
 Marginalize out selected variables.
 
- Protected Member Functions inherited from gtsam::FixedLagSmoother
void updateKeyTimestampMap (const KeyTimestampMap &newTimestamps)
 Update the Timestamps associated with the keys.
 
void eraseKeyTimestampMap (const KeyVector &keys)
 Erase keys from the Key-Timestamps database.
 
double getCurrentTimestamp () const
 Find the most recent timestamp of the system.
 
KeyVector findKeysBefore (double timestamp) const
 Find all of the keys associated with timestamps before the provided time.
 
KeyVector findKeysAfter (double timestamp) const
 Find all of the keys associated with timestamps before the provided time.
 

Protected Attributes

LevenbergMarquardtParams parameters_
 The L-M optimization parameters.
 
bool enforceConsistency_
 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. More...
 
NonlinearFactorGraph factors_
 The nonlinear factors.
 
Values theta_
 The current linearization point.
 
Values linearKeys_
 The set of keys involved in current linear factors. More...
 
Ordering ordering_
 The current ordering.
 
VectorValues delta_
 The current set of linear deltas.
 
std::queue< size_t > availableSlots_
 The set of available factor graph slots. More...
 
FactorIndex factorIndex_
 A cross-reference structure to allow efficient factor lookups by key.
 
- Protected Attributes inherited from gtsam::FixedLagSmoother
double smootherLag_
 The length of the smoother lag. More...
 
TimestampKeyMap timestampKeyMap_
 The current timestamp associated with each tracked key.
 
KeyTimestampMap keyTimestampMap_
 

Member Function Documentation

◆ calculateEstimate() [1/2]

Values gtsam::BatchFixedLagSmoother::calculateEstimate ( ) const
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.

◆ calculateEstimate() [2/2]

template<class VALUE >
VALUE gtsam::BatchFixedLagSmoother::calculateEstimate ( Key  key) const
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.

Parameters
key
Returns

◆ CalculateMarginalFactors()

GaussianFactorGraph gtsam::BatchFixedLagSmoother::CalculateMarginalFactors ( const GaussianFactorGraph graph,
const KeyVector keys,
const GaussianFactorGraph::Eliminate eliminateFunction = EliminatePreferCholesky 
)
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

Member Data Documentation

◆ availableSlots_

std::queue<size_t> gtsam::BatchFixedLagSmoother::availableSlots_
protected

The set of available factor graph slots.

These occur because we are constantly deleting factors, leaving holes.

◆ enforceConsistency_

bool gtsam::BatchFixedLagSmoother::enforceConsistency_
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

◆ linearKeys_

Values gtsam::BatchFixedLagSmoother::linearKeys_
protected

The set of keys involved in current linear factors.

These keys should not be relinearized.


The documentation for this class was generated from the following files: