Public Types | Public Member Functions | Static Public Member Functions | Protected Types | Protected Member Functions | Protected Attributes | Static Private Member Functions | List of all members
gtsam::BatchFixedLagSmoother Class Reference

#include <BatchFixedLagSmoother.h>

Inheritance diagram for gtsam::BatchFixedLagSmoother:
Inheritance graph
[legend]

Public Types

typedef std::shared_ptr< BatchFixedLagSmoothershared_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< FixedLagSmoothershared_ptr
 Typedef for a shared pointer to an Incremental Fixed-Lag Smoother. More...
 
typedef std::multimap< double, KeyTimestampKeyMap
 

Public Member Functions

 BatchFixedLagSmoother (double smootherLag=0.0, const LevenbergMarquardtParams &parameters=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 VectorValuesgetDelta () const
 
const NonlinearFactorGraphgetFactors () const
 
const ValuesgetLinearizationPoint () const
 
const OrderinggetOrdering () const
 
Matrix marginalCovariance (Key key) const
 Calculate marginal covariance on given variable. More...
 
const LevenbergMarquardtParamsparams () const
 
LevenbergMarquardtParamsparams ()
 
void print (const std::string &s="BatchFixedLagSmoother:\, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
 
Result update (const NonlinearFactorGraph &newFactors=NonlinearFactorGraph(), const Values &newTheta=Values(), const KeyTimestampMap &timestamps=KeyTimestampMap(), const FactorIndices &factorsToRemove=FactorIndices()) override
 
 ~BatchFixedLagSmoother () override
 
- Public Member Functions inherited from gtsam::FixedLagSmoother
 FixedLagSmoother (double smootherLag=0.0)
 
double smootherLag () const
 
double & smootherLag ()
 
const KeyTimestampMaptimestamps () 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_tavailableSlots_
 
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 std::set< Key > &keys, const std::string &label)
 
static void PrintKeySet (const gtsam::KeySet &keys, const std::string &label)
 
static void PrintSymbolicFactor (const NonlinearFactor::shared_ptr &factor)
 
static void PrintSymbolicFactor (const GaussianFactor::shared_ptr &factor)
 
static void PrintSymbolicGraph (const NonlinearFactorGraph &graph, const std::string &label)
 
static void PrintSymbolicGraph (const GaussianFactorGraph &graph, const std::string &label)
 

Detailed Description

Definition at line 29 of file nonlinear/BatchFixedLagSmoother.h.

Member Typedef Documentation

◆ FactorIndex

A typedef defining an Key-Factor mapping

Definition at line 123 of file nonlinear/BatchFixedLagSmoother.h.

◆ shared_ptr

Typedef for a shared pointer to an Incremental Fixed-Lag Smoother.

Definition at line 34 of file nonlinear/BatchFixedLagSmoother.h.

Constructor & Destructor Documentation

◆ BatchFixedLagSmoother()

gtsam::BatchFixedLagSmoother::BatchFixedLagSmoother ( double  smootherLag = 0.0,
const LevenbergMarquardtParams parameters = LevenbergMarquardtParams(),
bool  enforceConsistency = true 
)
inline

default constructor

Definition at line 37 of file nonlinear/BatchFixedLagSmoother.h.

◆ ~BatchFixedLagSmoother()

gtsam::BatchFixedLagSmoother::~BatchFixedLagSmoother ( )
inlineoverride

destructor

Definition at line 41 of file nonlinear/BatchFixedLagSmoother.h.

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.

Definition at line 59 of file nonlinear/BatchFixedLagSmoother.h.

◆ 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

Definition at line 70 of file nonlinear/BatchFixedLagSmoother.h.

◆ CalculateMarginalFactors() [1/2]

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

Definition at line 410 of file BatchFixedLagSmoother.cpp.

◆ CalculateMarginalFactors() [2/2]

NonlinearFactorGraph gtsam::BatchFixedLagSmoother::CalculateMarginalFactors ( const NonlinearFactorGraph graph,
const Values theta,
const KeyVector keys,
const GaussianFactorGraph::Eliminate eliminateFunction = EliminatePreferCholesky 
)
static

Marginalize specific keys from a nonlinear graph, wrap in LinearContainers.

Definition at line 423 of file BatchFixedLagSmoother.cpp.

◆ equals()

bool gtsam::BatchFixedLagSmoother::equals ( const FixedLagSmoother rhs,
double  tol = 1e-9 
) const
overridevirtual

Check if two IncrementalFixedLagSmoother Objects are equal

Reimplemented from gtsam::FixedLagSmoother.

Definition at line 38 of file BatchFixedLagSmoother.cpp.

◆ eraseKeys()

void gtsam::BatchFixedLagSmoother::eraseKeys ( const KeyVector keys)
protected

Erase any keys associated with timestamps before the provided time

Definition at line 154 of file BatchFixedLagSmoother.cpp.

◆ getDelta()

const VectorValues& gtsam::BatchFixedLagSmoother::getDelta ( ) const
inline

Access the current set of deltas to the linearization point

Definition at line 101 of file nonlinear/BatchFixedLagSmoother.h.

◆ getFactors()

const NonlinearFactorGraph& gtsam::BatchFixedLagSmoother::getFactors ( ) const
inline

Access the current set of factors

Definition at line 86 of file nonlinear/BatchFixedLagSmoother.h.

◆ getLinearizationPoint()

const Values& gtsam::BatchFixedLagSmoother::getLinearizationPoint ( ) const
inline

Access the current linearization point

Definition at line 91 of file nonlinear/BatchFixedLagSmoother.h.

◆ getOrdering()

const Ordering& gtsam::BatchFixedLagSmoother::getOrdering ( ) const
inline

Access the current ordering

Definition at line 96 of file nonlinear/BatchFixedLagSmoother.h.

◆ insertFactors()

void gtsam::BatchFixedLagSmoother::insertFactors ( const NonlinearFactorGraph newFactors)
protected

Augment the list of factors with a set of new factors

Definition at line 112 of file BatchFixedLagSmoother.cpp.

◆ marginalCovariance()

Matrix gtsam::BatchFixedLagSmoother::marginalCovariance ( Key  key) const

Calculate marginal covariance on given variable.

Definition at line 47 of file BatchFixedLagSmoother.cpp.

◆ marginalize()

void gtsam::BatchFixedLagSmoother::marginalize ( const KeyVector marginalizableKeys)
protected

Marginalize out selected variables

Definition at line 310 of file BatchFixedLagSmoother.cpp.

◆ optimize()

FixedLagSmoother::Result gtsam::BatchFixedLagSmoother::optimize ( )
protected

Optimize the current graph using a modified version of L-M

Definition at line 185 of file BatchFixedLagSmoother.cpp.

◆ params() [1/2]

const LevenbergMarquardtParams& gtsam::BatchFixedLagSmoother::params ( ) const
inline

read the current set of optimizer parameters

Definition at line 76 of file nonlinear/BatchFixedLagSmoother.h.

◆ params() [2/2]

LevenbergMarquardtParams& gtsam::BatchFixedLagSmoother::params ( )
inline

update the current set of optimizer parameters

Definition at line 81 of file nonlinear/BatchFixedLagSmoother.h.

◆ print()

void gtsam::BatchFixedLagSmoother::print ( const std::string &  s = "BatchFixedLagSmoother:\n",
const KeyFormatter keyFormatter = DefaultKeyFormatter 
) const
overridevirtual

Print the factor for debugging and testing (implementing Testable)

Reimplemented from gtsam::FixedLagSmoother.

Definition at line 31 of file BatchFixedLagSmoother.cpp.

◆ PrintKeySet() [1/2]

void gtsam::BatchFixedLagSmoother::PrintKeySet ( const std::set< Key > &  keys,
const std::string &  label 
)
staticprivate

Private methods for printing debug information

Definition at line 348 of file BatchFixedLagSmoother.cpp.

◆ PrintKeySet() [2/2]

void gtsam::BatchFixedLagSmoother::PrintKeySet ( const gtsam::KeySet keys,
const std::string &  label 
)
staticprivate

Definition at line 358 of file BatchFixedLagSmoother.cpp.

◆ PrintSymbolicFactor() [1/2]

void gtsam::BatchFixedLagSmoother::PrintSymbolicFactor ( const NonlinearFactor::shared_ptr factor)
staticprivate

Definition at line 368 of file BatchFixedLagSmoother.cpp.

◆ PrintSymbolicFactor() [2/2]

static void gtsam::BatchFixedLagSmoother::PrintSymbolicFactor ( const GaussianFactor::shared_ptr factor)
staticprivate

◆ PrintSymbolicGraph() [1/2]

void gtsam::BatchFixedLagSmoother::PrintSymbolicGraph ( const NonlinearFactorGraph graph,
const std::string &  label 
)
staticprivate

Definition at line 392 of file BatchFixedLagSmoother.cpp.

◆ PrintSymbolicGraph() [2/2]

void gtsam::BatchFixedLagSmoother::PrintSymbolicGraph ( const GaussianFactorGraph graph,
const std::string &  label 
)
staticprivate

Definition at line 401 of file BatchFixedLagSmoother.cpp.

◆ removeFactors()

void gtsam::BatchFixedLagSmoother::removeFactors ( const std::set< size_t > &  deleteFactors)
protected

Remove factors from the list of factors by slot index

Definition at line 133 of file BatchFixedLagSmoother.cpp.

◆ reorder()

void gtsam::BatchFixedLagSmoother::reorder ( const KeyVector marginalizeKeys = KeyVector())
protected

Use colamd to update into an efficient ordering

Definition at line 179 of file BatchFixedLagSmoother.cpp.

◆ update()

FixedLagSmoother::Result gtsam::BatchFixedLagSmoother::update ( const NonlinearFactorGraph newFactors = NonlinearFactorGraph(),
const Values newTheta = Values(),
const KeyTimestampMap timestamps = KeyTimestampMap(),
const FactorIndices factorsToRemove = FactorIndices() 
)
overridevirtual

Add new factors, updating the solution and relinearizing as needed.

Implements gtsam::FixedLagSmoother.

Definition at line 53 of file BatchFixedLagSmoother.cpp.

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.

Definition at line 149 of file nonlinear/BatchFixedLagSmoother.h.

◆ delta_

VectorValues gtsam::BatchFixedLagSmoother::delta_
protected

The current set of linear deltas

Definition at line 146 of file nonlinear/BatchFixedLagSmoother.h.

◆ 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

Definition at line 131 of file nonlinear/BatchFixedLagSmoother.h.

◆ factorIndex_

FactorIndex gtsam::BatchFixedLagSmoother::factorIndex_
protected

A cross-reference structure to allow efficient factor lookups by key

Definition at line 152 of file nonlinear/BatchFixedLagSmoother.h.

◆ factors_

NonlinearFactorGraph gtsam::BatchFixedLagSmoother::factors_
protected

The nonlinear factors

Definition at line 134 of file nonlinear/BatchFixedLagSmoother.h.

◆ linearValues_

Values gtsam::BatchFixedLagSmoother::linearValues_
protected

The set of values involved in current linear factors.

Definition at line 140 of file nonlinear/BatchFixedLagSmoother.h.

◆ ordering_

Ordering gtsam::BatchFixedLagSmoother::ordering_
protected

The current ordering

Definition at line 143 of file nonlinear/BatchFixedLagSmoother.h.

◆ parameters_

LevenbergMarquardtParams gtsam::BatchFixedLagSmoother::parameters_
protected

The L-M optimization parameters

Definition at line 126 of file nonlinear/BatchFixedLagSmoother.h.

◆ theta_

Values gtsam::BatchFixedLagSmoother::theta_
protected

The current linearization point

Definition at line 137 of file nonlinear/BatchFixedLagSmoother.h.


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


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:46:15