#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 std::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 std::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< std::shared_ptr< GaussianConditional >, shared_ptr >	eliminate (const Ordering &keys)

bool	equals (const GaussianFactor &lf, double tol=1e-9) const override
	assert equality up to a tolerance More...

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 with optional string More...

size_t	rows () const

void	setModel (bool anyConstrained, const Vector &sigmas)

std::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)

double	error (const HybridValues &c) const override

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

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

bool	empty () const
	Whether the factor is empty (involves zero variables). 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...

bool	equals (const This &other, double tol=1e-9) const
	check equality 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)

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)

Friends
GTSAM_EXPORT std::pair< std::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) .$

HessianFactor, which represent a Gaussian likelihood over a set of variables.

Discrete factors, such as

Definition at line 91 of file JacobianFactor.h.

Member Typedef Documentation

◆ ABlock

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

Definition at line 99 of file JacobianFactor.h.

◆ Base

typedef GaussianFactor gtsam::JacobianFactor::Base

Typedef to base class.

Definition at line 96 of file JacobianFactor.h.

◆ BVector

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

Definition at line 101 of file JacobianFactor.h.

◆ constABlock

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

Definition at line 100 of file JacobianFactor.h.

◆ constBVector

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

Definition at line 102 of file JacobianFactor.h.

◆ shared_ptr

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

shared_ptr to this class

Definition at line 97 of file JacobianFactor.h.

◆ This

typedef JacobianFactor gtsam::JacobianFactor::This

Typedef to this class.

Definition at line 95 of file JacobianFactor.h.

Constructor & Destructor Documentation

◆ JacobianFactor() [1/15]

JacobianFactor::JacobianFactor ( const GaussianFactor & gf )

explicit

Convert from other GaussianFactor

Definition at line 53 of file JacobianFactor.cpp.

◆ JacobianFactor() [2/15]

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

inline

Copy constructor

Definition at line 115 of file JacobianFactor.h.

◆ JacobianFactor() [3/15]

JacobianFactor::JacobianFactor ( const HessianFactor & hf )

explicit

Conversion from HessianFactor (does Cholesky to obtain Jacobian matrix)

Definition at line 90 of file JacobianFactor.cpp.

◆ JacobianFactor() [4/15]

JacobianFactor::JacobianFactor ( )

default constructor for I/O

Definition at line 47 of file JacobianFactor.cpp.

◆ JacobianFactor() [5/15]

JacobianFactor::JacobianFactor ( const Vector & b_in )

explicit

Construct Null factor

Definition at line 65 of file JacobianFactor.cpp.

◆ JacobianFactor() [6/15]

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

Construct unary factor

Definition at line 71 of file JacobianFactor.cpp.

◆ JacobianFactor() [7/15]

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 77 of file JacobianFactor.cpp.

◆ JacobianFactor() [8/15]

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 83 of file JacobianFactor.cpp.

◆ JacobianFactor() [9/15]

template<typename TERMS >

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.

◆ JacobianFactor() [10/15]

template<typename KEYS >

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.

◆ JacobianFactor() [11/15]

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 324 of file JacobianFactor.cpp.

◆ JacobianFactor() [12/15]

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 349 of file JacobianFactor.cpp.

◆ JacobianFactor() [13/15]

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 374 of file JacobianFactor.cpp.

◆ JacobianFactor() [14/15]

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 390 of file JacobianFactor.cpp.

◆ ~JacobianFactor()

gtsam::JacobianFactor::~JacobianFactor ( )

inlineoverride

Virtual destructor

Definition at line 187 of file JacobianFactor.h.

◆ JacobianFactor() [15/15]

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 410 of file JacobianFactor.h.

Member Function Documentation

◆ augmentedInformation()

Matrix JacobianFactor::augmentedInformation ( ) const

overridevirtual

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.

Implements gtsam::GaussianFactor.

Definition at line 494 of file JacobianFactor.cpp.

◆ augmentedJacobian()

Matrix JacobianFactor::augmentedJacobian ( ) const

overridevirtual

Return (dense) matrix associated with factor. The returned system is an augmented matrix: [A b] weights are baked in

Implements gtsam::GaussianFactor.

Definition at line 730 of file JacobianFactor.cpp.

◆ augmentedJacobianUnweighted()

Matrix JacobianFactor::augmentedJacobianUnweighted ( ) const

Return (dense) matrix associated with factor. The returned system is an augmented matrix: [A b] weights are not baked in

Definition at line 738 of file JacobianFactor.cpp.

◆ clone()

GaussianFactor::shared_ptr gtsam::JacobianFactor::clone ( ) const

inlineoverridevirtual

Clone this JacobianFactor

Implements gtsam::GaussianFactor.

Reimplemented in gtsam::LinearInequality, gtsam::LinearEquality, and gtsam::LinearCost.

Definition at line 190 of file JacobianFactor.h.

◆ cols()

size_t gtsam::JacobianFactor::cols ( void ) const

inline

return the number of columns in the corresponding linear system

Definition at line 289 of file JacobianFactor.h.

◆ eliminate()

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

Eliminate the requested variables.

Definition at line 760 of file JacobianFactor.cpp.

◆ equals()

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

overridevirtual

assert equality up to a tolerance

Implements gtsam::GaussianFactor.

Reimplemented in gtsam::LinearInequality, gtsam::LinearEquality, and gtsam::LinearCost.

Definition at line 420 of file JacobianFactor.cpp.

◆ error()

double JacobianFactor::error ( const VectorValues & c ) const

overridevirtual

Reimplemented from gtsam::GaussianFactor.

Reimplemented in gtsam::LinearInequality, gtsam::LinearEquality, and gtsam::LinearCost.

Definition at line 486 of file JacobianFactor.cpp.

◆ error_vector()

Vector JacobianFactor::error_vector ( const VectorValues & c ) const

(A*x-b)

Definition at line 479 of file JacobianFactor.cpp.

◆ fillTerms()

template<typename TERMS >

void JacobianFactor::fillTerms	(	const TERMS &	terms,
		const Vector &	b,
		const SharedDiagonal &	noiseModel
	)

protected

Internal function to fill blocks and set dimensions.

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

◆ get_model() [1/2]

const SharedDiagonal& gtsam::JacobianFactor::get_model ( ) const

inline

get a copy of model

Definition at line 292 of file JacobianFactor.h.

◆ get_model() [2/2]

SharedDiagonal& gtsam::JacobianFactor::get_model ( )

inline

get a copy of model (non-const version)

Definition at line 295 of file JacobianFactor.h.

◆ getA() [1/4]

constABlock gtsam::JacobianFactor::getA ( const_iterator variable ) const

inline

Get a view of the A matrix for the variable pointed to by the given key iterator

Definition at line 301 of file JacobianFactor.h.

◆ getA() [2/4]

constABlock gtsam::JacobianFactor::getA ( ) const

inline

Get a view of the A matrix, not weighted by noise

Definition at line 304 of file JacobianFactor.h.

◆ getA() [3/4]

ABlock gtsam::JacobianFactor::getA ( iterator variable )

inline

Get a view of the A matrix for the variable pointed to by the given key iterator (non-const version)

Definition at line 310 of file JacobianFactor.h.

◆ getA() [4/4]

ABlock gtsam::JacobianFactor::getA ( )

inline

Get a view of the A matrix

Definition at line 313 of file JacobianFactor.h.

◆ getb() [1/2]

const constBVector gtsam::JacobianFactor::getb ( ) const

inline

Get a view of the r.h.s. vector b, not weighted by noise

Definition at line 298 of file JacobianFactor.h.

◆ getb() [2/2]

BVector gtsam::JacobianFactor::getb ( )

inline

Get a view of the r.h.s. vector b (non-const version)

Definition at line 307 of file JacobianFactor.h.

◆ getDim()

DenseIndex gtsam::JacobianFactor::getDim ( const_iterator variable ) const

inlineoverridevirtual

Return the dimension of the variable pointed to by the given key iterator todo: Remove this in favor of keeping track of dimensions with variables?

Implements gtsam::GaussianFactor.

Definition at line 277 of file JacobianFactor.h.

◆ gradient()

Vector JacobianFactor::gradient	(	Key	key,
		const VectorValues &	x
	)		const

overridevirtual

Compute the gradient wrt a key at any values.

Implements gtsam::GaussianFactor.

Definition at line 707 of file JacobianFactor.cpp.

◆ gradientAtZero() [1/2]

VectorValues JacobianFactor::gradientAtZero ( ) const

overridevirtual

A'*b for Jacobian.

Implements gtsam::GaussianFactor.

Reimplemented in gtsam::RegularJacobianFactor< D >.

Definition at line 690 of file JacobianFactor.cpp.

◆ gradientAtZero() [2/2]

void JacobianFactor::gradientAtZero ( double * d ) const

overridevirtual

A'*b for Jacobian (raw memory version)

Implements gtsam::GaussianFactor.

Reimplemented in gtsam::RegularJacobianFactor< D >.

Definition at line 702 of file JacobianFactor.cpp.

◆ hessianBlockDiagonal()

map< Key, Matrix > JacobianFactor::hessianBlockDiagonal ( ) const

overridevirtual

Return the block diagonal of the Hessian for this factor.

Implements gtsam::GaussianFactor.

Definition at line 550 of file JacobianFactor.cpp.

◆ hessianDiagonal()

void JacobianFactor::hessianDiagonal ( double * d ) const

overridevirtual

Raw memory access version of hessianDiagonal.

Implements gtsam::GaussianFactor.

Reimplemented in gtsam::RegularJacobianFactor< D >.

Definition at line 545 of file JacobianFactor.cpp.

◆ hessianDiagonalAdd()

void JacobianFactor::hessianDiagonalAdd ( VectorValues & d ) const

overridevirtual

Add the current diagonal to a VectorValues instance.

Implements gtsam::GaussianFactor.

Definition at line 516 of file JacobianFactor.cpp.

◆ information()

Matrix JacobianFactor::information ( ) const

overridevirtual

Return the non-augmented information matrix represented by this GaussianFactor.

Implements gtsam::GaussianFactor.

Definition at line 505 of file JacobianFactor.cpp.

◆ isConstrained()

bool gtsam::JacobianFactor::isConstrained ( ) const

inline

is noise model constrained ?

Definition at line 270 of file JacobianFactor.h.

◆ jacobian()

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

overridevirtual

Returns (dense) A,b pair associated with factor, bakes in the weights.

Implements gtsam::GaussianFactor.

Definition at line 714 of file JacobianFactor.cpp.

◆ JacobianFactorHelper()

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

private

Helper function for public constructors: Build a dense joint factor from all the factors in a factor graph. Takes in ordered variable slots

Definition at line 206 of file JacobianFactor.cpp.

◆ jacobianUnweighted()

pair< Matrix, Vector > JacobianFactor::jacobianUnweighted ( ) const

Returns (dense) A,b pair associated with factor, does not bake in weights.

Definition at line 723 of file JacobianFactor.cpp.

◆ matrixObject() [1/2]

const VerticalBlockMatrix& gtsam::JacobianFactor::matrixObject ( ) const

inline

Return the full augmented Jacobian matrix of this factor as a VerticalBlockMatrix object.

Definition at line 257 of file JacobianFactor.h.

◆ matrixObject() [2/2]

VerticalBlockMatrix& gtsam::JacobianFactor::matrixObject ( )

inline

Mutable access to the full augmented Jacobian matrix of this factor as a VerticalBlockMatrix object.

Definition at line 260 of file JacobianFactor.h.

◆ multiplyHessianAdd() [1/2]

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

overridevirtual

y += alpha * A'*A*x

Implements gtsam::GaussianFactor.

Reimplemented in gtsam::RegularJacobianFactor< D >.

Definition at line 638 of file JacobianFactor.cpp.

◆ multiplyHessianAdd() [2/2]

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

Raw memory access version of multiplyHessianAdd y += alpha * A'*A*x Requires the vector accumulatedDims to tell the dimension of each variable: e.g.: x0 has dim 3, x2 has dim 6, x3 has dim 2, then accumulatedDims is [0 3 9 11 13] NOTE: size of accumulatedDims is size of keys + 1!! TODO(frank): we should probably kill this if no longer needed

Raw memory access version of multiplyHessianAdd y += alpha * A'*A*x Note: this is not assuming a fixed dimension for the variables, but requires the vector accumulatedDims to tell the dimension of each variable: e.g.: x0 has dim 3, x2 has dim 6, x3 has dim 2, then accumulatedDims is [0 3 9 11 13] NOTE: size of accumulatedDims is size of keys + 1!! TODO Frank asks: why is this here if not regular ????

Use Eigen magic to access raw memory

Just iterate over all A matrices and multiply in correct config part (looping over keys) E.g.: Jacobian A = [A0 A1 A2] multiplies x = [x0 x1 x2]' Hence: Ax = A0 x0 + A1 x1 + A2 x2 (hence we loop over the keys and accumulate)

Deal with noise properly, need to Double* whiten as we are dividing by variance

multiply with alpha

Again iterate over all A matrices and insert Ai^T into y

Definition at line 653 of file JacobianFactor.cpp.

◆ negate()

GaussianFactor::shared_ptr JacobianFactor::negate ( ) const

overridevirtual

Construct the corresponding anti-factor to negate information stored stored in this factor.

Returns: a HessianFactor with negated Hessian matrices

Implements gtsam::GaussianFactor.

Definition at line 753 of file JacobianFactor.cpp.

◆ operator*()

Vector JacobianFactor::operator* ( const VectorValues & x ) const

Return A*x

Definition at line 601 of file JacobianFactor.cpp.

◆ print()

void JacobianFactor::print	(	const std::string &	s = `""`,
		const KeyFormatter &	formatter = `DefaultKeyFormatter`
	)		const

overridevirtual

print with optional string

Implements gtsam::GaussianFactor.

Reimplemented in gtsam::LinearInequality, gtsam::LinearEquality, and gtsam::LinearCost.

Definition at line 403 of file JacobianFactor.cpp.

◆ rows()

size_t gtsam::JacobianFactor::rows ( void ) const

inline

return the number of rows in the corresponding linear system

Definition at line 284 of file JacobianFactor.h.

◆ setModel()

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

set noiseModel correctly

Definition at line 768 of file JacobianFactor.cpp.

◆ splitConditional()

GaussianConditional::shared_ptr JacobianFactor::splitConditional ( size_t nrFrontals )

splits a pre-factorized factor into a conditional, and changes the current factor to be the remaining component. Performs same operation as eliminate(), but without running QR. NOTE: looks at dimension of noise model to determine how many rows to keep.

Parameters

nrFrontals number of keys to eliminate

Definition at line 819 of file JacobianFactor.cpp.

◆ transposeMultiplyAdd()

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

x += alpha * A'*e. If x is initially missing any values, they are created and assumed to start as zero vectors.

Definition at line 618 of file JacobianFactor.cpp.

◆ unweighted_error()

Vector JacobianFactor::unweighted_error ( const VectorValues & c ) const

Definition at line 471 of file JacobianFactor.cpp.

◆ updateHessian()

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

overridevirtual

Update an information matrix by adding the information corresponding to this factor (used internally during elimination).

Parameters

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

Implements gtsam::GaussianFactor.

Definition at line 563 of file JacobianFactor.cpp.

◆ whiten()

JacobianFactor JacobianFactor::whiten ( ) const

Return a whitened version of the factor, i.e. with unit diagonal noise model.

Definition at line 743 of file JacobianFactor.cpp.

Friends And Related Function Documentation

◆ ExpressionFactor

template<typename T >

friend class ExpressionFactor

friend

Definition at line 416 of file JacobianFactor.h.

Member Data Documentation

◆ Ab_

VerticalBlockMatrix gtsam::JacobianFactor::Ab_

protected

Definition at line 106 of file JacobianFactor.h.

◆ model_

noiseModel::Diagonal::shared_ptr gtsam::JacobianFactor::model_

protected

Definition at line 107 of file JacobianFactor.h.

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

Public Types

Public Member Functions

Protected Member Functions

Protected Attributes

Private Member Functions

Friends

Additional Inherited Members

Detailed Description

Member Typedef Documentation

◆ ABlock

◆ Base

◆ BVector

◆ constABlock

◆ constBVector

◆ shared_ptr

◆ This

Constructor & Destructor Documentation

◆ JacobianFactor() [1/15]

◆ JacobianFactor() [2/15]

◆ JacobianFactor() [3/15]

◆ JacobianFactor() [4/15]

◆ JacobianFactor() [5/15]

◆ JacobianFactor() [6/15]

◆ JacobianFactor() [7/15]

◆ JacobianFactor() [8/15]

◆ JacobianFactor() [9/15]

◆ JacobianFactor() [10/15]

◆ JacobianFactor() [11/15]

◆ JacobianFactor() [12/15]

◆ JacobianFactor() [13/15]

◆ JacobianFactor() [14/15]

◆ ~JacobianFactor()

◆ JacobianFactor() [15/15]

Member Function Documentation

◆ augmentedInformation()

◆ augmentedJacobian()

◆ augmentedJacobianUnweighted()

◆ clone()

◆ cols()

◆ eliminate()

◆ equals()

◆ error()

◆ error_vector()

◆ fillTerms()

◆ get_model() [1/2]

◆ get_model() [2/2]

◆ getA() [1/4]

◆ getA() [2/4]

◆ getA() [3/4]

◆ getA() [4/4]

◆ getb() [1/2]

◆ getb() [2/2]

◆ getDim()

◆ gradient()

◆ gradientAtZero() [1/2]

◆ gradientAtZero() [2/2]

◆ hessianBlockDiagonal()

◆ hessianDiagonal()

◆ hessianDiagonalAdd()

◆ information()

◆ isConstrained()

◆ jacobian()

◆ JacobianFactorHelper()

◆ jacobianUnweighted()

◆ matrixObject() [1/2]

◆ matrixObject() [2/2]

◆ multiplyHessianAdd() [1/2]

◆ multiplyHessianAdd() [2/2]

◆ negate()

◆ operator*()

◆ print()

◆ rows()

◆ setModel()

◆ splitConditional()

◆ transposeMultiplyAdd()

◆ unweighted_error()

◆ updateHessian()

◆ whiten()

Friends And Related Function Documentation

◆ ExpressionFactor