#include <NonlinearFactorGraph.h>

Inheritance diagram for gtsam::NonlinearFactorGraph:

[legend]

Public Types
typedef FactorGraph< NonlinearFactor >	Base

typedef std::shared_ptr< This >	shared_ptr

typedef NonlinearFactorGraph	This

Public Types inherited from gtsam::FactorGraph< NonlinearFactor >
typedef FastVector< sharedFactor >::const_iterator	const_iterator

typedef NonlinearFactor	FactorType
	factor type More...

typedef FastVector< sharedFactor >::iterator	iterator

typedef std::shared_ptr< NonlinearFactor >	sharedFactor
	Shared pointer to a factor. More...

typedef sharedFactor	value_type

Public Member Functions
Standard Constructors
	NonlinearFactorGraph ()

template<typename ITERATOR >
	NonlinearFactorGraph (ITERATOR firstFactor, ITERATOR lastFactor)

template<class CONTAINER >
	NonlinearFactorGraph (const CONTAINER &factors)

template<class DERIVEDFACTOR >
	NonlinearFactorGraph (const FactorGraph< DERIVEDFACTOR > &graph)

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

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

std::string	dot (const Values &values, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const GraphvizFormatting &writer=GraphvizFormatting()) const
	Output to graphviz format string, with Values/extra options. More...

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

void	dot (std::ostream &os, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const
	Output to graphviz format, stream version. More...

std::string	dot (const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const
	Output to graphviz format string. More...

void	saveGraph (const std::string &filename, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const
	output to file with graphviz format. More...

Public Member Functions inherited from gtsam::FactorGraph< NonlinearFactor >
	FactorGraph (std::initializer_list< std::shared_ptr< DERIVEDFACTOR >> sharedFactors)

virtual	~FactorGraph ()=default

void	reserve (size_t size)

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

IsDerived< DERIVEDFACTOR >	push_back (const DERIVEDFACTOR &factor)

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

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

std::enable_if< std::is_base_of< FactorType, DERIVEDFACTOR >::value, This >::type &	operator+= (std::shared_ptr< DERIVEDFACTOR > factor)
	Append factor to factor graph. More...

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

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

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

void	add (const FACTOR_OR_CONTAINER &factorOrContainer)

This &	operator+= (const FACTOR_OR_CONTAINER &factorOrContainer)

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)

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

size_t	size () const

bool	empty () const

const sharedFactor	at (size_t i) const

sharedFactor &	at (size_t i)

std::shared_ptr< F >	at (size_t i)

const std::shared_ptr< F >	at (size_t i) const
	Const version of templated `at` method. More...

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

double	error (const HybridValues &values) const

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)

void	dot (std::ostream &os, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const
	Output to graphviz format, stream version. More...

std::string	dot (const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const
	Output to graphviz format string. More...

void	saveGraph (const std::string &filename, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const
	output to file with graphviz format. More...

size_t	nrFactors () const

KeySet	keys () const

KeyVector	keyVector () const

bool	exists (size_t idx) const

Private Member Functions
std::shared_ptr< HessianFactor >	linearizeToHessianFactor (const Values &values, const Scatter &scatter, const Dampen &dampen=nullptr) const

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

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

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

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)

Additional Inherited Members
Protected Member Functions inherited from gtsam::FactorGraph< NonlinearFactor >
bool	isEqual (const FactorGraph &other) const
	Check exact equality of the factor pointers. Useful for derived ==. More...

	FactorGraph ()

	FactorGraph (ITERATOR firstFactor, ITERATOR lastFactor)

	FactorGraph (const CONTAINER &factors)

Protected Attributes inherited from gtsam::FactorGraph< NonlinearFactor >
FastVector< sharedFactor >	factors_

Detailed Description

Definition at line 55 of file NonlinearFactorGraph.h.

Member Typedef Documentation

◆ Base

typedef FactorGraph<NonlinearFactor> gtsam::NonlinearFactorGraph::Base

Definition at line 59 of file NonlinearFactorGraph.h.

◆ Dampen

typedef std::function<void(const std::shared_ptr<HessianFactor>& hessianFactor)> gtsam::NonlinearFactorGraph::Dampen

typdef for dampen functions used below

Definition at line 133 of file NonlinearFactorGraph.h.

◆ shared_ptr

typedef std::shared_ptr<This> gtsam::NonlinearFactorGraph::shared_ptr

Definition at line 61 of file NonlinearFactorGraph.h.

◆ This

typedef NonlinearFactorGraph gtsam::NonlinearFactorGraph::This

Definition at line 60 of file NonlinearFactorGraph.h.

Constructor & Destructor Documentation

◆ NonlinearFactorGraph() [1/4]

gtsam::NonlinearFactorGraph::NonlinearFactorGraph ( )

inline

Default constructor

Definition at line 67 of file NonlinearFactorGraph.h.

◆ NonlinearFactorGraph() [2/4]

template<typename ITERATOR >

gtsam::NonlinearFactorGraph::NonlinearFactorGraph	(	ITERATOR	firstFactor,
		ITERATOR	lastFactor
	)

inline

Construct from iterator over factors

Definition at line 71 of file NonlinearFactorGraph.h.

◆ NonlinearFactorGraph() [3/4]

template<class CONTAINER >

gtsam::NonlinearFactorGraph::NonlinearFactorGraph ( const CONTAINER & factors )

inlineexplicit

Construct from container of factors (shared_ptr or plain objects)

Definition at line 75 of file NonlinearFactorGraph.h.

◆ NonlinearFactorGraph() [4/4]

template<class DERIVEDFACTOR >

gtsam::NonlinearFactorGraph::NonlinearFactorGraph ( const FactorGraph< DERIVEDFACTOR > & graph )

inline

Implicit copy/downcast constructor to override explicit template container constructor

Definition at line 79 of file NonlinearFactorGraph.h.

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

Definition at line 187 of file NonlinearFactorGraph.h.

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

Definition at line 215 of file NonlinearFactorGraph.h.

◆ 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

Definition at line 199 of file NonlinearFactorGraph.h.

◆ clone()

NonlinearFactorGraph gtsam::NonlinearFactorGraph::clone ( ) const

Clone() performs a deep-copy of the graph, including all of the factors.

Definition at line 372 of file NonlinearFactorGraph.cpp.

◆ dot() [1/4]

std::string gtsam::FactorGraph< FACTOR >::dot

Output to graphviz format string.

Definition at line 168 of file FactorGraph-inst.h.

◆ dot() [2/4]

std::string gtsam::NonlinearFactorGraph::dot	(	const Values &	values,
		const KeyFormatter &	keyFormatter = `DefaultKeyFormatter`,
		const GraphvizFormatting &	writer = `GraphvizFormatting()`
	)		const

Output to graphviz format string, with Values/extra options.

Definition at line 151 of file NonlinearFactorGraph.cpp.

◆ dot() [3/4]

void gtsam::FactorGraph< FACTOR >::dot

Output to graphviz format, stream version.

Definition at line 141 of file FactorGraph-inst.h.

◆ dot() [4/4]

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

Definition at line 102 of file NonlinearFactorGraph.cpp.

◆ equals()

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

Test equality

Definition at line 97 of file NonlinearFactorGraph.cpp.

◆ error()

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

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

Definition at line 170 of file NonlinearFactorGraph.cpp.

◆ linearize()

GaussianFactorGraph::shared_ptr gtsam::NonlinearFactorGraph::linearize ( const Values & linearizationPoint ) const

Linearize a nonlinear factor graph.

Definition at line 239 of file NonlinearFactorGraph.cpp.

◆ linearizeToHessianFactor() [1/3]

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.

Definition at line 344 of file NonlinearFactorGraph.cpp.

◆ linearizeToHessianFactor() [2/3]

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.

Definition at line 335 of file NonlinearFactorGraph.cpp.

◆ linearizeToHessianFactor() [3/3]

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

private

Linearize from Scatter rather than from Ordering. Made private because it doesn't include gttic.

Definition at line 312 of file NonlinearFactorGraph.cpp.

◆ orderingCOLAMD()

Ordering gtsam::NonlinearFactorGraph::orderingCOLAMD ( ) const

Compute a fill-reducing ordering using COLAMD.

Definition at line 182 of file NonlinearFactorGraph.cpp.

◆ 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

Definition at line 188 of file NonlinearFactorGraph.cpp.

◆ print()

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

overridevirtual

print

Reimplemented from gtsam::FactorGraph< NonlinearFactor >.

Definition at line 55 of file NonlinearFactorGraph.cpp.

◆ 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

print errors along with factors

Definition at line 71 of file NonlinearFactorGraph.cpp.

◆ probPrime()

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

Unnormalized probability. O(n)

Definition at line 49 of file NonlinearFactorGraph.cpp.

◆ 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

Definition at line 384 of file NonlinearFactorGraph.cpp.

◆ saveGraph() [1/2]

void gtsam::FactorGraph< FACTOR >::saveGraph

output to file with graphviz format.

Definition at line 177 of file FactorGraph-inst.h.

◆ saveGraph() [2/2]

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

Definition at line 160 of file NonlinearFactorGraph.cpp.

◆ symbolic()

SymbolicFactorGraph::shared_ptr gtsam::NonlinearFactorGraph::symbolic ( ) const

Create a symbolic factor graph

Definition at line 194 of file NonlinearFactorGraph.cpp.

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

Definition at line 353 of file NonlinearFactorGraph.cpp.

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

Definition at line 362 of file NonlinearFactorGraph.cpp.

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

Public Types

Public Member Functions

Private Member Functions

Standard Interface

Additional Inherited Members

Detailed Description

Member Typedef Documentation

◆ Base

◆ Dampen

◆ shared_ptr

◆ This

Constructor & Destructor Documentation

◆ NonlinearFactorGraph() [1/4]

◆ NonlinearFactorGraph() [2/4]

◆ NonlinearFactorGraph() [3/4]

◆ NonlinearFactorGraph() [4/4]

Member Function Documentation

◆ addExpressionFactor()

◆ addPrior() [1/2]

◆ addPrior() [2/2]

◆ clone()

◆ dot() [1/4]

◆ dot() [2/4]

◆ dot() [3/4]

◆ dot() [4/4]

◆ equals()

◆ error()

◆ linearize()

◆ linearizeToHessianFactor() [1/3]

◆ linearizeToHessianFactor() [2/3]

◆ linearizeToHessianFactor() [3/3]

◆ orderingCOLAMD()

◆ orderingCOLAMDConstrained()

◆ print()

◆ printErrors()

◆ probPrime()

◆ rekey()

◆ saveGraph() [1/2]

◆ saveGraph() [2/2]

◆ symbolic()

◆ updateCholesky() [1/2]

◆ updateCholesky() [2/2]