#include <ExpressionFactorGraph.h>

Inheritance diagram for gtsam::ExpressionFactorGraph:

Collaboration diagram for gtsam::ExpressionFactorGraph:

Public Types
typedef FactorGraph< NonlinearFactor >	Base

typedef NonlinearFactorGraph	This

typedef std::shared_ptr< This >	shared_ptr

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

Public Member Functions
Adding Factors
template<typename T >
void	addExpressionFactor (const Expression< T > &h, const T &z, const SharedNoiseModel &R)

Testable
void	print (const std::string &str="NonlinearFactorGraph: ", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override

void	printErrors (const Values &values, const std::string &str="NonlinearFactorGraph: ", const KeyFormatter &keyFormatter=DefaultKeyFormatter, const std::function< bool(const Factor , double, size_t)> &printCondition=[](const Factor , double, size_t) {return true;}) const

bool	equals (const NonlinearFactorGraph &other, double tol=1e-9) const

Testable
bool	equals (const This &fg, double tol=1e-9) const
	Check equality up to tolerance.

Standard Interface
double	error (const HybridValues &values) const

size_t	size () const

bool	empty () const

const sharedFactor	at (size_t i) const

sharedFactor &	at (size_t i)

const sharedFactor	operator[] (size_t i) const

sharedFactor &	operator[] (size_t i)

const_iterator	begin () const

const_iterator	end () const

sharedFactor	front () const

sharedFactor	back () const

Graph Display
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.

Graph Display
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.

Adding Single Factors
void	reserve (size_t size)

IsDerived< DERIVEDFACTOR >	push_back (std::shared_ptr< DERIVEDFACTOR > factor)
	Add a factor directly using a shared_ptr.

IsDerived< DERIVEDFACTOR >	push_back (const DERIVEDFACTOR &factor)

IsDerived< DERIVEDFACTOR >	emplace_shared (Args &&... args)
	Emplace a shared pointer to factor of given type.

IsDerived< DERIVEDFACTOR >	add (std::shared_ptr< DERIVEDFACTOR > factor)
	`add` is a synonym for push_back.

Adding via iterators
HasDerivedElementType< ITERATOR >	push_back (ITERATOR firstFactor, ITERATOR lastFactor)

HasDerivedValueType< ITERATOR >	push_back (ITERATOR firstFactor, ITERATOR lastFactor)
	Push back many factors with an iterator (factors are copied)

Adding via container
HasDerivedElementType< CONTAINER >	push_back (const CONTAINER &container)

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)

Specialized versions
std::enable_if< std::is_base_of< This, typename CLIQUE::FactorGraphType >::value >::type	push_back (const BayesTree< CLIQUE > &bayesTree)

FactorIndices	add_factors (const CONTAINER &factors, bool useEmptySlots=false)

Modifying Factor Graphs (imperative, discouraged)
iterator	begin ()

iterator	end ()

virtual void	resize (size_t size)

void	remove (size_t i)

void	replace (size_t index, sharedFactor factor)

iterator	erase (iterator item)

iterator	erase (iterator first, iterator last)

Advanced Interface
size_t	nrFactors () const

KeySet	keys () const

KeyVector	keyVector () const

bool	exists (size_t idx) const

Protected Member Functions
bool	isEqual (const FactorGraph &other) const
	Check exact equality of the factor pointers. Useful for derived ==.

Protected Attributes
FastVector< sharedFactor >	factors_

Standard Interface
double	error (const Values &values) const

double	probPrime (const Values &values) const

std::shared_ptr< SymbolicFactorGraph >	symbolic () const

Ordering	orderingCOLAMD () const

Ordering	orderingCOLAMDConstrained (const FastMap< Key, int > &constraints) const

std::shared_ptr< GaussianFactorGraph >	linearize (const Values &linearizationPoint) const
	Linearize a nonlinear factor graph.

std::shared_ptr< HessianFactor >	linearizeToHessianFactor (const Values &values, const Dampen &dampen=nullptr) const

std::shared_ptr< HessianFactor >	linearizeToHessianFactor (const Values &values, const Ordering &ordering, const Dampen &dampen=nullptr) const

Values	updateCholesky (const Values &values, const Dampen &dampen=nullptr) const

Values	updateCholesky (const Values &values, const Ordering &ordering, const Dampen &dampen=nullptr) const

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

template<typename T >
void	addExpressionFactor (const SharedNoiseModel &R, const T &z, const Expression< T > &h)

template<typename T >
void	addPrior (Key key, const T &prior, const SharedNoiseModel &model=nullptr)

template<typename T >
void	addPrior (Key key, const T &prior, const Matrix &covariance)

typedef std::function< void(const std::shared_ptr< HessianFactor > &hessianFactor)>	Dampen
	typdef for dampen functions used below

Detailed Description

Factor graph that supports adding ExpressionFactors directly

Member Function Documentation

◆ add()

void gtsam::FactorGraph< NonlinearFactor >::add ( const FACTOR_OR_CONTAINER & factorOrContainer )

inlineinherited

Add a factor or container of factors, including STL collections, BayesTrees, etc.

◆ add_factors()

FactorIndices gtsam::FactorGraph< NonlinearFactor >::add_factors	(	const CONTAINER &	factors,
		bool	useEmptySlots = `false`
	)

inherited

Add new factors to a factor graph and returns a list of new factor indices, optionally finding and reusing empty factor slots.

◆ addExpressionFactor() [1/2]

template<typename T >

void gtsam::ExpressionFactorGraph::addExpressionFactor	(	const Expression< T > &	h,
		const T &	z,
		const SharedNoiseModel &	R
	)

inline

Directly add ExpressionFactor that implements |h(x)-z|^2_R

Parameters

h	expression that implements measurement function
z	measurement
R	model

◆ addExpressionFactor() [2/2]

template<typename T >

void gtsam::NonlinearFactorGraph::addExpressionFactor	(	const SharedNoiseModel &	R,
		const T &	z,
		const Expression< T > &	h
	)

inlineinherited

Directly add ExpressionFactor that implements |h(x)-z|^2_R

Parameters

h	expression that implements measurement function
z	measurement
R	model

◆ addPrior() [1/2]

template<typename T >

void gtsam::NonlinearFactorGraph::addPrior	(	Key	key,
		const T &	prior,
		const SharedNoiseModel &	model = `nullptr`
	)

inlineinherited

Convenience method which adds a PriorFactor to the factor graph.

Parameters

key	Variable key
prior	The variable's prior value
model	Noise model for prior factor

◆ addPrior() [2/2]

template<typename T >

void gtsam::NonlinearFactorGraph::addPrior	(	Key	key,
		const T &	prior,
		const Matrix &	covariance
	)

inlineinherited

Convenience method which adds a PriorFactor to the factor graph.

Parameters

key	Variable key
prior	The variable's prior value
covariance	Covariance matrix.

Note that the smart noise model associated with the prior factor automatically picks the right noise model (e.g. a diagonal noise model if the provided covariance matrix is diagonal).

◆ at() [1/2]

const sharedFactor gtsam::FactorGraph< NonlinearFactor >::at ( size_t i ) const

inlineinherited

Get a specific factor by index (this checks array bounds and may throw an exception, as opposed to operator[] which does not).

◆ at() [2/2]

sharedFactor& gtsam::FactorGraph< NonlinearFactor >::at ( size_t i )

inlineinherited

Get a specific factor by index (this checks array bounds and may throw an exception, as opposed to operator[] which does not).

◆ back()

sharedFactor gtsam::FactorGraph< NonlinearFactor >::back ( ) const

inlineinherited

Get the last factor

◆ begin() [1/2]

const_iterator gtsam::FactorGraph< NonlinearFactor >::begin ( ) const

inlineinherited

Iterator to beginning of factors.

◆ begin() [2/2]

iterator gtsam::FactorGraph< NonlinearFactor >::begin ( )

inlineinherited

non-const STL-style begin()

◆ empty()

bool gtsam::FactorGraph< NonlinearFactor >::empty ( ) const

inlineinherited

Check if the graph is empty (null factors set by remove() will cause this to return false).

◆ end() [1/2]

const_iterator gtsam::FactorGraph< NonlinearFactor >::end ( ) const

inlineinherited

Iterator to end of factors.

◆ end() [2/2]

iterator gtsam::FactorGraph< NonlinearFactor >::end ( )

inlineinherited

non-const STL-style end()

◆ equals()

bool gtsam::NonlinearFactorGraph::equals	(	const NonlinearFactorGraph &	other,
		double	tol = `1e-9`
	)		const

inherited

Test equality

◆ erase() [1/2]

iterator gtsam::FactorGraph< NonlinearFactor >::erase ( iterator item )

inlineinherited

Erase factor and rearrange other factors to take up the empty space

◆ erase() [2/2]

iterator gtsam::FactorGraph< NonlinearFactor >::erase	(	iterator	first,
		iterator	last
	)

inlineinherited

Erase factors and rearrange other factors to take up the empty space

◆ error() [1/2]

double gtsam::NonlinearFactorGraph::error ( const Values & values ) const

inherited

unnormalized error, \( \sum_i 0.5 (h_i(X_i)-z)^2 / \sigma^2 \) in the most common case

◆ error() [2/2]

double gtsam::FactorGraph< NonlinearFactor >::error ( const HybridValues & values ) const

inherited

Add error for all factors.

◆ exists()

bool gtsam::FactorGraph< NonlinearFactor >::exists ( size_t idx ) const

inlineinherited

MATLAB interface utility: Checks whether a factor index idx exists in the graph and is a live pointer

◆ front()

sharedFactor gtsam::FactorGraph< NonlinearFactor >::front ( ) const

inlineinherited

Get the first factor

◆ keys()

KeySet gtsam::FactorGraph< NonlinearFactor >::keys ( ) const

inherited

Potentially slow function to return all keys involved, sorted, as a set

◆ keyVector()

KeyVector gtsam::FactorGraph< NonlinearFactor >::keyVector ( ) const

inherited

Potentially slow function to return all keys involved, sorted, as a vector

◆ linearizeToHessianFactor() [1/2]

std::shared_ptr<HessianFactor> gtsam::NonlinearFactorGraph::linearizeToHessianFactor	(	const Values &	values,
		const Dampen &	dampen = `nullptr`
	)		const

inherited

Instead of producing a GaussianFactorGraph, pre-allocate and linearize directly into a HessianFactor. Avoids the many mallocs and pointer indirection in constructing a new graph, and hence useful in case a dense solve is appropriate for your problem. An optional lambda function can be used to apply damping on the filled Hessian. No parallelism is exploited, because all the factors write in the same memory.

◆ linearizeToHessianFactor() [2/2]

std::shared_ptr<HessianFactor> gtsam::NonlinearFactorGraph::linearizeToHessianFactor	(	const Values &	values,
		const Ordering &	ordering,
		const Dampen &	dampen = `nullptr`
	)		const

inherited

Instead of producing a GaussianFactorGraph, pre-allocate and linearize directly into a HessianFactor. Avoids the many mallocs and pointer indirection in constructing a new graph, and hence useful in case a dense solve is appropriate for your problem. An ordering is given that still decides how the Hessian is laid out. An optional lambda function can be used to apply damping on the filled Hessian. No parallelism is exploited, because all the factors write in the same memory.

◆ nrFactors()

size_t gtsam::FactorGraph< NonlinearFactor >::nrFactors ( ) const

inherited

return the number of non-null factors

◆ operator[]() [1/2]

const sharedFactor gtsam::FactorGraph< NonlinearFactor >::operator[] ( size_t i ) const

inlineinherited

Get a specific factor by index (this does not check array bounds, as opposed to at() which does).

◆ operator[]() [2/2]

sharedFactor& gtsam::FactorGraph< NonlinearFactor >::operator[] ( size_t i )

inlineinherited

Get a specific factor by index (this does not check array bounds, as opposed to at() which does).

◆ orderingCOLAMD()

Ordering gtsam::NonlinearFactorGraph::orderingCOLAMD ( ) const

inherited

Compute a fill-reducing ordering using COLAMD.

◆ orderingCOLAMDConstrained()

Ordering gtsam::NonlinearFactorGraph::orderingCOLAMDConstrained ( const FastMap< Key, int > & constraints ) const

inherited

Compute a fill-reducing ordering with constraints using CCOLAMD

Parameters

constraints is a map of Key->group, where 0 is unconstrained, and higher group numbers are further back in the ordering. Only keys with nonzero group indices need to appear in the constraints, unconstrained is assumed for all other variables

◆ print()

void gtsam::NonlinearFactorGraph::print	(	const std::string &	str = `"NonlinearFactorGraph: "`,
		const KeyFormatter &	keyFormatter = `DefaultKeyFormatter`
	)		const

overridevirtualinherited

print

Reimplemented from gtsam::FactorGraph< NonlinearFactor >.

◆ printErrors()

void gtsam::NonlinearFactorGraph::printErrors	(	const Values &	values,
		const std::string &	str = `"NonlinearFactorGraph: "`,
		const KeyFormatter &	keyFormatter = `DefaultKeyFormatter`,
		const std::function< bool(const Factor *, double, size_t)> &	printCondition = `[](const Factor *, double, size_t) {return true;}`
	)		const

inherited

print errors along with factors

◆ probPrime()

double gtsam::NonlinearFactorGraph::probPrime ( const Values & values ) const

inherited

Unnormalized probability. O(n)

◆ push_back() [1/4]

IsDerived<DERIVEDFACTOR> gtsam::FactorGraph< NonlinearFactor >::push_back ( const DERIVEDFACTOR & factor )

inlineinherited

Add a factor by value, will be copy-constructed (use push_back with a shared_ptr to avoid the copy).

◆ push_back() [2/4]

HasDerivedElementType<ITERATOR> gtsam::FactorGraph< NonlinearFactor >::push_back	(	ITERATOR	firstFactor,
		ITERATOR	lastFactor
	)

inlineinherited

Push back many factors with an iterator over shared_ptr (factors are not copied)

◆ push_back() [3/4]

HasDerivedElementType<CONTAINER> gtsam::FactorGraph< NonlinearFactor >::push_back ( const CONTAINER & container )

inlineinherited

Push back many factors as shared_ptr's in a container (factors are not copied)

◆ push_back() [4/4]

std::enable_if< std::is_base_of<This, typename CLIQUE::FactorGraphType>::value>::type gtsam::FactorGraph< NonlinearFactor >::push_back ( const BayesTree< CLIQUE > & bayesTree )

inlineinherited

Push back a BayesTree as a collection of factors. NOTE: This should be hidden in derived classes in favor of a type-specialized version that calls this templated function.

◆ rekey()

NonlinearFactorGraph gtsam::NonlinearFactorGraph::rekey ( const std::map< Key, Key > & rekey_mapping ) const

inherited

Rekey() performs a deep-copy of all of the factors, and changes keys according to a mapping.

Keys not specified in the mapping will remain unchanged.

Parameters

rekey_mapping is a map of old->new keys

Returns: a cloned graph with updated keys

◆ remove()

void gtsam::FactorGraph< NonlinearFactor >::remove ( size_t i )

inlineinherited

delete factor without re-arranging indexes by inserting a nullptr pointer

◆ replace()

void gtsam::FactorGraph< NonlinearFactor >::replace	(	size_t	index,
		sharedFactor	factor
	)

inlineinherited

replace a factor by index

◆ reserve()

void gtsam::FactorGraph< NonlinearFactor >::reserve ( size_t size )

inlineinherited

Reserve space for the specified number of factors if you know in advance how many there will be (works like FastVector::reserve).

◆ resize()

virtual void gtsam::FactorGraph< NonlinearFactor >::resize ( size_t size )

inlinevirtualinherited

Directly resize the number of factors in the graph. If the new size is less than the original, factors at the end will be removed. If the new size is larger than the original, null factors will be appended.

◆ size()

size_t gtsam::FactorGraph< NonlinearFactor >::size ( ) const

inlineinherited

return the number of factors (including any null factors set by remove() ).

◆ symbolic()

std::shared_ptr<SymbolicFactorGraph> gtsam::NonlinearFactorGraph::symbolic ( ) const

inherited

Create a symbolic factor graph

◆ updateCholesky() [1/2]

Values gtsam::NonlinearFactorGraph::updateCholesky	(	const Values &	values,
		const Dampen &	dampen = `nullptr`
	)		const

inherited

Linearize and solve in one pass. Calls linearizeToHessianFactor, densely solves the normal equations, and updates the values.

◆ updateCholesky() [2/2]

Values gtsam::NonlinearFactorGraph::updateCholesky	(	const Values &	values,
		const Ordering &	ordering,
		const Dampen &	dampen = `nullptr`
	)		const

inherited

Linearize and solve in one pass. Calls linearizeToHessianFactor, densely solves the normal equations, and updates the values.

Member Data Documentation

◆ factors_

FastVector<sharedFactor> gtsam::FactorGraph< NonlinearFactor >::factors_

protectedinherited

concept check, makes sure FACTOR defines print and equals Collection of factors

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

/home/docs/checkouts/readthedocs.org/user_builds/gtsam-jlblanco-docs/checkouts/latest/gtsam/nonlinear/ExpressionFactorGraph.h

Public Types

Public Member Functions

Protected Member Functions

Protected Attributes

Standard Interface

Detailed Description

Member Function Documentation

◆ add()

◆ add_factors()

◆ addExpressionFactor() [1/2]

◆ addExpressionFactor() [2/2]

◆ addPrior() [1/2]

◆ addPrior() [2/2]

◆ at() [1/2]

◆ at() [2/2]

◆ back()

◆ begin() [1/2]

◆ begin() [2/2]

◆ empty()

◆ end() [1/2]

◆ end() [2/2]

◆ equals()

◆ erase() [1/2]

◆ erase() [2/2]

◆ error() [1/2]

◆ error() [2/2]

◆ exists()

◆ front()

◆ keys()

◆ keyVector()

◆ linearizeToHessianFactor() [1/2]

◆ linearizeToHessianFactor() [2/2]

◆ nrFactors()

◆ operator[]() [1/2]

◆ operator[]() [2/2]

◆ orderingCOLAMD()

◆ orderingCOLAMDConstrained()

◆ print()

◆ printErrors()

◆ probPrime()

◆ push_back() [1/4]

◆ push_back() [2/4]

◆ push_back() [3/4]

◆ push_back() [4/4]

◆ rekey()

◆ remove()

◆ replace()

◆ reserve()

◆ resize()

◆ size()

◆ symbolic()

◆ updateCholesky() [1/2]

◆ updateCholesky() [2/2]

Member Data Documentation

◆ factors_