#include <NonlinearFactorGraph.h>

Inheritance diagram for gtsam::NonlinearFactorGraph:

[legend]

Public Types
typedef FactorGraph< NonlinearFactor >	Base

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

typedef boost::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 boost::shared_ptr< NonlinearFactor >	sharedFactor
	Shared pointer to a factor. More...

typedef sharedFactor	value_type

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

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

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

double	error (const Values &values) const

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

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

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

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

	NonlinearFactorGraph ()

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

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

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

Ordering	orderingCOLAMD () const

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

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

double	probPrime (const Values &values) const

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

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

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

boost::shared_ptr< SymbolicFactorGraph >	symbolic () const

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

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

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

virtual	~NonlinearFactorGraph ()
	Destructor. More...

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

void	reserve (size_t size)

IsDerived< DERIVEDFACTOR >	push_back (boost::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 (boost::shared_ptr< DERIVEDFACTOR > factor)
	`add` is a synonym for push_back. More...

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

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

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

iterator	begin ()

iterator	end ()

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)

size_t	nrFactors () const

KeySet	keys () const

KeyVector	keyVector () const

bool	exists (size_t idx) const

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

template<class ARCHIVE >
void	serialize (ARCHIVE &ar, const unsigned int)

Friends
class	boost::serialization::access

Additional Inherited Members
Protected Member Functions inherited from gtsam::FactorGraph< NonlinearFactor >
	FactorGraph ()

	FactorGraph (ITERATOR firstFactor, ITERATOR lastFactor)

	FactorGraph (const CONTAINER &factors)

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

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.

Definition at line 78 of file NonlinearFactorGraph.h.

Member Typedef Documentation

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

Definition at line 82 of file NonlinearFactorGraph.h.

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

typdef for dampen functions used below

Definition at line 163 of file NonlinearFactorGraph.h.

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

Definition at line 84 of file NonlinearFactorGraph.h.

typedef NonlinearFactorGraph gtsam::NonlinearFactorGraph::This

Definition at line 83 of file NonlinearFactorGraph.h.

Constructor & Destructor Documentation

gtsam::NonlinearFactorGraph::NonlinearFactorGraph ( )

inline

Default constructor

Definition at line 87 of file NonlinearFactorGraph.h.

template<typename ITERATOR >

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

inline

Construct from iterator over factors

Definition at line 91 of file NonlinearFactorGraph.h.

template<class CONTAINER >

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

inlineexplicit

Construct from container of factors (shared_ptr or plain objects)

Definition at line 95 of file NonlinearFactorGraph.h.

template<class DERIVEDFACTOR >

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

inline

Implicit copy/downcast constructor to override explicit template container constructor

Definition at line 99 of file NonlinearFactorGraph.h.

virtual gtsam::NonlinearFactorGraph::~NonlinearFactorGraph ( )

inlinevirtual

Destructor.

Definition at line 102 of file NonlinearFactorGraph.h.

Member Function Documentation

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 217 of file NonlinearFactorGraph.h.

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 229 of file NonlinearFactorGraph.h.

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 245 of file NonlinearFactorGraph.h.

NonlinearFactorGraph gtsam::NonlinearFactorGraph::clone ( ) const

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

Definition at line 463 of file NonlinearFactorGraph.cpp.

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

Test equality

Definition at line 89 of file NonlinearFactorGraph.cpp.

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

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

Definition at line 271 of file NonlinearFactorGraph.cpp.

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

Linearize a nonlinear factor graph.

Definition at line 340 of file NonlinearFactorGraph.cpp.

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 435 of file NonlinearFactorGraph.cpp.

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 426 of file NonlinearFactorGraph.cpp.

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 403 of file NonlinearFactorGraph.cpp.

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

inline

Deprecated:

Definition at line 269 of file NonlinearFactorGraph.h.

Ordering gtsam::NonlinearFactorGraph::orderingCOLAMD ( ) const

Compute a fill-reducing ordering using COLAMD.

Definition at line 283 of file NonlinearFactorGraph.cpp.

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 289 of file NonlinearFactorGraph.cpp.

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

overridevirtual

print

Reimplemented from gtsam::FactorGraph< NonlinearFactor >.

Definition at line 53 of file NonlinearFactorGraph.cpp.

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 64 of file NonlinearFactorGraph.cpp.

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

Unnormalized probability. O(n)

Definition at line 48 of file NonlinearFactorGraph.cpp.

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 475 of file NonlinearFactorGraph.cpp.

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

Definition at line 94 of file NonlinearFactorGraph.cpp.

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.

Definition at line 261 of file NonlinearFactorGraph.cpp.

template<class ARCHIVE >

void gtsam::NonlinearFactorGraph::serialize	(	ARCHIVE &	ar,
		const unsigned	int
	)

inlineprivate

Definition at line 261 of file NonlinearFactorGraph.h.

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

Create a symbolic factor graph

Definition at line 295 of file NonlinearFactorGraph.cpp.

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 444 of file NonlinearFactorGraph.cpp.

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 453 of file NonlinearFactorGraph.cpp.

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

inline

Deprecated:

Definition at line 274 of file NonlinearFactorGraph.h.

Friends And Related Function Documentation

friend class boost::serialization::access

friend

Serialization function

Definition at line 259 of file NonlinearFactorGraph.h.

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

Public Types

Public Member Functions

Private Member Functions

Friends

Additional Inherited Members

Detailed Description

Member Typedef Documentation

Constructor & Destructor Documentation

Member Function Documentation

Friends And Related Function Documentation