#include <GaussianFactorGraph.h>

Inheritance diagram for gtsam::GaussianFactorGraph:

Public Types
typedef FactorGraph< GaussianFactor >	Base
	Typedef to base factor graph type. More...

typedef EliminateableFactorGraph< This >	BaseEliminateable
	Typedef to base elimination class. More...

typedef KeySet	Keys

typedef std::shared_ptr< This >	shared_ptr
	shared_ptr to this class More...

typedef GaussianFactorGraph	This
	Typedef to this class. More...

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

typedef GaussianFactor	FactorType
	factor type More...

typedef FastVector< sharedFactor >::iterator	iterator

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

typedef sharedFactor	value_type

Public Types inherited from gtsam::EliminateableFactorGraph< GaussianFactorGraph >
typedef EliminationTraitsType::BayesNetType	BayesNetType
	Bayes net type produced by sequential elimination. More...

typedef EliminationTraitsType::BayesTreeType	BayesTreeType
	Bayes tree type produced by multifrontal elimination. More...

typedef EliminationTraitsType::ConditionalType	ConditionalType
	Conditional type stored in the Bayes net produced by elimination. More...

typedef std::function< EliminationResult(const FactorGraphType &, const Ordering &)>	Eliminate
	The function type that does a single dense elimination step on a subgraph. More...

typedef std::pair< std::shared_ptr< ConditionalType >, std::shared_ptr< _FactorType > >	EliminationResult

typedef EliminationTraits< FactorGraphType >	EliminationTraitsType
	Typedef to the specific EliminationTraits for this graph. More...

typedef EliminationTraitsType::EliminationTreeType	EliminationTreeType
	Elimination tree type that can do sequential elimination of this graph. More...

typedef EliminationTraitsType::JunctionTreeType	JunctionTreeType
	Junction tree type that can do multifrontal elimination of this graph. More...

typedef std::optional< Ordering::OrderingType >	OptionalOrderingType
	Typedef for an optional ordering type. More...

typedef std::optional< std::reference_wrapper< const VariableIndex > >	OptionalVariableIndex

Public Member Functions
void	add (const GaussianFactor &factor)

void	add (const sharedFactor &factor)

void	add (const Vector &b)

void	add (Key key1, const Matrix &A1, const Vector &b, const SharedDiagonal &model=SharedDiagonal())

void	add (Key key1, const Matrix &A1, Key key2, const Matrix &A2, const Vector &b, const SharedDiagonal &model=SharedDiagonal())

void	add (Key key1, const Matrix &A1, Key key2, const Matrix &A2, Key key3, const Matrix &A3, const Vector &b, const SharedDiagonal &model=SharedDiagonal())

template<class TERMS >
void	add (const TERMS &terms, const Vector &b, const SharedDiagonal &model=SharedDiagonal())

virtual GaussianFactorGraph	clone () const

virtual GaussianFactorGraph::shared_ptr	cloneToPtr () const

double	error (const VectorValues &x) const

std::map< Key, size_t >	getKeyDimMap () const

Keys	keys () const

GaussianFactorGraph	negate () const

double	probPrime (const VectorValues &c) const

Constructors
	GaussianFactorGraph ()

	GaussianFactorGraph (std::initializer_list< sharedFactor > factors)

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

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

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

Testable
bool	equals (const This &fg, double tol=1e-9) const

Linear Algebra
std::vector< std::tuple< int, int, double > >	sparseJacobian (const Ordering &ordering, size_t &nrows, size_t &ncols) const

std::vector< std::tuple< int, int, double > >	sparseJacobian () const

Matrix	sparseJacobian_ () const

Matrix	augmentedJacobian (const Ordering &ordering) const

Matrix	augmentedJacobian () const

std::pair< Matrix, Vector >	jacobian (const Ordering &ordering) const

std::pair< Matrix, Vector >	jacobian () const

Matrix	augmentedHessian (const Ordering &ordering) const

Matrix	augmentedHessian () const

std::pair< Matrix, Vector >	hessian (const Ordering &ordering) const

std::pair< Matrix, Vector >	hessian () const

virtual VectorValues	hessianDiagonal () const

virtual std::map< Key, Matrix >	hessianBlockDiagonal () const

VectorValues	optimize (const Eliminate &function=EliminationTraitsType::DefaultEliminate) const

VectorValues	optimize (const Ordering &, const Eliminate &function=EliminationTraitsType::DefaultEliminate) const

VectorValues	optimizeDensely () const

VectorValues	gradient (const VectorValues &x0) const

virtual VectorValues	gradientAtZero () const

VectorValues	optimizeGradientSearch () const

VectorValues	transposeMultiply (const Errors &e) const

void	transposeMultiplyAdd (double alpha, const Errors &e, VectorValues &x) const

Errors	gaussianErrors (const VectorValues &x) const

Errors	operator* (const VectorValues &x) const

void	multiplyHessianAdd (double alpha, const VectorValues &x, VectorValues &y) const

void	multiplyInPlace (const VectorValues &x, Errors &e) const

void	multiplyInPlace (const VectorValues &x, const Errors::iterator &e) const

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

Public Member Functions inherited from gtsam::FactorGraph< GaussianFactor >
	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...

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)

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)

virtual void	print (const std::string &s="FactorGraph", const KeyFormatter &formatter=DefaultKeyFormatter) const
	Print out graph to std::cout, with optional key formatter. More...

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)

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

Public Member Functions inherited from gtsam::EliminateableFactorGraph< GaussianFactorGraph >
std::shared_ptr< BayesTreeType >	eliminateMultifrontal (OptionalOrderingType orderingType={}, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const

std::shared_ptr< BayesTreeType >	eliminateMultifrontal (const Ordering &ordering, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const

std::pair< std::shared_ptr< BayesTreeType >, std::shared_ptr< FactorGraphType > >	eliminatePartialMultifrontal (const Ordering &ordering, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const

std::pair< std::shared_ptr< BayesTreeType >, std::shared_ptr< FactorGraphType > >	eliminatePartialMultifrontal (const KeyVector &variables, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const

std::pair< std::shared_ptr< BayesNetType >, std::shared_ptr< FactorGraphType > >	eliminatePartialSequential (const Ordering &ordering, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const

std::pair< std::shared_ptr< BayesNetType >, std::shared_ptr< FactorGraphType > >	eliminatePartialSequential (const KeyVector &variables, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const

std::shared_ptr< BayesNetType >	eliminateSequential (OptionalOrderingType orderingType={}, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const

std::shared_ptr< BayesNetType >	eliminateSequential (const Ordering &ordering, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const

std::shared_ptr< FactorGraphType >	marginal (const KeyVector &variables, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const

std::shared_ptr< BayesNetType >	marginalMultifrontalBayesNet (const Ordering &variables, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const

std::shared_ptr< BayesNetType >	marginalMultifrontalBayesNet (const KeyVector &variables, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const

std::shared_ptr< BayesNetType >	marginalMultifrontalBayesNet (const Ordering &variables, const Ordering &marginalizedVariableOrdering, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const

std::shared_ptr< BayesNetType >	marginalMultifrontalBayesNet (const KeyVector &variables, const Ordering &marginalizedVariableOrdering, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const

std::shared_ptr< BayesTreeType >	marginalMultifrontalBayesTree (const Ordering &variables, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const

std::shared_ptr< BayesTreeType >	marginalMultifrontalBayesTree (const KeyVector &variables, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const

std::shared_ptr< BayesTreeType >	marginalMultifrontalBayesTree (const Ordering &variables, const Ordering &marginalizedVariableOrdering, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const

std::shared_ptr< BayesTreeType >	marginalMultifrontalBayesTree (const KeyVector &variables, const Ordering &marginalizedVariableOrdering, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const

Friends
bool	operator== (const GaussianFactorGraph &lhs, const GaussianFactorGraph &rhs)
	Check exact equality. More...

Additional Inherited Members
Protected Member Functions inherited from gtsam::FactorGraph< GaussianFactor >
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< GaussianFactor >
FastVector< sharedFactor >	factors_

Detailed Description

A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e. Factor == GaussianFactor VectorValues = A values structure of vectors Most of the time, linear factor graphs arise by linearizing a non-linear factor graph.

Definition at line 73 of file GaussianFactorGraph.h.

Member Typedef Documentation

◆ Base

typedef FactorGraph<GaussianFactor> gtsam::GaussianFactorGraph::Base

Typedef to base factor graph type.

Definition at line 80 of file GaussianFactorGraph.h.

◆ BaseEliminateable

typedef EliminateableFactorGraph<This> gtsam::GaussianFactorGraph::BaseEliminateable

Typedef to base elimination class.

Definition at line 81 of file GaussianFactorGraph.h.

◆ Keys

typedef KeySet gtsam::GaussianFactorGraph::Keys

Return the set of variables involved in the factors (computes a set union).

Definition at line 161 of file GaussianFactorGraph.h.

◆ shared_ptr

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

shared_ptr to this class

Definition at line 82 of file GaussianFactorGraph.h.

◆ This

typedef GaussianFactorGraph gtsam::GaussianFactorGraph::This

Typedef to this class.

Definition at line 79 of file GaussianFactorGraph.h.

Constructor & Destructor Documentation

◆ GaussianFactorGraph() [1/5]

gtsam::GaussianFactorGraph::GaussianFactorGraph ( )

inline

Default constructor

Definition at line 88 of file GaussianFactorGraph.h.

◆ GaussianFactorGraph() [2/5]

gtsam::GaussianFactorGraph::GaussianFactorGraph ( std::initializer_list< sharedFactor > factors )

inline

Construct from an initializer lists of GaussianFactor shared pointers. Example: GaussianFactorGraph graph = { factor1, factor2, factor3 };

Definition at line 95 of file GaussianFactorGraph.h.

◆ GaussianFactorGraph() [3/5]

template<typename ITERATOR >

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

inline

Construct from iterator over factors

Definition at line 100 of file GaussianFactorGraph.h.

◆ GaussianFactorGraph() [4/5]

template<class CONTAINER >

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

inlineexplicit

Construct from container of factors (shared_ptr or plain objects)

Definition at line 104 of file GaussianFactorGraph.h.

◆ GaussianFactorGraph() [5/5]

template<class DERIVEDFACTOR >

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

inline

Implicit copy/downcast constructor to override explicit template container constructor

Definition at line 108 of file GaussianFactorGraph.h.

Member Function Documentation

◆ add() [1/7]

void gtsam::GaussianFactorGraph::add ( const GaussianFactor & factor )

inline

Add a factor by value - makes a copy

Definition at line 125 of file GaussianFactorGraph.h.

◆ add() [2/7]

void gtsam::GaussianFactorGraph::add ( const sharedFactor & factor )

inline

Add a factor by pointer - stores pointer without copying the factor

Definition at line 128 of file GaussianFactorGraph.h.

◆ add() [3/7]

void gtsam::GaussianFactorGraph::add ( const Vector & b )

inline

Add a null factor

Definition at line 131 of file GaussianFactorGraph.h.

◆ add() [4/7]

void gtsam::GaussianFactorGraph::add	(	Key	key1,
		const Matrix &	A1,
		const Vector &	b,
		const SharedDiagonal &	model = `SharedDiagonal()`
	)

inline

Add a unary factor

Definition at line 135 of file GaussianFactorGraph.h.

◆ add() [5/7]

void gtsam::GaussianFactorGraph::add	(	Key	key1,
		const Matrix &	A1,
		Key	key2,
		const Matrix &	A2,
		const Vector &	b,
		const SharedDiagonal &	model = `SharedDiagonal()`
	)

inline

Add a binary factor

Definition at line 140 of file GaussianFactorGraph.h.

◆ add() [6/7]

void gtsam::GaussianFactorGraph::add	(	Key	key1,
		const Matrix &	A1,
		Key	key2,
		const Matrix &	A2,
		Key	key3,
		const Matrix &	A3,
		const Vector &	b,
		const SharedDiagonal &	model = `SharedDiagonal()`
	)

inline

Add a ternary factor

Definition at line 146 of file GaussianFactorGraph.h.

◆ add() [7/7]

template<class TERMS >

void gtsam::GaussianFactorGraph::add	(	const TERMS &	terms,
		const Vector &	b,
		const SharedDiagonal &	model = `SharedDiagonal()`
	)

inline

Add an n-ary factor

Definition at line 154 of file GaussianFactorGraph.h.

◆ augmentedHessian() [1/2]

Matrix gtsam::GaussianFactorGraph::augmentedHessian ( const Ordering & ordering ) const

Return a dense $\Lambda \in \mathbb{R}^{n+1 \times n+1}$ Hessian matrix, augmented with the information vector $\eta$ . The augmented Hessian is

$\left[ \begin{array}{ccc} \Lambda & \eta \\ \eta^T & c \end{array} \right]$

and the negative log-likelihood is $\frac{1}{2} x^T \Lambda x + \eta^T x + c$ .

Definition at line 247 of file GaussianFactorGraph.cpp.

◆ augmentedHessian() [2/2]

Matrix gtsam::GaussianFactorGraph::augmentedHessian ( ) const

Return a dense $\Lambda \in \mathbb{R}^{n+1 \times n+1}$ Hessian matrix, augmented with the information vector $\eta$ . The augmented Hessian is

$\left[ \begin{array}{ccc} \Lambda & \eta \\ \eta^T & c \end{array} \right]$

and the negative log-likelihood is $\frac{1}{2} x^T \Lambda x + \eta^T x + c$ .

Definition at line 256 of file GaussianFactorGraph.cpp.

◆ augmentedJacobian() [1/2]

Matrix gtsam::GaussianFactorGraph::augmentedJacobian ( const Ordering & ordering ) const

Return a dense $[ \;A\;b\; ] \in \mathbb{R}^{m \times n+1}$ Jacobian matrix, augmented with b with the noise models baked into A and b. The negative log-likelihood is $\frac{1}{2} \Vert Ax-b \Vert^2$ . See also GaussianFactorGraph::jacobian and GaussianFactorGraph::sparseJacobian.

Definition at line 217 of file GaussianFactorGraph.cpp.

◆ augmentedJacobian() [2/2]

Matrix gtsam::GaussianFactorGraph::augmentedJacobian ( ) const

Return a dense $[ \;A\;b\; ] \in \mathbb{R}^{m \times n+1}$ Jacobian matrix, augmented with b with the noise models baked into A and b. The negative log-likelihood is $\frac{1}{2} \Vert Ax-b \Vert^2$ . See also GaussianFactorGraph::jacobian and GaussianFactorGraph::sparseJacobian.

Definition at line 225 of file GaussianFactorGraph.cpp.

◆ clone()

GaussianFactorGraph gtsam::GaussianFactorGraph::clone ( ) const

virtual

Clone() performs a deep-copy of the graph, including all of the factors. Cloning preserves null factors so indices for the original graph are still valid for the cloned graph.

Definition at line 94 of file GaussianFactorGraph.cpp.

◆ cloneToPtr()

GaussianFactorGraph::shared_ptr gtsam::GaussianFactorGraph::cloneToPtr ( ) const

virtual

CloneToPtr() performs a simple assignment to a new graph and returns it. There is no preservation of null factors!

Definition at line 87 of file GaussianFactorGraph.cpp.

◆ equals()

bool gtsam::GaussianFactorGraph::equals	(	const This &	fg,
		double	tol = `1e-9`
	)		const

Definition at line 42 of file GaussianFactorGraph.cpp.

◆ error()

double gtsam::GaussianFactorGraph::error ( const VectorValues & x ) const

unnormalized error

Definition at line 71 of file GaussianFactorGraph.cpp.

◆ gaussianErrors()

Errors gtsam::GaussianFactorGraph::gaussianErrors ( const VectorValues & x ) const

return A*x-b

Definition at line 512 of file GaussianFactorGraph.cpp.

◆ getKeyDimMap()

std::map< Key, size_t > gtsam::GaussianFactorGraph::getKeyDimMap ( ) const

Definition at line 57 of file GaussianFactorGraph.cpp.

◆ gradient()

VectorValues gtsam::GaussianFactorGraph::gradient ( const VectorValues & x0 ) const

Compute the gradient of the energy function, $\nabla_{x=x_0} \left\Vert \Sigma^{-1} A x - b \right\Vert^2$ , centered around $ x = x_0 $ . The gradient is $ A^T(Ax-b) $ .

Parameters

fg	The Jacobian factor graph $(A,b)$
x0	The center about which to compute the gradient

Returns: The gradient as a VectorValues

Definition at line 357 of file GaussianFactorGraph.cpp.

◆ gradientAtZero()

VectorValues gtsam::GaussianFactorGraph::gradientAtZero ( ) const

virtual

Compute the gradient of the energy function, $\nabla_{x=0} \left\Vert \Sigma^{-1} A x - b \right\Vert^2$ , centered around zero. The gradient is $ A^T(Ax-b) $ .

Parameters

fg	The Jacobian factor graph $(A,b)$
[output]	g A VectorValues to store the gradient, which must be preallocated, see allocateVectorValues

Returns: The gradient as a VectorValues

Definition at line 369 of file GaussianFactorGraph.cpp.

◆ hessian() [1/2]

pair< Matrix, Vector > gtsam::GaussianFactorGraph::hessian ( const Ordering & ordering ) const

Return the dense Hessian $\Lambda$ and information vector $\eta$ , with the noise models baked in. The negative log-likelihood is {1}{2} x^T x + ^T x + c. See also GaussianFactorGraph::augmentedHessian.

Definition at line 264 of file GaussianFactorGraph.cpp.

◆ hessian() [2/2]

pair< Matrix, Vector > gtsam::GaussianFactorGraph::hessian ( ) const

Return the dense Hessian $\Lambda$ and information vector $\eta$ , with the noise models baked in. The negative log-likelihood is {1}{2} x^T x + ^T x + c. See also GaussianFactorGraph::augmentedHessian.

Definition at line 272 of file GaussianFactorGraph.cpp.

◆ hessianBlockDiagonal()

map< Key, Matrix > gtsam::GaussianFactorGraph::hessianBlockDiagonal ( ) const

virtual

Return the block diagonal of the Hessian for this factor

Definition at line 290 of file GaussianFactorGraph.cpp.

◆ hessianDiagonal()

VectorValues gtsam::GaussianFactorGraph::hessianDiagonal ( ) const

virtual

Return only the diagonal of the Hessian A'*A, as a VectorValues

Definition at line 279 of file GaussianFactorGraph.cpp.

◆ jacobian() [1/2]

pair< Matrix, Vector > gtsam::GaussianFactorGraph::jacobian ( const Ordering & ordering ) const

Return the dense Jacobian $ A $ and right-hand-side $ b $ , with the noise models baked into A and b. The negative log-likelihood is $\frac{1}{2} \Vert Ax-b \Vert^2$ . See also GaussianFactorGraph::augmentedJacobian and GaussianFactorGraph::sparseJacobian.

Definition at line 232 of file GaussianFactorGraph.cpp.

◆ jacobian() [2/2]

pair< Matrix, Vector > gtsam::GaussianFactorGraph::jacobian ( ) const

Return the dense Jacobian $ A $ and right-hand-side $ b $ , with the noise models baked into A and b. The negative log-likelihood is $\frac{1}{2} \Vert Ax-b \Vert^2$ . See also GaussianFactorGraph::augmentedJacobian and GaussianFactorGraph::sparseJacobian.

Definition at line 240 of file GaussianFactorGraph.cpp.

◆ keys()

GaussianFactorGraph::Keys gtsam::GaussianFactorGraph::keys ( ) const

Definition at line 48 of file GaussianFactorGraph.cpp.

◆ multiplyHessianAdd()

void gtsam::GaussianFactorGraph::multiplyHessianAdd	(	double	alpha,
		const VectorValues &	x,
		VectorValues &	y
	)		const

y += alpha*A'A*x

Definition at line 420 of file GaussianFactorGraph.cpp.

◆ multiplyInPlace() [1/2]

void gtsam::GaussianFactorGraph::multiplyInPlace	(	const VectorValues &	x,
		Errors &	e
	)		const

In-place version e <- A*x that overwrites e.

Definition at line 427 of file GaussianFactorGraph.cpp.

◆ multiplyInPlace() [2/2]

void gtsam::GaussianFactorGraph::multiplyInPlace	(	const VectorValues &	x,
		const Errors::iterator &	e
	)		const

In-place version e <- A*x that takes an iterator.

Definition at line 432 of file GaussianFactorGraph.cpp.

◆ negate()

GaussianFactorGraph gtsam::GaussianFactorGraph::negate ( ) const

Returns the negation of all factors in this graph - corresponds to antifactors. Will convert all factors to HessianFactors due to negation of information. Cloning preserves null factors so indices for the original graph are still valid for the cloned graph.

Definition at line 106 of file GaussianFactorGraph.cpp.

◆ operator*()

Errors gtsam::GaussianFactorGraph::operator* ( const VectorValues & x ) const

return A*x

Definition at line 410 of file GaussianFactorGraph.cpp.

◆ optimize() [1/2]

VectorValues gtsam::GaussianFactorGraph::optimize ( const Eliminate & function = EliminationTraitsType::DefaultEliminate ) const

Solve the factor graph by performing multifrontal variable elimination in COLAMD order using the dense elimination function specified in function (default EliminatePreferCholesky), followed by back-substitution in the Bayes tree resulting from elimination. Is equivalent to calling graph.eliminateMultifrontal()->optimize().

Definition at line 309 of file GaussianFactorGraph.cpp.

◆ optimize() [2/2]

VectorValues gtsam::GaussianFactorGraph::optimize	(	const Ordering &	ordering,
		const Eliminate &	function = `EliminationTraitsType::DefaultEliminate`
	)		const

Solve the factor graph by performing multifrontal variable elimination in COLAMD order using the dense elimination function specified in function (default EliminatePreferCholesky), followed by back-substitution in the Bayes tree resulting from elimination. Is equivalent to calling graph.eliminateMultifrontal()->optimize().

Definition at line 316 of file GaussianFactorGraph.cpp.

◆ optimizeDensely()

VectorValues gtsam::GaussianFactorGraph::optimizeDensely ( ) const

Optimize using Eigen's dense Cholesky factorization

Definition at line 323 of file GaussianFactorGraph.cpp.

◆ optimizeGradientSearch()

VectorValues gtsam::GaussianFactorGraph::optimizeGradientSearch ( ) const

Optimize along the gradient direction, with a closed-form computation to perform the line search. The gradient is computed about $\delta x=0$ .

This function returns $\delta x$ that minimizes a reparametrized problem. The error function of a GaussianBayesNet is

$f(\delta x) = \frac{1}{2} |R \delta x - d|^2 = \frac{1}{2}d^T d - d^T R \delta x + \frac{1}{2} \delta x^T R^T R \delta x$

with gradient and Hessian

$g(\delta x) = R^T(R\delta x - d), \qquad G(\delta x) = R^T R.$

This function performs the line search in the direction of the gradient evaluated at $g = g(\delta x = 0)$ with step size $\alpha$ that minimizes $f(\delta x = \alpha g)$ :

$f(\alpha) = \frac{1}{2} d^T d + g^T \delta x + \frac{1}{2} \alpha^2 g^T G g$

Optimizing by setting the derivative to zero yields $\hat \alpha = (-g^T g) / (g^T G g)$ . For efficiency, this function evaluates the denominator without computing the Hessian $ G $ , returning

$\delta x = \hat\alpha g = \frac{-g^T g}{(R g)^T(R g)}$

Definition at line 381 of file GaussianFactorGraph.cpp.

◆ printErrors()

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

Definition at line 523 of file GaussianFactorGraph.cpp.

◆ probPrime()

double gtsam::GaussianFactorGraph::probPrime ( const VectorValues & c ) const

Unnormalized probability. O(n)

Definition at line 81 of file GaussianFactorGraph.cpp.

◆ sparseJacobian() [1/2]

SparseTriplets gtsam::GaussianFactorGraph::sparseJacobian	(	const Ordering &	ordering,
		size_t &	nrows,
		size_t &	ncols
	)		const

Returns a sparse augmented Jacbian matrix as a vector of i, j, and s, where i(k) and j(k) are the base 0 row and column indices, and s(k) is the entry as a double. The standard deviations are baked into A and b

Returns: the sparse matrix as a std::vector of std::tuples

Parameters

	ordering	the column ordering
[out]	nrows	The number of rows in the augmented Jacobian
[out]	ncols	The number of columns in the augmented Jacobian

Definition at line 119 of file GaussianFactorGraph.cpp.

◆ sparseJacobian() [2/2]

SparseTriplets gtsam::GaussianFactorGraph::sparseJacobian ( ) const

Returns a sparse augmented Jacobian matrix with default ordering

Definition at line 193 of file GaussianFactorGraph.cpp.

◆ sparseJacobian_()

Matrix gtsam::GaussianFactorGraph::sparseJacobian_ ( ) const

Matrix version of sparseJacobian: generates a 3*m matrix with [i,j,s] entries such that S(i(k),j(k)) = s(k), which can be given to MATLAB's sparse. Note: i, j are 1-indexed. The standard deviations are baked into A and b

Definition at line 199 of file GaussianFactorGraph.cpp.

◆ transposeMultiply()

VectorValues gtsam::GaussianFactorGraph::transposeMultiply ( const Errors & e ) const

x = A'*e

************************************************************************* */* ************************************************************************* */

Definition at line 493 of file GaussianFactorGraph.cpp.

◆ transposeMultiplyAdd()

void gtsam::GaussianFactorGraph::transposeMultiplyAdd	(	double	alpha,
		const Errors &	e,
		VectorValues &	x
	)		const

x += alpha*A'*e

Definition at line 455 of file GaussianFactorGraph.cpp.

Friends And Related Function Documentation

◆ operator==

bool operator==	(	const GaussianFactorGraph &	lhs,
		const GaussianFactorGraph &	rhs
	)

friend

Check exact equality.

Definition at line 119 of file GaussianFactorGraph.h.

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

Public Types

Public Member Functions

Friends

Additional Inherited Members

Detailed Description

Member Typedef Documentation

◆ Base

◆ BaseEliminateable

◆ Keys

◆ shared_ptr

◆ This

Constructor & Destructor Documentation

◆ GaussianFactorGraph() [1/5]

◆ GaussianFactorGraph() [2/5]

◆ GaussianFactorGraph() [3/5]

◆ GaussianFactorGraph() [4/5]

◆ GaussianFactorGraph() [5/5]

Member Function Documentation

◆ add() [1/7]

◆ add() [2/7]

◆ add() [3/7]

◆ add() [4/7]

◆ add() [5/7]

◆ add() [6/7]

◆ add() [7/7]

◆ augmentedHessian() [1/2]

◆ augmentedHessian() [2/2]

◆ augmentedJacobian() [1/2]

◆ augmentedJacobian() [2/2]

◆ clone()

◆ cloneToPtr()

◆ equals()

◆ error()

◆ gaussianErrors()

◆ getKeyDimMap()

◆ gradient()

◆ gradientAtZero()

◆ hessian() [1/2]

◆ hessian() [2/2]

◆ hessianBlockDiagonal()

◆ hessianDiagonal()

◆ jacobian() [1/2]

◆ jacobian() [2/2]

◆ keys()

◆ multiplyHessianAdd()

◆ multiplyInPlace() [1/2]

◆ multiplyInPlace() [2/2]

◆ negate()

◆ operator*()

◆ optimize() [1/2]

◆ optimize() [2/2]

◆ optimizeDensely()

◆ optimizeGradientSearch()

◆ printErrors()

◆ probPrime()

◆ sparseJacobian() [1/2]

◆ sparseJacobian() [2/2]

◆ sparseJacobian_()

◆ transposeMultiply()

◆ transposeMultiplyAdd()

Friends And Related Function Documentation

◆ operator==