Detailed Description

template<typename PoseType>
class gtsam::WnoaFactorGraph< PoseType >

Factor graph specialized for WNOA interpolation-aware computation.

WnoaFactorGraph wraps standard factor graph functionalities with utilities to compute interpolated pose/velocity states under a White-Noise-on-Acceleration (WNOA) motion prior. It stores the mapping from interpolated query times to their left/right bordering estimated states and precomputes interpolation helpers for efficient repeated evaluation.

The graph provides optimized linearization and error computation routines that can exploit precomputed interpolation batches to reduce repeated work when evaluating many wrapper factors using the same query times.

Template Parameters

PoseType Pose group/type (e.g. Pose2, Pose3) used by the interpolator.

Inheritance diagram for gtsam::WnoaFactorGraph< PoseType >:

Public Member Functions
std::shared_ptr< GaussianFactorGraph >	linearize (const Values &linearizationPoint) const override
	Linearize the graph into a GaussianFactorGraph.
double	error (const Values &values) const override
	Compute the unnormalized graph error (sum of factor losses).
std::shared_ptr< const NonlinearFactorGraph >	cloneShared () const override
	Clone into a shared pointer while preserving WnoaFactorGraph behavior.
	WnoaFactorGraph (std::unordered_map< StateData, std::pair< StateData, StateData > > interp_map, const VectorN q_psd_diag, bool fixed_noise_model=false)
	Construct a WnoaFactorGraph with interpolation metadata.
Public Member Functions inherited from gtsam::ExpressionFactorGraph
template<typename T>
void	addExpressionFactor (const Expression< T > &h, const T &z, const SharedNoiseModel &R)
	Directly add ExpressionFactor that implements \|h(x)-z\|^2_R.
Public Member Functions inherited from gtsam::NonlinearFactorGraph
	NonlinearFactorGraph ()
	Default constructor.
template<typename ITERATOR>
	NonlinearFactorGraph (ITERATOR firstFactor, ITERATOR lastFactor)
	Construct from iterator over factors.
template<class CONTAINER>
	NonlinearFactorGraph (const CONTAINER &factors)
	Construct from container of factors (shared_ptr or plain objects).
template<class DERIVEDFACTOR>
	NonlinearFactorGraph (const FactorGraph< DERIVEDFACTOR > &graph)
	Implicit copy/downcast constructor to override explicit template container constructor.
void	print (const std::string &str="NonlinearFactorGraph: ", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
	print
void	printErrors (const Values &values, const std::string &str="NonlinearFactorGraph: ", const KeyFormatter &keyFormatter=DefaultKeyFormatter, const FactorErrorPredicate &printCondition=FactorErrorPredicate{[](const Factor *, double, size_t) { return true;}}) const
	print errors along with factors
bool	equals (const NonlinearFactorGraph &other, double tol=1e-9) const
	Test equality.
void	dot (std::ostream &os, const Values &values, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const GraphvizFormatting &writer=GraphvizFormatting()) const
	Output to graphviz format, stream version, with Values/extra options.
std::string	dot (const Values &values, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const GraphvizFormatting &writer=GraphvizFormatting()) const
	Output to graphviz format string, with Values/extra options.
void	saveGraph (const std::string &filename, const Values &values, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const GraphvizFormatting &writer=GraphvizFormatting()) const
	output to file with graphviz format, with Values/extra options.
void	dot (std::ostream &os, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const
	Output to graphviz format, stream version.
std::string	dot (const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const
	Output to graphviz format string.
void	saveGraph (const std::string &filename, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const
	output to file with graphviz format.
double	probPrime (const Values &values) const
	Unnormalized probability.
std::shared_ptr< SymbolicFactorGraph >	symbolic () const
	Create a symbolic factor graph.
Ordering	orderingCOLAMD () const
	Compute a fill-reducing ordering using COLAMD.
Ordering	orderingCOLAMDConstrained (const FastMap< Key, int > &constraints) const
	Compute a fill-reducing ordering with constraints using CCOLAMD.
std::shared_ptr< HessianFactor >	linearizeToHessianFactor (const Values &values, const Dampen &dampen=nullptr) const
	Instead of producing a GaussianFactorGraph, pre-allocate and linearize directly into a HessianFactor.
std::shared_ptr< HessianFactor >	linearizeToHessianFactor (const Values &values, const Ordering &ordering, const Dampen &dampen=nullptr) const
	Instead of producing a GaussianFactorGraph, pre-allocate and linearize directly into a HessianFactor.
Values	updateCholesky (const Values &values, const Dampen &dampen=nullptr) const
	Linearize and solve in one pass.
Values	updateCholesky (const Values &values, const Ordering &ordering, const Dampen &dampen=nullptr) const
	Linearize and solve in one pass.
NonlinearFactorGraph	clone () const
	Clone() performs a deep-copy of the graph, including all of the factors.
NonlinearFactorGraph	rekey (const std::map< Key, Key > &rekey_mapping) const
	Rekey() performs a deep-copy of all of the factors, and changes keys according to a mapping.
template<typename T>
void	addExpressionFactor (const SharedNoiseModel &R, const T &z, const Expression< T > &h)
	Directly add ExpressionFactor that implements \|h(x)-z\|^2_R.
template<typename T>
void	addPrior (Key key, const T &prior, const SharedNoiseModel &model=nullptr)
	Convenience method which adds a PriorFactor to the factor graph.
template<typename T>
void	addPrior (Key key, const T &prior, const Matrix &covariance)
	Convenience method which adds a PriorFactor to the factor graph.
Public Member Functions inherited from gtsam::FactorGraph< NonlinearFactor >
virtual	~FactorGraph ()=default
	Default destructor Public and virtual so boost serialization can call it.
	FactorGraph (std::initializer_list< std::shared_ptr< DERIVEDFACTOR > > sharedFactors)
	Constructor that takes an initializer list of shared pointers.
void	reserve (size_t size)
	Reserve space for the specified number of factors if you know in advance how many there will be (works like FastVector::reserve).
IsDerived< DERIVEDFACTOR >	push_back (std::shared_ptr< DERIVEDFACTOR > factor)
	Add a factor directly using a shared_ptr.
IsDerived< DERIVEDFACTOR >	emplace_shared (Args &&... args)
	Emplace a shared pointer to factor of given type.
IsDerived< DERIVEDFACTOR >	push_back (const DERIVEDFACTOR &factor)
	Add a factor by value, will be copy-constructed (use push_back with a shared_ptr to avoid the copy).
IsDerived< DERIVEDFACTOR >	add (std::shared_ptr< DERIVEDFACTOR > factor)
	add is a synonym for push_back.
std::enable_if< std::is_base_of< FactorType, DERIVEDFACTOR >::value, This >::type &	operator+= (std::shared_ptr< DERIVEDFACTOR > factor)
	Append factor to factor graph.
std::enable_if< std::is_base_of< FactorType, DERIVEDFACTOR >::value, This >::type &	operator, (std::shared_ptr< DERIVEDFACTOR > factor)
	Overload comma operator to allow for append chaining.
HasDerivedElementType< ITERATOR >	push_back (ITERATOR firstFactor, ITERATOR lastFactor)
	Push back many factors with an iterator over shared_ptr (factors are not copied).
HasDerivedValueType< ITERATOR >	push_back (ITERATOR firstFactor, ITERATOR lastFactor)
	Push back many factors with an iterator (factors are copied).
HasDerivedElementType< CONTAINER >	push_back (const CONTAINER &container)
	Push back many factors as shared_ptr's in a container (factors are not copied).
HasDerivedValueType< CONTAINER >	push_back (const CONTAINER &container)
	Push back non-pointer objects in a container (factors are copied).
void	add (const FACTOR_OR_CONTAINER &factorOrContainer)
	Add a factor or container of factors, including STL collections, BayesTrees, etc.
This &	operator+= (const FACTOR_OR_CONTAINER &factorOrContainer)
	Add a factor or container of factors, including STL collections, BayesTrees, etc.
std::enable_if< std::is_base_of< This, typenameCLIQUE::FactorGraphType >::value >::type	push_back (const BayesTree< CLIQUE > &bayesTree)
	Push back a BayesTree as a collection of factors.
FactorIndices	add_factors (const CONTAINER &factors, bool useEmptySlots=false)
	Add new factors to a factor graph and returns a list of new factor indices, optionally finding and reusing empty factor slots.
bool	equals (const This &fg, double tol=1e-9) const
	Check equality up to tolerance.
size_t	size () const
	return the number of factors (including any null factors set by remove() ).
bool	empty () const
	Check if the graph is empty (null factors set by remove() will cause this to return false).
const sharedFactor	at (size_t i) const
	Get a specific factor by index (this checks array bounds and may throw an exception, as opposed to operator[] which does not).
sharedFactor &	at (size_t i)
	Get a specific factor by index (this checks array bounds and may throw an exception, as opposed to operator[] which does not).
std::shared_ptr< F >	at (size_t i)
	Get a specific factor by index and typecast to factor type F (this checks array bounds and may throw an exception, as opposed to operator[] which does not).
const std::shared_ptr< F >	at (size_t i) const
	Const version of templated at method.
const sharedFactor	operator[] (size_t i) const
	Get a specific factor by index (this does not check array bounds, as opposed to at() which does).
sharedFactor &	operator[] (size_t i)
	Get a specific factor by index (this does not check array bounds, as opposed to at() which does).
const_iterator	begin () const
	Iterator to beginning of factors.
const_iterator	end () const
	Iterator to end of factors.
sharedFactor	front () const
	Get the first factor.
sharedFactor	back () const
	Get the last factor.
double	error (const HybridValues &values) const
	Add error for all factors.
iterator	begin ()
	non-const STL-style begin()
iterator	end ()
	non-const STL-style end()
virtual void	resize (size_t size)
	Directly resize the number of factors in the graph.
void	remove (size_t i)
	delete factor without re-arranging indexes by inserting a nullptr pointer
void	replace (size_t index, sharedFactor factor)
	replace a factor by index
iterator	erase (iterator item)
	Erase factor and rearrange other factors to take up the empty space.
iterator	erase (iterator first, iterator last)
	Erase factors and rearrange other factors to take up the empty space.
void	dot (std::ostream &os, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const
	Output to graphviz format, stream version.
std::string	dot (const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const
	Output to graphviz format string.
void	saveGraph (const std::string &filename, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const
	output to file with graphviz format.
size_t	nrFactors () const
	return the number of non-null factors
KeySet	keys () const
	Potentially slow function to return all keys involved, sorted, as a set.
KeyVector	keyVector () const
	Potentially slow function to return all keys involved, sorted, as a vector.
bool	exists (size_t idx) const
	MATLAB interface utility: Checks whether a factor index idx exists in the graph and is a live pointer.

Additional Inherited Members
Public Types inherited from gtsam::NonlinearFactorGraph
typedef FactorGraph< NonlinearFactor >	Base
typedef NonlinearFactorGraph	This
typedef std::shared_ptr< This >	shared_ptr
typedef std::function< void(const std::shared_ptr< HessianFactor > &hessianFactor)>	Dampen
	typdef for dampen functions used below
Public Types inherited from gtsam::FactorGraph< NonlinearFactor >
typedef NonlinearFactor	FactorType
	factor type
typedef std::shared_ptr< NonlinearFactor >	sharedFactor
	Shared pointer to a factor.
typedef sharedFactor	value_type
typedef FastVector< sharedFactor >::iterator	iterator
typedef FastVector< sharedFactor >::const_iterator	const_iterator
Protected Member Functions inherited from gtsam::FactorGraph< NonlinearFactor >
bool	isEqual (const FactorGraph &other) const
	Check exact equality of the factor pointers. Useful for derived ==.
	FactorGraph ()
	Default constructor.
	FactorGraph (ITERATOR firstFactor, ITERATOR lastFactor)
	Constructor from iterator over factors (shared_ptr or plain objects).
	FactorGraph (const CONTAINER &factors)
	Construct from container of factors (shared_ptr or plain objects).
Protected Attributes inherited from gtsam::FactorGraph< NonlinearFactor >
FastVector< sharedFactor >	factors_
	concept check, makes sure FACTOR defines print and equals

Constructor & Destructor Documentation

◆ WnoaFactorGraph()

template<typename PoseType>

gtsam::WnoaFactorGraph< PoseType >::WnoaFactorGraph	(	std::unordered_map< StateData, std::pair< StateData, StateData > >	interp_map,
		const VectorN	q_psd_diag,
		bool	fixed_noise_model = false )

Construct a WnoaFactorGraph with interpolation metadata.

Note that the interpolation information must be known a priori to properly construct the graph. It is therefore recommended to build a standard NonlinearFactorGraph first, then use the interpolateFactorGraph function. Adding factors to the graph after construction is not recommended, but if done, the user is responsible for ensuring that the interpolation metadata is consistent with the factors in the graph.

Parameters

interp_map	Mapping from each interpolated StateData to its left/right bordering estimated StateData.
q_psd_diag	Diagonal PSD vector for the WNOA interpolator (size must match PoseType dimension).
fixed_noise_model	If true, the graph will not augment measurement noise for interpolation.

Member Function Documentation

◆ cloneShared()

template<typename PoseType>

std::shared_ptr< const NonlinearFactorGraph > gtsam::WnoaFactorGraph< PoseType >::cloneShared ( ) const

inlineoverridevirtual

Clone into a shared pointer while preserving WnoaFactorGraph behavior.

Reimplemented from gtsam::NonlinearFactorGraph.

◆ error()

template<typename PoseType>

double gtsam::WnoaFactorGraph< PoseType >::error ( const Values & values ) const

overridevirtual

Compute the unnormalized graph error (sum of factor losses).

Computes the scalar error over all factors in the graph. When the graph contains interpolation wrapper factors this method uses the interpolator to evaluate interpolated states as part of the residual computation and can exploit precomputation to improve throughput.

Parameters

values Current Values used to evaluate the error.

Returns: double Scalar unnormalized error (sum of factor losses).

Reimplemented from gtsam::NonlinearFactorGraph.

◆ linearize()

template<typename PoseType>

std::shared_ptr< GaussianFactorGraph > gtsam::WnoaFactorGraph< PoseType >::linearize ( const Values & linearizationPoint ) const

overridevirtual

Linearize the graph into a GaussianFactorGraph.

This routine produces a linearized Gaussian factor graph evaluated at linearizationPoint. It exploits precomputed interpolation data and batching to reduce duplicated interpolation work across wrapper factors.

Parameters

linearizationPoint Values at which to linearize the nonlinear graph.

Returns: std::shared_ptr<GaussianFactorGraph> Linearized Gaussian factor graph.

Reimplemented from gtsam::NonlinearFactorGraph.

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

/tmp/gtsam-4.3.0-doxygen.rsXPUS/source/gtsam/nonlinear/WnoaFactorGraph.h
/tmp/gtsam-4.3.0-doxygen.rsXPUS/source/gtsam/nonlinear/WnoaFactorGraph.cpp

Detailed Description

Public Member Functions

Additional Inherited Members

Constructor & Destructor Documentation

◆ WnoaFactorGraph()

Member Function Documentation

◆ cloneShared()

◆ error()

◆ linearize()