Detailed Description

A non-linear factor graph is a graph of non-Gaussian, i.e.

non-linear factors, which derive from NonlinearFactor. The values structures are typically (in SAM) more general than just vectors, e.g., Rot3 or Pose3, which are objects in non-linear manifolds. Linearizing the non-linear factor graph creates a linear factor graph on the tangent vector space at the linearization point. Because the tangent space is a true vector space, the config type will be an VectorValues in that linearized factor graph.

Inheritance diagram for gtsam::NonlinearFactorGraph:

Public Member Functions
	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.

virtual	~NonlinearFactorGraph ()
	Destructor.

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

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
	print errors along with factors

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

void	saveGraph (std::ostream &stm, const Values &values=Values(), const GraphvizFormatting &graphvizFormatting=GraphvizFormatting(), const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
	Write the graph in GraphViz format for visualization.

void	saveGraph (const std::string &file, const Values &values=Values(), const GraphvizFormatting &graphvizFormatting=GraphvizFormatting(), const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
	Write the graph in GraphViz format to file for visualization. More...

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

double	probPrime (const Values &values) const
	Unnormalized probability. More...

boost::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. More...

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

boost::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. More...

boost::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. More...

Values	updateCholesky (const Values &values, const Dampen &dampen=nullptr) const
	Linearize and solve in one pass. More...

Values	updateCholesky (const Values &values, const Ordering &ordering, const Dampen &dampen=nullptr) const
	Linearize and solve in one pass. More...

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

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

template<typename T >
void	addPrior (Key key, const T &prior, const SharedNoiseModel &model=nullptr)
	Convenience method which adds a PriorFactor to the factor graph. More...

template<typename T >
void	addPrior (Key key, const T &prior, const Matrix &covariance)
	Convenience method which adds a PriorFactor to the factor graph. More...

Public Member Functions inherited from gtsam::FactorGraph< NonlinearFactor >
virtual	~FactorGraph ()=default
	Default destructor.

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 (boost::shared_ptr< DERIVEDFACTOR > factor)
	Add a factor directly using a shared_ptr.

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 >	emplace_shared (Args &&... args)
	Emplace a shared pointer to factor of given type.

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

std::enable_if< std::is_base_of< FactorType, DERIVEDFACTOR >::value, boost::assign::list_inserter< RefCallPushBack< This > > >::type	operator+= (boost::shared_ptr< DERIVEDFACTOR > factor)
	`+=` works well with boost::assign list inserter.

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.

boost::assign::list_inserter< CRefCallPushBack< 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. More...

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.

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).

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.

iterator	begin ()
	non-const STL-style begin()

iterator	end ()
	non-const STL-style end()

void	resize (size_t size)
	Directly resize the number of factors in the graph. More...

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.

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.

Public Types
typedef FactorGraph< NonlinearFactor >	Base

typedef NonlinearFactorGraph	This

typedef boost::shared_ptr< This >	shared_ptr

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

Public Types inherited from gtsam::FactorGraph< NonlinearFactor >
typedef NonlinearFactor	FactorType
	factor type

typedef boost::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

Friends
class	boost::serialization::access
	Serialization function.

Additional Inherited Members
Protected Member Functions inherited from gtsam::FactorGraph< NonlinearFactor >
	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 More...

Member Function Documentation

◆ addExpressionFactor()

template<typename T >

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

inline

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 Matrix &	covariance
	)

inline

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).

◆ addPrior() [2/2]

template<typename T >

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

inline

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

◆ linearizeToHessianFactor() [1/2]

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

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]

HessianFactor::shared_ptr gtsam::NonlinearFactorGraph::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.

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.

◆ orderingCOLAMDConstrained()

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

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

overridevirtual

print

Reimplemented from gtsam::FactorGraph< NonlinearFactor >.

◆ probPrime()

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

Unnormalized probability.

O(n)

◆ rekey()

NonlinearFactorGraph gtsam::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.

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

◆ saveGraph()

void gtsam::NonlinearFactorGraph::saveGraph	(	const std::string &	file,
		const Values &	values = `Values()`,
		const GraphvizFormatting &	graphvizFormatting = `GraphvizFormatting()`,
		const KeyFormatter &	keyFormatter = `DefaultKeyFormatter`
	)		const

Write the graph in GraphViz format to file for visualization.

This is a wrapper friendly version since wrapped languages don't have access to C++ streams.

◆ updateCholesky() [1/2]

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

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

Linearize and solve in one pass.

Calls linearizeToHessianFactor, densely solves the normal equations, and updates the values.

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

/Users/dellaert/git/github/gtsam/nonlinear/NonlinearFactorGraph.h
/Users/dellaert/git/github/gtsam/nonlinear/NonlinearFactorGraph.cpp

Detailed Description

Public Member Functions

Public Types

Friends

Additional Inherited Members

Member Function Documentation

◆ addExpressionFactor()

◆ addPrior() [1/2]

◆ addPrior() [2/2]

◆ linearizeToHessianFactor() [1/2]

◆ linearizeToHessianFactor() [2/2]

◆ orderingCOLAMDConstrained()

◆ print()

◆ probPrime()

◆ rekey()

◆ saveGraph()

◆ updateCholesky() [1/2]

◆ updateCholesky() [2/2]