#include <JacobianFactor.h>

Inheritance diagram for gtsam::JacobianFactor:

[legend]

Public Types
typedef VerticalBlockMatrix::Block	ABlock

typedef GaussianFactor	Base
	Typedef to base class. More...

typedef ABlock::ColXpr	BVector

typedef VerticalBlockMatrix::constBlock	constABlock

typedef constABlock::ConstColXpr	constBVector

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

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

Public Types inherited from gtsam::GaussianFactor
typedef Factor	Base
	Our base class. More...

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

typedef GaussianFactor	This
	This class. More...

Public Types inherited from gtsam::Factor
typedef KeyVector::const_iterator	const_iterator
	Const iterator over keys. More...

typedef KeyVector::iterator	iterator
	Iterator over keys. More...

Public Member Functions
Matrix	augmentedInformation () const override

Matrix	augmentedJacobian () const override

Matrix	augmentedJacobianUnweighted () const

GaussianFactor::shared_ptr	clone () const override

size_t	cols () const

std::pair< boost::shared_ptr< GaussianConditional >, shared_ptr >	eliminate (const Ordering &keys)

bool	empty () const override

bool	equals (const GaussianFactor &lf, double tol=1e-9) const override

double	error (const VectorValues &c) const override

Vector	error_vector (const VectorValues &c) const

const SharedDiagonal &	get_model () const

SharedDiagonal &	get_model ()

constABlock	getA (const_iterator variable) const

constABlock	getA () const

ABlock	getA (iterator variable)

ABlock	getA ()

const constBVector	getb () const

BVector	getb ()

DenseIndex	getDim (const_iterator variable) const override

Vector	gradient (Key key, const VectorValues &x) const override
	Compute the gradient wrt a key at any values. More...

VectorValues	gradientAtZero () const override
	A'*b for Jacobian. More...

void	gradientAtZero (double *d) const override
	A'*b for Jacobian (raw memory version) More...

std::map< Key, Matrix >	hessianBlockDiagonal () const override
	Return the block diagonal of the Hessian for this factor. More...

void	hessianDiagonal (double *d) const override
	Raw memory access version of hessianDiagonal. More...

void	hessianDiagonalAdd (VectorValues &d) const override
	Add the current diagonal to a VectorValues instance. More...

Matrix	information () const override

bool	isConstrained () const

std::pair< Matrix, Vector >	jacobian () const override
	Returns (dense) A,b pair associated with factor, bakes in the weights. More...

	JacobianFactor (const GaussianFactor &gf)

	JacobianFactor (const JacobianFactor &jf)

	JacobianFactor (const HessianFactor &hf)

	JacobianFactor ()

	JacobianFactor (const Vector &b_in)

	JacobianFactor (Key i1, const Matrix &A1, const Vector &b, const SharedDiagonal &model=SharedDiagonal())

	JacobianFactor (Key i1, const Matrix &A1, Key i2, const Matrix &A2, const Vector &b, const SharedDiagonal &model=SharedDiagonal())

	JacobianFactor (Key i1, const Matrix &A1, Key i2, const Matrix &A2, Key i3, const Matrix &A3, const Vector &b, const SharedDiagonal &model=SharedDiagonal())

template<typename TERMS >
	JacobianFactor (const TERMS &terms, const Vector &b, const SharedDiagonal &model=SharedDiagonal())

template<typename KEYS >
	JacobianFactor (const KEYS &keys, const VerticalBlockMatrix &augmentedMatrix, const SharedDiagonal &sigmas=SharedDiagonal())

	JacobianFactor (const GaussianFactorGraph &graph)

	JacobianFactor (const GaussianFactorGraph &graph, const VariableSlots &p_variableSlots)

	JacobianFactor (const GaussianFactorGraph &graph, const Ordering &ordering)

	JacobianFactor (const GaussianFactorGraph &graph, const Ordering &ordering, const VariableSlots &p_variableSlots)

std::pair< Matrix, Vector >	jacobianUnweighted () const
	Returns (dense) A,b pair associated with factor, does not bake in weights. More...

const VerticalBlockMatrix &	matrixObject () const

VerticalBlockMatrix &	matrixObject ()

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

void	multiplyHessianAdd (double alpha, const double x, double y, const std::vector< size_t > &accumulatedDims) const

GaussianFactor::shared_ptr	negate () const override

Vector	operator* (const VectorValues &x) const

void	print (const std::string &s="", const KeyFormatter &formatter=DefaultKeyFormatter) const override
	print More...

size_t	rows () const

void	setModel (bool anyConstrained, const Vector &sigmas)

boost::shared_ptr< GaussianConditional >	splitConditional (size_t nrFrontals)

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

Vector	unweighted_error (const VectorValues &c) const

void	updateHessian (const KeyVector &keys, SymmetricBlockMatrix *info) const override

JacobianFactor	whiten () const

	~JacobianFactor () override

Public Member Functions inherited from gtsam::GaussianFactor
	GaussianFactor ()

template<typename CONTAINER >
	GaussianFactor (const CONTAINER &keys)

VectorValues	hessianDiagonal () const
	Return the diagonal of the Hessian for this factor. More...

virtual	~GaussianFactor ()

Public Member Functions inherited from gtsam::Factor
virtual	~Factor ()=default
	Default destructor. More...

Key	front () const
	First key. More...

Key	back () const
	Last key. More...

const_iterator	find (Key key) const
	find More...

const KeyVector &	keys () const
	Access the factor's involved variable keys. More...

const_iterator	begin () const

const_iterator	end () const

size_t	size () const

virtual void	printKeys (const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
	print only keys More...

KeyVector &	keys ()

iterator	begin ()

iterator	end ()

Protected Member Functions
template<typename TERMS >
void	fillTerms (const TERMS &terms, const Vector &b, const SharedDiagonal &noiseModel)
	Internal function to fill blocks and set dimensions. More...

Protected Member Functions inherited from gtsam::Factor
	Factor ()

template<typename CONTAINER >
	Factor (const CONTAINER &keys)

template<typename ITERATOR >
	Factor (ITERATOR first, ITERATOR last)

bool	equals (const This &other, double tol=1e-9) const
	check equality More...

Protected Attributes
VerticalBlockMatrix	Ab_

noiseModel::Diagonal::shared_ptr	model_

Protected Attributes inherited from gtsam::Factor
KeyVector	keys_
	The keys involved in this factor. More...

Private Member Functions
template<class KEYS , class DIMENSIONS >
	JacobianFactor (const KEYS &keys, const DIMENSIONS &dims, DenseIndex m, const SharedDiagonal &model=SharedDiagonal())

void	JacobianFactorHelper (const GaussianFactorGraph &graph, const FastVector< VariableSlots::const_iterator > &orderedSlots)

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

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

Friends
class	boost::serialization::access

GTSAM_EXPORT std::pair< boost::shared_ptr< GaussianConditional >, shared_ptr >	EliminateQR (const GaussianFactorGraph &factors, const Ordering &keys)

template<typename T >
class	ExpressionFactor

Additional Inherited Members
Static Public Member Functions inherited from gtsam::GaussianFactor
template<typename CONTAINER >
static DenseIndex	Slot (const CONTAINER &keys, Key key)

Static Protected Member Functions inherited from gtsam::Factor
template<typename CONTAINER >
static Factor	FromKeys (const CONTAINER &keys)

template<typename ITERATOR >
static Factor	FromIterators (ITERATOR first, ITERATOR last)

Detailed Description

A Gaussian factor in the squared-error form.

JacobianFactor implements a Gaussian, which has quadratic negative log-likelihood

$E(x) = \frac{1}{2} (Ax-b)^T \Sigma^{-1} (Ax-b)$

where $\Sigma$ is a diagonal covariance matrix. The matrix $ A $ , r.h.s. vector $ b $ , and diagonal noise model $\Sigma$ are stored in this class.

This factor represents the sum-of-squares error of a linear measurement function, and is created upon linearization of a NoiseModelFactor, which in turn is a sum-of-squares factor with a nonlinear measurement function.

Here is an example of how this factor represents a sum-of-squares error:

Letting $ h(x) $ be a linear measurement prediction function, $ z $ be the actual observed measurement, the residual is

$f(x) = h(x) - z .$

If we expect noise with diagonal covariance matrix $\Sigma$ on this measurement, then the negative log-likelihood of the Gaussian induced by this measurement model is

$E(x) = \frac{1}{2} (h(x) - z)^T \Sigma^{-1} (h(x) - z) .$

Because $ h(x) $ is linear, we can write it as

$h(x) = Ax + e$

and thus we have

$E(x) = \frac{1}{2} (Ax-b)^T \Sigma^{-1} (Ax-b)$

where $ b = z - e $ .

This factor can involve an arbitrary number of variables, and in the above example $ x $ would almost always be only be a subset of the variables in the entire factor graph. There are special constructors for 1-, 2-, and 3- way JacobianFactors, and additional constructors for creating n-way JacobianFactors. The Jacobian matrix $ A $ is passed to these constructors in blocks, for example, for a 2-way factor, the constructor would accept $ A1 $ and $ A2 $ , as well as the variable indices $ j1 $ and $ j2 $ and the negative log-likelihood represented by this factor would be

$E(x) = \frac{1}{2} (A_1 x_{j1} + A_2 x_{j2} - b)^T \Sigma^{-1} (A_1 x_{j1} + A_2 x_{j2} - b) .$

Definition at line 90 of file JacobianFactor.h.

Member Typedef Documentation

typedef VerticalBlockMatrix::Block gtsam::JacobianFactor::ABlock

Definition at line 98 of file JacobianFactor.h.

typedef GaussianFactor gtsam::JacobianFactor::Base

Typedef to base class.

Definition at line 95 of file JacobianFactor.h.

typedef ABlock::ColXpr gtsam::JacobianFactor::BVector

Definition at line 100 of file JacobianFactor.h.

typedef VerticalBlockMatrix::constBlock gtsam::JacobianFactor::constABlock

Definition at line 99 of file JacobianFactor.h.

typedef constABlock::ConstColXpr gtsam::JacobianFactor::constBVector

Definition at line 101 of file JacobianFactor.h.

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

shared_ptr to this class

Definition at line 96 of file JacobianFactor.h.

typedef JacobianFactor gtsam::JacobianFactor::This

Typedef to this class.

Definition at line 94 of file JacobianFactor.h.

Constructor & Destructor Documentation

gtsam::JacobianFactor::JacobianFactor ( const GaussianFactor & gf )

explicit

Convert from other GaussianFactor

Definition at line 66 of file JacobianFactor.cpp.

gtsam::JacobianFactor::JacobianFactor ( const JacobianFactor & jf )

inline

Copy constructor

Definition at line 114 of file JacobianFactor.h.

gtsam::JacobianFactor::JacobianFactor ( const HessianFactor & hf )

explicit

Conversion from HessianFactor (does Cholesky to obtain Jacobian matrix)

Definition at line 105 of file JacobianFactor.cpp.

gtsam::JacobianFactor::JacobianFactor ( )

default constructor for I/O

Definition at line 60 of file JacobianFactor.cpp.

gtsam::JacobianFactor::JacobianFactor ( const Vector & b_in )

explicit

Construct Null factor

Definition at line 78 of file JacobianFactor.cpp.

gtsam::JacobianFactor::JacobianFactor	(	Key	i1,
		const Matrix &	A1,
		const Vector &	b,
		const SharedDiagonal &	model = `SharedDiagonal()`
	)

Construct unary factor

Definition at line 84 of file JacobianFactor.cpp.

gtsam::JacobianFactor::JacobianFactor	(	Key	i1,
		const Matrix &	A1,
		Key	i2,
		const Matrix &	A2,
		const Vector &	b,
		const SharedDiagonal &	model = `SharedDiagonal()`
	)

Construct binary factor

Definition at line 90 of file JacobianFactor.cpp.

gtsam::JacobianFactor::JacobianFactor	(	Key	i1,
		const Matrix &	A1,
		Key	i2,
		const Matrix &	A2,
		Key	i3,
		const Matrix &	A3,
		const Vector &	b,
		const SharedDiagonal &	model = `SharedDiagonal()`
	)

Construct ternary factor

Definition at line 96 of file JacobianFactor.cpp.

template<typename TERMS >

gtsam::JacobianFactor::JacobianFactor	(	const TERMS &	terms,
		const Vector &	b,
		const SharedDiagonal &	model = `SharedDiagonal()`
	)

Construct an n-ary factor

Template Parameters

TERMS A container whose value type is std::pair<Key, Matrix>, specifying the collection of keys and matrices making up the factor.

Definition at line 27 of file JacobianFactor-inl.h.

template<typename KEYS >

gtsam::JacobianFactor::JacobianFactor	(	const KEYS &	keys,
		const VerticalBlockMatrix &	augmentedMatrix,
		const SharedDiagonal &	sigmas = `SharedDiagonal()`
	)

Constructor with arbitrary number keys, and where the augmented matrix is given all together instead of in block terms. Note that only the active view of the provided augmented matrix is used, and that the matrix data is copied into a newly-allocated matrix in the constructed factor.

Definition at line 34 of file JacobianFactor-inl.h.

gtsam::JacobianFactor::JacobianFactor ( const GaussianFactorGraph & graph )

explicit

Build a dense joint factor from all the factors in a factor graph. If a VariableSlots structure computed for graph is already available, providing it will reduce the amount of computation performed.

Definition at line 343 of file JacobianFactor.cpp.

gtsam::JacobianFactor::JacobianFactor	(	const GaussianFactorGraph &	graph,
		const VariableSlots &	p_variableSlots
	)

explicit

Build a dense joint factor from all the factors in a factor graph. If a VariableSlots structure computed for graph is already available, providing it will reduce the amount of computation performed.

Definition at line 368 of file JacobianFactor.cpp.

gtsam::JacobianFactor::JacobianFactor	(	const GaussianFactorGraph &	graph,
		const Ordering &	ordering
	)

explicit

Build a dense joint factor from all the factors in a factor graph. If a VariableSlots structure computed for graph is already available, providing it will reduce the amount of computation performed.

Definition at line 393 of file JacobianFactor.cpp.

gtsam::JacobianFactor::JacobianFactor	(	const GaussianFactorGraph &	graph,
		const Ordering &	ordering,
		const VariableSlots &	p_variableSlots
	)

explicit

Build a dense joint factor from all the factors in a factor graph. If a VariableSlots structure computed for graph is already available, providing it will reduce the amount of computation performed.

Definition at line 409 of file JacobianFactor.cpp.

gtsam::JacobianFactor::~JacobianFactor ( )

inlineoverride

Virtual destructor

Definition at line 186 of file JacobianFactor.h.

template<class KEYS , class DIMENSIONS >

gtsam::JacobianFactor::JacobianFactor	(	const KEYS &	keys,
		const DIMENSIONS &	dims,
		DenseIndex	m,
		const SharedDiagonal &	model = `SharedDiagonal()`
	)

inlineprivate

Unsafe Constructor that creates an uninitialized Jacobian of right size

Parameters

keys	in some order
diemnsions	of the variables in same order
m	output dimension
model	noise model (default nullptr)

Definition at line 407 of file JacobianFactor.h.

Member Function Documentation

Matrix gtsam::JacobianFactor::augmentedInformation ( ) const

overridevirtual

0.5*(A*x-b)'D(A*x-b) Return the augmented information matrix represented by this GaussianFactor. The augmented information matrix contains the information matrix with an additional column holding the information vector, and an additional row holding the transpose of the information vector. The lower-right entry contains the constant error term (when $\delta x = 0$ ). The augmented information matrix is described in more detail in HessianFactor, which in fact stores an augmented information matrix.

void gtsam::JacobianFactor::multiplyHessianAdd	(	double	alpha,
		const double *	x,
		double *	y,
		const std::vector< size_t > &	accumulatedDims
	)		const

void gtsam::JacobianFactor::setModel	(	bool	anyConstrained,
		const Vector &	sigmas
	)

void gtsam::JacobianFactor::transposeMultiplyAdd	(	double	alpha,
		const Vector &	e,
		VectorValues &	x
	)		const

scatter	A mapping from variable index to slot index in this HessianFactor
info	The information matrix to be updated

Public Types

Public Member Functions

Protected Member Functions

Protected Attributes

Private Member Functions

Friends

Additional Inherited Members

Detailed Description

Member Typedef Documentation

Constructor & Destructor Documentation

Member Function Documentation

Friends And Related Function Documentation

Member Data Documentation