#include <ConcurrentBatchSmoother.h>

Inheritance diagram for gtsam::ConcurrentBatchSmoother:

Classes
struct	Result

Public Types
typedef ConcurrentSmoother	Base
	typedef for base class More...

typedef boost::shared_ptr< ConcurrentBatchSmoother >	shared_ptr

Public Types inherited from gtsam::ConcurrentSmoother
typedef boost::shared_ptr< ConcurrentSmoother >	shared_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 VectorValues &	getDelta () const

const NonlinearFactorGraph &	getFactors () const

const Values &	getLinearizationPoint () const

const Ordering &	getOrdering () const

void	getSummarizedFactors (NonlinearFactorGraph &summarizedFactors, Values &separatorValues) override

void	postsync () override

void	presync () override

void	print (const std::string &s="Concurrent Batch Smoother:\n", 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 boost::optional< std::vector< size_t > > &removeFactorIndices=boost::none)

	~ConcurrentBatchSmoother () override

Public Member Functions inherited from gtsam::ConcurrentSmoother
	ConcurrentSmoother ()

virtual	~ConcurrentSmoother ()

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 smoother. More...

std::vector< size_t >	filterSummarizationSlots_
	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_t >	insertFactors (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

typedef ConcurrentSmoother gtsam::ConcurrentBatchSmoother::Base

typedef for base class

Definition at line 35 of file ConcurrentBatchSmoother.h.

typedef boost::shared_ptr<ConcurrentBatchSmoother> gtsam::ConcurrentBatchSmoother::shared_ptr

Definition at line 34 of file ConcurrentBatchSmoother.h.

Constructor & Destructor Documentation

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

inline

Default constructor

Definition at line 57 of file ConcurrentBatchSmoother.h.

gtsam::ConcurrentBatchSmoother::~ConcurrentBatchSmoother ( )

inlineoverride

Default destructor

Definition at line 60 of file ConcurrentBatchSmoother.h.

Member Function Documentation

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.

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.

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.

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.

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

inline

Access the current set of factors

Definition at line 69 of file ConcurrentBatchSmoother.h.

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

inline

Access the current linearization point

Definition at line 74 of file ConcurrentBatchSmoother.h.

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

inline

Access the current ordering

Definition at line 79 of file ConcurrentBatchSmoother.h.

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

summarizedFactors The summarized factors for the filter branch

Implements gtsam::ConcurrentSmoother.

Definition at line 115 of file ConcurrentBatchSmoother.cpp.

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

private

Augment the graph with new factors

Parameters

factors The factors to add to the graph

Returns: The slots in the graph where they were inserted

Definition at line 182 of file ConcurrentBatchSmoother.cpp.

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

private

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

Definition at line 239 of file ConcurrentBatchSmoother.cpp.

void gtsam::ConcurrentBatchSmoother::postsync ( )

overridevirtual

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

Reimplemented from gtsam::ConcurrentSmoother.

Definition at line 174 of file ConcurrentBatchSmoother.cpp.

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.

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.

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 402 of file ConcurrentBatchSmoother.cpp.

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 384 of file ConcurrentBatchSmoother.cpp.

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

private

Remove factors from the graph by slot index

Parameters

slots The slots in the factor graph that should be deleted

Definition at line 210 of file ConcurrentBatchSmoother.cpp.

void gtsam::ConcurrentBatchSmoother::reorder ( )

private

Use colamd to update into an efficient ordering

Definition at line 228 of file ConcurrentBatchSmoother.cpp.

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

smootherFactors	A set of new factors added to the smoother from the filter
smootherValues	Linearization points for any new variables
summarizedFactors	An updated version of the filter branch summarized factors
rootValues	The linearization point of the root variables

Implements gtsam::ConcurrentSmoother.

Definition at line 129 of file ConcurrentBatchSmoother.cpp.

ConcurrentBatchSmoother::Result gtsam::ConcurrentBatchSmoother::update	(	const NonlinearFactorGraph &	newFactors = `NonlinearFactorGraph()`,
		const Values &	newTheta = `Values()`,
		const boost::optional< std::vector< size_t > > &	removeFactorIndices = `boost::none`
	)

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

newFactors	The new factors to be added to the smoother
newTheta	Initialization 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.

void gtsam::ConcurrentBatchSmoother::updateSmootherSummarization ( )

private

Calculate the smoother marginal factors on the separator variables

Definition at line 360 of file ConcurrentBatchSmoother.cpp.

Member Data Documentation

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.

VectorValues gtsam::ConcurrentBatchSmoother::delta_

protected

The current set of linear deltas from the linearization point.

Definition at line 161 of file ConcurrentBatchSmoother.h.

NonlinearFactorGraph gtsam::ConcurrentBatchSmoother::factors_

protected

The set of all factors currently in the smoother.

Definition at line 158 of file ConcurrentBatchSmoother.h.

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 gtsam::ConcurrentBatchSmoother::ordering_

protected

The current ordering used to calculate the linear deltas.

Definition at line 160 of file ConcurrentBatchSmoother.h.

LevenbergMarquardtParams gtsam::ConcurrentBatchSmoother::parameters_

protected

LM parameters.

Definition at line 157 of file ConcurrentBatchSmoother.h.

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.

NonlinearFactorGraph gtsam::ConcurrentBatchSmoother::smootherSummarization_

protected

A temporary holding place for calculated smoother summarization.

Definition at line 168 of file ConcurrentBatchSmoother.h.

Values gtsam::ConcurrentBatchSmoother::theta_

protected

Current linearization point of all variables in the smoother.

Definition at line 159 of file ConcurrentBatchSmoother.h.

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:

Classes

Public Types

Public Member Functions

Protected Attributes

Private Member Functions

Static Private Member Functions

Detailed Description

Member Typedef Documentation

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation