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

#include <ConcurrentBatchSmoother.h>

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

Classes

struct  Result
 

Public Types

typedef ConcurrentSmoother Base
 typedef for base class More...
 
typedef std::shared_ptr< ConcurrentBatchSmoothershared_ptr
 
- Public Types inherited from gtsam::ConcurrentSmoother
typedef std::shared_ptr< ConcurrentSmoothershared_ptr
 

Public Member Functions

Values calculateEstimate () const
 
template<class VALUE >
VALUE calculateEstimate (Key key) const
 
 ConcurrentBatchSmoother (const LevenbergMarquardtParams &parameters=LevenbergMarquardtParams())
 
bool equals (const ConcurrentSmoother &rhs, double tol=1e-9) const override
 
const VectorValuesgetDelta () const
 
const NonlinearFactorGraphgetFactors () const
 
const ValuesgetLinearizationPoint () const
 
const OrderinggetOrdering () const
 
void getSummarizedFactors (NonlinearFactorGraph &summarizedFactors, Values &separatorValues) override
 
void postsync () override
 
void presync () override
 
void print (const std::string &s="Concurrent Batch Smoother:\, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
 
void synchronize (const NonlinearFactorGraph &smootherFactors, const Values &smootherValues, const NonlinearFactorGraph &summarizedFactors, const Values &separatorValues) override
 
virtual Result update (const NonlinearFactorGraph &newFactors=NonlinearFactorGraph(), const Values &newTheta=Values(), const std::optional< std::vector< size_t > > &removeFactorIndices={})
 
 ~ConcurrentBatchSmoother () override
 
- Public Member Functions inherited from gtsam::ConcurrentSmoother
 ConcurrentSmoother ()
 
virtual ~ConcurrentSmoother ()=default
 

Protected Attributes

std::queue< size_tavailableSlots_
 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 smoother. More...
 
std::vector< size_tfilterSummarizationSlots_
 The slots in factor graph that correspond to the current filter summarization factors. More...
 
Ordering ordering_
 The current ordering used to calculate the linear deltas. More...
 
LevenbergMarquardtParams parameters_
 LM parameters. More...
 
Values separatorValues_
 The linearization points of the separator variables. These should not be updated during optimization. More...
 
NonlinearFactorGraph smootherSummarization_
 A temporary holding place for calculated smoother summarization. More...
 
Values theta_
 Current linearization point of all variables in the smoother. More...
 
VariableIndex variableIndex_
 The current variable index, which allows efficient factor lookup by variable. More...
 

Private Member Functions

std::vector< size_tinsertFactors (const NonlinearFactorGraph &factors)
 
Result optimize ()
 
void removeFactors (const std::vector< size_t > &slots)
 
void reorder ()
 
void updateSmootherSummarization ()
 

Static Private Member Functions

static void PrintLinearFactor (const GaussianFactor::shared_ptr &factor, const std::string &indent="", const KeyFormatter &keyFormatter=DefaultKeyFormatter)
 
static void PrintNonlinearFactor (const NonlinearFactor::shared_ptr &factor, const std::string &indent="", const KeyFormatter &keyFormatter=DefaultKeyFormatter)
 

Detailed Description

A Levenberg-Marquardt Batch Smoother that implements the Concurrent Filtering and Smoother interface.

Definition at line 31 of file ConcurrentBatchSmoother.h.

Member Typedef Documentation

◆ Base

typedef for base class

Definition at line 35 of file ConcurrentBatchSmoother.h.

◆ shared_ptr

Definition at line 34 of file ConcurrentBatchSmoother.h.

Constructor & Destructor Documentation

◆ ConcurrentBatchSmoother()

gtsam::ConcurrentBatchSmoother::ConcurrentBatchSmoother ( const LevenbergMarquardtParams parameters = LevenbergMarquardtParams())
inline

Default constructor

Definition at line 57 of file ConcurrentBatchSmoother.h.

◆ ~ConcurrentBatchSmoother()

gtsam::ConcurrentBatchSmoother::~ConcurrentBatchSmoother ( )
inlineoverride

Default destructor

Definition at line 60 of file ConcurrentBatchSmoother.h.

Member Function Documentation

◆ calculateEstimate() [1/2]

Values gtsam::ConcurrentBatchSmoother::calculateEstimate ( ) const
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 91 of file ConcurrentBatchSmoother.h.

◆ calculateEstimate() [2/2]

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

Parameters
key
Returns

Definition at line 101 of file ConcurrentBatchSmoother.h.

◆ equals()

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

Check if two Concurrent Smoothers are equal

Implements gtsam::ConcurrentSmoother.

Definition at line 38 of file ConcurrentBatchSmoother.cpp.

◆ getDelta()

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

Access the current set of deltas to the linearization point

Definition at line 84 of file ConcurrentBatchSmoother.h.

◆ getFactors()

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

Access the current set of factors

Definition at line 69 of file ConcurrentBatchSmoother.h.

◆ getLinearizationPoint()

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

Access the current linearization point

Definition at line 74 of file ConcurrentBatchSmoother.h.

◆ getOrdering()

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

Access the current ordering

Definition at line 79 of file ConcurrentBatchSmoother.h.

◆ getSummarizedFactors()

void gtsam::ConcurrentBatchSmoother::getSummarizedFactors ( NonlinearFactorGraph summarizedFactors,
Values separatorValues 
)
overridevirtual

Populate the provided containers with factors that constitute the smoother branch summarization needed by the filter.

Parameters
summarizedFactorsThe summarized factors for the filter branch

Implements gtsam::ConcurrentSmoother.

Definition at line 115 of file ConcurrentBatchSmoother.cpp.

◆ insertFactors()

std::vector< size_t > gtsam::ConcurrentBatchSmoother::insertFactors ( const NonlinearFactorGraph factors)
private

Augment the graph with new factors

Parameters
factorsThe factors to add to the graph
Returns
The slots in the graph where they were inserted

Definition at line 186 of file ConcurrentBatchSmoother.cpp.

◆ optimize()

ConcurrentBatchSmoother::Result gtsam::ConcurrentBatchSmoother::optimize ( )
private

Use a modified version of L-M to update the linearization point and delta

Definition at line 243 of file ConcurrentBatchSmoother.cpp.

◆ postsync()

void gtsam::ConcurrentBatchSmoother::postsync ( )
overridevirtual

Perform any required operations after the synchronization process finishes. Called by 'synchronize'

Reimplemented from gtsam::ConcurrentSmoother.

Definition at line 178 of file ConcurrentBatchSmoother.cpp.

◆ presync()

void gtsam::ConcurrentBatchSmoother::presync ( )
overridevirtual

Perform any required operations before the synchronization process starts. Called by 'synchronize'

Reimplemented from gtsam::ConcurrentSmoother.

Definition at line 107 of file ConcurrentBatchSmoother.cpp.

◆ print()

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

Implement a GTSAM standard 'print' function

Implements gtsam::ConcurrentSmoother.

Definition at line 28 of file ConcurrentBatchSmoother.cpp.

◆ PrintLinearFactor()

void gtsam::ConcurrentBatchSmoother::PrintLinearFactor ( const GaussianFactor::shared_ptr factor,
const std::string &  indent = "",
const KeyFormatter keyFormatter = DefaultKeyFormatter 
)
staticprivate

Print just the nonlinear keys in a linear factor

Definition at line 403 of file ConcurrentBatchSmoother.cpp.

◆ PrintNonlinearFactor()

void gtsam::ConcurrentBatchSmoother::PrintNonlinearFactor ( const NonlinearFactor::shared_ptr factor,
const std::string &  indent = "",
const KeyFormatter keyFormatter = DefaultKeyFormatter 
)
staticprivate

Print just the nonlinear keys in a nonlinear factor

Definition at line 385 of file ConcurrentBatchSmoother.cpp.

◆ removeFactors()

void gtsam::ConcurrentBatchSmoother::removeFactors ( const std::vector< size_t > &  slots)
private

Remove factors from the graph by slot index

Parameters
slotsThe slots in the factor graph that should be deleted

Definition at line 214 of file ConcurrentBatchSmoother.cpp.

◆ reorder()

void gtsam::ConcurrentBatchSmoother::reorder ( )
private

Use colamd to update into an efficient ordering

Definition at line 232 of file ConcurrentBatchSmoother.cpp.

◆ synchronize()

void gtsam::ConcurrentBatchSmoother::synchronize ( const NonlinearFactorGraph smootherFactors,
const Values smootherValues,
const NonlinearFactorGraph summarizedFactors,
const Values separatorValues 
)
overridevirtual

Apply the new smoother factors sent by the filter, and the updated version of the filter branch summarized factors.

Parameters
smootherFactorsA set of new factors added to the smoother from the filter
smootherValuesLinearization points for any new variables
summarizedFactorsAn updated version of the filter branch summarized factors
rootValuesThe linearization point of the root variables

Implements gtsam::ConcurrentSmoother.

Definition at line 129 of file ConcurrentBatchSmoother.cpp.

◆ update()

ConcurrentBatchSmoother::Result gtsam::ConcurrentBatchSmoother::update ( const NonlinearFactorGraph newFactors = NonlinearFactorGraph(),
const Values newTheta = Values(),
const std::optional< std::vector< size_t > > &  removeFactorIndices = {} 
)
virtual

Add new factors and variables to the smoother.

Add new measurements, and optionally new variables, to the smoother. This runs a full step of the ISAM2 algorithm, relinearizing and updating the solution as needed, according to the wildfire and relinearize thresholds.

Parameters
newFactorsThe new factors to be added to the smoother
newThetaInitialization points for new variables to be added to the smoother You must include here all new variables occuring in newFactors (which were not already in the smoother). There must not be any variables here that do not occur in newFactors, and additionally, variables that were already in the system must not be included here.

Definition at line 49 of file ConcurrentBatchSmoother.cpp.

◆ updateSmootherSummarization()

void gtsam::ConcurrentBatchSmoother::updateSmootherSummarization ( )
private

Calculate the smoother marginal factors on the separator variables

Definition at line 364 of file ConcurrentBatchSmoother.cpp.

Member Data Documentation

◆ availableSlots_

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

The set of available factor graph slots caused by deleting factors.

Definition at line 163 of file ConcurrentBatchSmoother.h.

◆ delta_

VectorValues gtsam::ConcurrentBatchSmoother::delta_
protected

The current set of linear deltas from the linearization point.

Definition at line 161 of file ConcurrentBatchSmoother.h.

◆ factors_

NonlinearFactorGraph gtsam::ConcurrentBatchSmoother::factors_
protected

The set of all factors currently in the smoother.

Definition at line 158 of file ConcurrentBatchSmoother.h.

◆ filterSummarizationSlots_

std::vector<size_t> gtsam::ConcurrentBatchSmoother::filterSummarizationSlots_
protected

The slots in factor graph that correspond to the current filter summarization factors.

Definition at line 165 of file ConcurrentBatchSmoother.h.

◆ ordering_

Ordering gtsam::ConcurrentBatchSmoother::ordering_
protected

The current ordering used to calculate the linear deltas.

Definition at line 160 of file ConcurrentBatchSmoother.h.

◆ parameters_

LevenbergMarquardtParams gtsam::ConcurrentBatchSmoother::parameters_
protected

LM parameters.

Definition at line 157 of file ConcurrentBatchSmoother.h.

◆ separatorValues_

Values gtsam::ConcurrentBatchSmoother::separatorValues_
protected

The linearization points of the separator variables. These should not be updated during optimization.

Definition at line 164 of file ConcurrentBatchSmoother.h.

◆ smootherSummarization_

NonlinearFactorGraph gtsam::ConcurrentBatchSmoother::smootherSummarization_
protected

A temporary holding place for calculated smoother summarization.

Definition at line 168 of file ConcurrentBatchSmoother.h.

◆ theta_

Values gtsam::ConcurrentBatchSmoother::theta_
protected

Current linearization point of all variables in the smoother.

Definition at line 159 of file ConcurrentBatchSmoother.h.

◆ variableIndex_

VariableIndex gtsam::ConcurrentBatchSmoother::variableIndex_
protected

The current variable index, which allows efficient factor lookup by variable.

Definition at line 162 of file ConcurrentBatchSmoother.h.


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


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