#include <ConcurrentBatchFilter.h>

Inheritance diagram for gtsam::ConcurrentBatchFilter:

Classes
struct	Result

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

typedef std::shared_ptr< ConcurrentBatchFilter >	shared_ptr

Public Types inherited from gtsam::ConcurrentFilter
typedef std::shared_ptr< ConcurrentFilter >	shared_ptr

Public Member Functions
Values	calculateEstimate () const

template<class VALUE >
VALUE	calculateEstimate (Key key) const

	ConcurrentBatchFilter (const LevenbergMarquardtParams &parameters=LevenbergMarquardtParams())

bool	equals (const ConcurrentFilter &rhs, double tol=1e-9) const override

const VectorValues &	getDelta () const

const NonlinearFactorGraph &	getFactors () const

const Values &	getLinearizationPoint () const

const Ordering &	getOrdering () const

void	getSmootherFactors (NonlinearFactorGraph &smootherFactors, Values &smootherValues) override

void	getSummarizedFactors (NonlinearFactorGraph &filterSummarization, Values &filterSummarizationValues) override

void	postsync () override

void	presync () override

void	print (const std::string &s="Concurrent Batch Filter:\, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override

void	synchronize (const NonlinearFactorGraph &smootherSummarization, const Values &smootherSummarizationValues) override

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

	~ConcurrentBatchFilter () override=default

Public Member Functions inherited from gtsam::ConcurrentFilter
	ConcurrentFilter ()=default

virtual	~ConcurrentFilter ()=default

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

NonlinearFactorGraph	filterSummarization_
	A temporary holding place for calculated filter summarization factors to be sent to the smoother. More...

Ordering	ordering_
	The current ordering used to calculate the linear deltas. More...

LevenbergMarquardtParams	parameters_
	LM parameters. More...

std::vector< size_t >	separatorSummarizationSlots_
	The slots in factor graph that correspond to the current smoother summarization on the current separator. More...

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

NonlinearFactorGraph	smootherFactors_
	A temporary holding place for the set of full nonlinear factors being sent to the smoother. More...

NonlinearFactorGraph	smootherShortcut_
	A set of conditional factors from the old separator to the current separator (recursively calculated during each filter update) More...

NonlinearFactorGraph	smootherSummarization_
	The smoother summarization on the old separator sent by the smoother during the last synchronization. More...

Values	smootherValues_
	A temporary holding place for the linearization points of all keys being sent to the smoother. More...

Values	theta_
	Current linearization point of all variables in the filter. More...

Private Member Functions
std::vector< size_t >	insertFactors (const NonlinearFactorGraph &factors)

void	moveSeparator (const FastList< Key > &keysToMove)

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

void	reorder (const std::optional< FastList< Key > > &keysToMove={})

Static Private Member Functions
static void	optimize (const NonlinearFactorGraph &factors, Values &theta, const Ordering &ordering, VectorValues &delta, const Values &linearValues, const LevenbergMarquardtParams &parameters, Result &result)

template<class Container >
static void	PrintKeys (const Container &keys, const std::string &indent, const std::string &title, const KeyFormatter &keyFormatter=DefaultKeyFormatter)
	Implementation of PrintKeys. More...

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

static void	PrintLinearFactorGraph (const GaussianFactorGraph &factors, const std::string &indent="", const std::string &title="", const KeyFormatter &keyFormatter=DefaultKeyFormatter)

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

static void	PrintNonlinearFactorGraph (const NonlinearFactorGraph &factors, const std::string &indent="", const std::string &title="", const KeyFormatter &keyFormatter=DefaultKeyFormatter)

static void	PrintNonlinearFactorGraph (const NonlinearFactorGraph &factors, const std::vector< size_t > &slots, const std::string &indent="", const std::string &title="", const KeyFormatter &keyFormatter=DefaultKeyFormatter)

Detailed Description

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

Definition at line 31 of file ConcurrentBatchFilter.h.

Member Typedef Documentation

◆ Base

typedef ConcurrentFilter gtsam::ConcurrentBatchFilter::Base

typedef for base class

Definition at line 35 of file ConcurrentBatchFilter.h.

◆ shared_ptr

typedef std::shared_ptr<ConcurrentBatchFilter> gtsam::ConcurrentBatchFilter::shared_ptr

Definition at line 34 of file ConcurrentBatchFilter.h.

Constructor & Destructor Documentation

◆ ConcurrentBatchFilter()

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

inline

Default constructor

Definition at line 64 of file ConcurrentBatchFilter.h.

◆ ~ConcurrentBatchFilter()

gtsam::ConcurrentBatchFilter::~ConcurrentBatchFilter ( )

overridedefault

Default destructor

Member Function Documentation

◆ calculateEstimate() [1/2]

Values gtsam::ConcurrentBatchFilter::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 98 of file ConcurrentBatchFilter.h.

◆ calculateEstimate() [2/2]

template<class VALUE >

VALUE gtsam::ConcurrentBatchFilter::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 108 of file ConcurrentBatchFilter.h.

◆ equals()

bool gtsam::ConcurrentBatchFilter::equals	(	const ConcurrentFilter &	rhs,
		double	tol = `1e-9`
	)		const

overridevirtual

Check if two Concurrent Filters are equal

Implements gtsam::ConcurrentFilter.

Definition at line 105 of file ConcurrentBatchFilter.cpp.

◆ getDelta()

const VectorValues& gtsam::ConcurrentBatchFilter::getDelta ( ) const

inline

Access the current set of deltas to the linearization point

Definition at line 91 of file ConcurrentBatchFilter.h.

◆ getFactors()

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

inline

Access the current set of factors

Definition at line 76 of file ConcurrentBatchFilter.h.

◆ getLinearizationPoint()

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

inline

Access the current linearization point

Definition at line 81 of file ConcurrentBatchFilter.h.

◆ getOrdering()

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

inline

Access the current ordering

Definition at line 86 of file ConcurrentBatchFilter.h.

◆ getSmootherFactors()

void gtsam::ConcurrentBatchFilter::getSmootherFactors	(	NonlinearFactorGraph &	smootherFactors,
		Values &	smootherValues
	)

overridevirtual

Populate the provided containers with factors being sent to the smoother from the filter. These may be original nonlinear factors, or factors encoding a summarization of the filter information. The specifics will be implementation-specific for a given filter.

Parameters

smootherFactors	The new factors to be added to the smoother
smootherValues	The linearization points of any new variables

Implements gtsam::ConcurrentFilter.

Definition at line 288 of file ConcurrentBatchFilter.cpp.

◆ getSummarizedFactors()

void gtsam::ConcurrentBatchFilter::getSummarizedFactors	(	NonlinearFactorGraph &	filterSummarization,
		Values &	filterSummarizationValues
	)

overridevirtual

Populate the provided containers with factors that constitute the filter branch summarization needed by the smoother. Also, linearization points for the new root clique must be provided.

Parameters

summarizedFactors	The summarized factors for the filter branch
rootValues	The linearization points of the root clique variables

Implements gtsam::ConcurrentFilter.

Definition at line 276 of file ConcurrentBatchFilter.cpp.

◆ insertFactors()

std::vector< size_t > gtsam::ConcurrentBatchFilter::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 312 of file ConcurrentBatchFilter.cpp.

◆ moveSeparator()

void gtsam::ConcurrentBatchFilter::moveSeparator ( const FastList< Key > & keysToMove )

private

Marginalize out the set of requested variables from the filter, caching them for the smoother This effectively moves the separator.

Parameters

keysToMove The set of keys to move from the filter to the smoother

Definition at line 510 of file ConcurrentBatchFilter.cpp.

◆ optimize()

void gtsam::ConcurrentBatchFilter::optimize	(	const NonlinearFactorGraph &	factors,
		Values &	theta,
		const Ordering &	ordering,
		VectorValues &	delta,
		const Values &	linearValues,
		const LevenbergMarquardtParams &	parameters,
		ConcurrentBatchFilter::Result &	result
	)

staticprivate

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

Definition at line 370 of file ConcurrentBatchFilter.cpp.

◆ postsync()

void gtsam::ConcurrentBatchFilter::postsync ( )

overridevirtual

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

Reimplemented from gtsam::ConcurrentFilter.

Definition at line 300 of file ConcurrentBatchFilter.cpp.

◆ presync()

void gtsam::ConcurrentBatchFilter::presync ( )

overridevirtual

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

Reimplemented from gtsam::ConcurrentFilter.

Definition at line 192 of file ConcurrentBatchFilter.cpp.

◆ print()

void gtsam::ConcurrentBatchFilter::print	(	const std::string &	s = `"Concurrent Batch Filter:\n"`,
		const KeyFormatter &	keyFormatter = `DefaultKeyFormatter`
	)		const

overridevirtual

Implement a GTSAM standard 'print' function

Implements gtsam::ConcurrentFilter.

Definition at line 96 of file ConcurrentBatchFilter.cpp.

◆ PrintKeys()

template<class Container >

void gtsam::ConcurrentBatchFilter::PrintKeys	(	const Container &	keys,
		const std::string &	indent,
		const std::string &	title,
		const KeyFormatter &	keyFormatter = `DefaultKeyFormatter`
	)

staticprivate

Implementation of PrintKeys.

Print just the nonlinear keys contained inside a container

Definition at line 245 of file ConcurrentBatchFilter.h.

◆ PrintLinearFactor()

void gtsam::ConcurrentBatchFilter::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 64 of file ConcurrentBatchFilter.cpp.

◆ PrintLinearFactorGraph()

void gtsam::ConcurrentBatchFilter::PrintLinearFactorGraph	(	const GaussianFactorGraph &	factors,
		const std::string &	indent = `""`,
		const std::string &	title = `""`,
		const KeyFormatter &	keyFormatter = `DefaultKeyFormatter`
	)

staticprivate

Print just the nonlinear keys in each linear factor for a whole Gaussian Factor Graph

Definition at line 87 of file ConcurrentBatchFilter.cpp.

◆ PrintNonlinearFactor()

void gtsam::ConcurrentBatchFilter::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 27 of file ConcurrentBatchFilter.cpp.

◆ PrintNonlinearFactorGraph() [1/2]

void gtsam::ConcurrentBatchFilter::PrintNonlinearFactorGraph	(	const NonlinearFactorGraph &	factors,
		const std::string &	indent = `""`,
		const std::string &	title = `""`,
		const KeyFormatter &	keyFormatter = `DefaultKeyFormatter`
	)

staticprivate

Print just the nonlinear keys in each factor for a whole Nonlinear Factor Graph

Definition at line 46 of file ConcurrentBatchFilter.cpp.

◆ PrintNonlinearFactorGraph() [2/2]

void gtsam::ConcurrentBatchFilter::PrintNonlinearFactorGraph	(	const NonlinearFactorGraph &	factors,
		const std::vector< size_t > &	slots,
		const std::string &	indent = `""`,
		const std::string &	title = `""`,
		const KeyFormatter &	keyFormatter = `DefaultKeyFormatter`
	)

staticprivate

Print just the nonlinear keys of specific factors in a Nonlinear Factor Graph

Definition at line 55 of file ConcurrentBatchFilter.cpp.

◆ removeFactors()

void gtsam::ConcurrentBatchFilter::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 340 of file ConcurrentBatchFilter.cpp.

◆ reorder()

void gtsam::ConcurrentBatchFilter::reorder ( const std::optional< FastList< Key > > & keysToMove = {} )

private

Use colamd to update into an efficient ordering

Definition at line 358 of file ConcurrentBatchFilter.cpp.

◆ synchronize()

void gtsam::ConcurrentBatchFilter::synchronize	(	const NonlinearFactorGraph &	smootherSummarization,
		const Values &	smootherSummarizationValues
	)

overridevirtual

Apply the updated version of the smoother branch summarized factors.

Parameters

summarizedFactors An updated version of the smoother branch summarized factors

Implements gtsam::ConcurrentFilter.

Definition at line 200 of file ConcurrentBatchFilter.cpp.

◆ update()

ConcurrentBatchFilter::Result gtsam::ConcurrentBatchFilter::update	(	const NonlinearFactorGraph &	newFactors = `NonlinearFactorGraph()`,
		const Values &	newTheta = `Values()`,
		const std::optional< FastList< Key > > &	keysToMove = `{}`,
		const std::optional< std::vector< size_t > > &	removeFactorIndices = `{}`
	)

virtual

Add new factors and variables to the filter.

Add new measurements, and optionally new variables, to the filter. This runs a full update step of the derived filter algorithm

Parameters

newFactors	The new factors to be added to the smoother
newTheta	Initialization points for new variables to be added to the filter You must include here all new variables occurring in newFactors that were not already in the filter.
keysToMove	An optional set of keys to move from the filter to the smoother
removeFactorIndices	An optional set of indices corresponding to the factors you want to remove from the graph

Definition at line 121 of file ConcurrentBatchFilter.cpp.

Member Data Documentation

◆ availableSlots_

std::queue<size_t> gtsam::ConcurrentBatchFilter::availableSlots_

protected

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

Definition at line 174 of file ConcurrentBatchFilter.h.

◆ delta_

VectorValues gtsam::ConcurrentBatchFilter::delta_

protected

The current set of linear deltas from the linearization point.

Definition at line 173 of file ConcurrentBatchFilter.h.

◆ factors_

NonlinearFactorGraph gtsam::ConcurrentBatchFilter::factors_

protected

The set of all factors currently in the filter.

Definition at line 170 of file ConcurrentBatchFilter.h.

◆ filterSummarization_

NonlinearFactorGraph gtsam::ConcurrentBatchFilter::filterSummarization_

protected

A temporary holding place for calculated filter summarization factors to be sent to the smoother.

Definition at line 183 of file ConcurrentBatchFilter.h.

◆ ordering_

Ordering gtsam::ConcurrentBatchFilter::ordering_

protected

The current ordering used to calculate the linear deltas.

Definition at line 172 of file ConcurrentBatchFilter.h.

◆ parameters_

LevenbergMarquardtParams gtsam::ConcurrentBatchFilter::parameters_

protected

LM parameters.

Definition at line 169 of file ConcurrentBatchFilter.h.

◆ separatorSummarizationSlots_

std::vector<size_t> gtsam::ConcurrentBatchFilter::separatorSummarizationSlots_

protected

The slots in factor graph that correspond to the current smoother summarization on the current separator.

Definition at line 176 of file ConcurrentBatchFilter.h.

◆ separatorValues_

Values gtsam::ConcurrentBatchFilter::separatorValues_

protected

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

Definition at line 175 of file ConcurrentBatchFilter.h.

◆ smootherFactors_

NonlinearFactorGraph gtsam::ConcurrentBatchFilter::smootherFactors_

protected

A temporary holding place for the set of full nonlinear factors being sent to the smoother.

Definition at line 184 of file ConcurrentBatchFilter.h.

◆ smootherShortcut_

NonlinearFactorGraph gtsam::ConcurrentBatchFilter::smootherShortcut_

protected

A set of conditional factors from the old separator to the current separator (recursively calculated during each filter update)

Definition at line 180 of file ConcurrentBatchFilter.h.

◆ smootherSummarization_

NonlinearFactorGraph gtsam::ConcurrentBatchFilter::smootherSummarization_

protected

The smoother summarization on the old separator sent by the smoother during the last synchronization.

Definition at line 179 of file ConcurrentBatchFilter.h.

◆ smootherValues_

Values gtsam::ConcurrentBatchFilter::smootherValues_

protected

A temporary holding place for the linearization points of all keys being sent to the smoother.

Definition at line 185 of file ConcurrentBatchFilter.h.

◆ theta_

Values gtsam::ConcurrentBatchFilter::theta_

protected

Current linearization point of all variables in the filter.

Definition at line 171 of file ConcurrentBatchFilter.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

◆ Base

◆ shared_ptr

Constructor & Destructor Documentation

◆ ConcurrentBatchFilter()

◆ ~ConcurrentBatchFilter()

Member Function Documentation

◆ calculateEstimate() [1/2]

◆ calculateEstimate() [2/2]

◆ equals()

◆ getDelta()

◆ getFactors()

◆ getLinearizationPoint()

◆ getOrdering()

◆ getSmootherFactors()

◆ getSummarizedFactors()

◆ insertFactors()

◆ moveSeparator()

◆ optimize()

◆ postsync()

◆ presync()

◆ print()

◆ PrintKeys()

◆ PrintLinearFactor()

◆ PrintLinearFactorGraph()

◆ PrintNonlinearFactor()

◆ PrintNonlinearFactorGraph() [1/2]

◆ PrintNonlinearFactorGraph() [2/2]

◆ removeFactors()

◆ reorder()

◆ synchronize()

◆ update()

Member Data Documentation

◆ availableSlots_

◆ delta_

◆ factors_

◆ filterSummarization_

◆ ordering_

◆ parameters_

◆ separatorSummarizationSlots_

◆ separatorValues_

◆ smootherFactors_

◆ smootherShortcut_

◆ smootherSummarization_

◆ smootherValues_

◆ theta_