A Gaussian factor using the canonical parameters (information form) More...

#include <HessianFactor.h>

Inheritance diagram for gtsam::HessianFactor:

[legend]

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

typedef SymmetricBlockMatrix::Block	Block
	A block from the Hessian matrix. More...

typedef SymmetricBlockMatrix::constBlock	constBlock
	A block from the Hessian matrix (const version) More...

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

typedef HessianFactor	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

GaussianFactor::shared_ptr	clone () const override

double &	constantTerm ()

double	constantTerm () const

std::shared_ptr< GaussianConditional >	eliminateCholesky (const Ordering &keys)

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

double	error (const HybridValues &c) const override

virtual double	error (const VectorValues &c) const

double	error (const VectorValues &c) const override

DenseIndex	getDim (const_iterator variable) const override

Vector	gradient (Key key, const VectorValues &x) const override

VectorValues	gradientAtZero () const override
	eta for Hessian More...

void	gradientAtZero (double *d) const override
	Raw memory access version of gradientAtZero. 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...

	HessianFactor ()

	HessianFactor (const GaussianFactor &factor)

	HessianFactor (const GaussianFactorGraph &factors)

	HessianFactor (const GaussianFactorGraph &factors, const Scatter &scatter)

	HessianFactor (const JacobianFactor &cg)

template<typename KEYS >
	HessianFactor (const KEYS &keys, const SymmetricBlockMatrix &augmentedInformation)

	HessianFactor (const KeyVector &js, const std::vector< Matrix > &Gs, const std::vector< Vector > &gs, double f)

	HessianFactor (Key j, const Matrix &G, const Vector &g, double f)

	HessianFactor (Key j, const Vector &mu, const Matrix &Sigma)

	HessianFactor (Key j1, Key j2, const Matrix &G11, const Matrix &G12, const Vector &g1, const Matrix &G22, const Vector &g2, double f)

	HessianFactor (Key j1, Key j2, Key j3, const Matrix &G11, const Matrix &G12, const Matrix &G13, const Vector &g1, const Matrix &G22, const Matrix &G23, const Vector &g2, const Matrix &G33, const Vector &g3, double f)

SymmetricBlockMatrix &	info ()

const SymmetricBlockMatrix &	info () const
	Return underlying information matrix. More...

Matrix	information () const override

Eigen::SelfAdjointView< SymmetricBlockMatrix::constBlock, Eigen::Upper >	informationView () const
	Return self-adjoint view onto the information matrix (NOT augmented). More...

std::pair< Matrix, Vector >	jacobian () const override
	Return (dense) matrix associated with factor. More...

SymmetricBlockMatrix::Block	linearTerm ()

SymmetricBlockMatrix::constBlock	linearTerm () const

SymmetricBlockMatrix::constBlock	linearTerm (const_iterator j) const

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

GaussianFactor::shared_ptr	negate () const override

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

size_t	rows () const

VectorValues	solve ()
	Solve the system A'A delta = A'b in-place, return delta as VectorValues. More...

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

void	updateHessian (HessianFactor *other) const

	~HessianFactor () 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 Attributes
SymmetricBlockMatrix	info_
	The full augmented information matrix, s.t. the quadratic error is 0.5*[x -1]'H[x -1]. More...

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

Private Member Functions
void	Allocate (const Scatter &scatter)
	Allocate for given scatter pattern. More...

	HessianFactor (const Scatter &scatter)
	Constructor with given scatter pattern, allocating but not initializing storage. More...

Friends
class	NonlinearClusterTree

class	NonlinearFactorGraph

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

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

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

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

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 using the canonical parameters (information form)

HessianFactor implements a general quadratic factor of the form

$E(x) = 0.5 x^T G x - x^T g + 0.5 f$

that stores the matrix $ G $ , the vector $ g $ , and the constant term $ f $ .

When $ G $ is positive semidefinite, this factor represents a Gaussian, in which case $ G $ is the information matrix $\Lambda$ , $ g $ is the information vector $\eta$ , and $ f $ is the residual sum-square-error at the mean, when $x = \mu$ .

Indeed, the negative log-likelihood of a Gaussian is (up to a constant) $E(x) = 0.5(x-\mu)^T P^{-1} (x-\mu)$ with $\mu$ the mean and $ P $ the covariance matrix. Expanding the product we get

$E(x) = 0.5 x^T P^{-1} x - x^T P^{-1} \mu + 0.5 \mu^T P^{-1} \mu$

We define the Information matrix (or Hessian) $\Lambda = P^{-1}$ and the information vector $\eta = P^{-1} \mu = \Lambda \mu$ to arrive at the canonical form of the Gaussian:

$E(x) = 0.5 x^T \Lambda x - x^T \eta + 0.5 \mu^T \Lambda \mu$

This factor is one of the factors that can be in a GaussianFactorGraph. It may be returned from NonlinearFactor::linearize(), but is also used internally to store the Hessian during Cholesky elimination.

This can represent a quadratic factor with characteristics that cannot be represented using a JacobianFactor (which has the form $E(x) = \Vert Ax - b \Vert^2$ and stores the Jacobian $ A $ and error vector $ b $ , i.e. is a sum-of-squares factor). For example, a HessianFactor need not be positive semidefinite, it can be indefinite or even negative semidefinite.

If a HessianFactor is indefinite or negative semi-definite, then in order for solving the linear system to be possible, the Hessian of the full system must be positive definite (i.e. when all small Hessians are combined, the result must be positive definite). If this is not the case, an error will occur during elimination.

This class stores G, g, and f as an augmented matrix HessianFactor::matrix_. The upper-left n x n blocks of HessianFactor::matrix_ store the upper-right triangle of G, the upper-right-most column of length n of HessianFactor::matrix_ stores g, and the lower-right entry of HessianFactor::matrix_ stores f, i.e.

HessianFactor::matrix_ = [ G11 G12 G13 ... g1
                             0 G22 G23 ... g2
                             0   0 G33 ... g3
                             :   :   :      :
                             0   0   0 ...  f ]

Blocks can be accessed as follows:

G11 = info(begin(), begin());
G12 = info(begin(), begin()+1);
G23 = info(begin()+1, begin()+2);
g2 = linearTerm(begin()+1);
f = constantTerm();
.......

Definition at line 100 of file HessianFactor.h.

Member Typedef Documentation

◆ Base

typedef GaussianFactor gtsam::HessianFactor::Base

Typedef to base class.

Definition at line 107 of file HessianFactor.h.

◆ Block

typedef SymmetricBlockMatrix::Block gtsam::HessianFactor::Block

A block from the Hessian matrix.

Definition at line 110 of file HessianFactor.h.

◆ constBlock

typedef SymmetricBlockMatrix::constBlock gtsam::HessianFactor::constBlock

A block from the Hessian matrix (const version)

Definition at line 111 of file HessianFactor.h.

◆ shared_ptr

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

A shared_ptr to this class.

Definition at line 109 of file HessianFactor.h.

◆ This

typedef HessianFactor gtsam::HessianFactor::This

Typedef to this class.

Definition at line 108 of file HessianFactor.h.

Constructor & Destructor Documentation

◆ HessianFactor() [1/12]

gtsam::HessianFactor::HessianFactor ( )

default constructor for I/O

Definition at line 67 of file HessianFactor.cpp.

◆ HessianFactor() [2/12]

gtsam::HessianFactor::HessianFactor	(	Key	j,
		const Matrix &	G,
		const Vector &	g,
		double	f
	)

Construct a unary factor. G is the quadratic term (Hessian matrix), g the linear term (a vector), and f the constant term. The quadratic error is: 0.5*(f - 2*x'*g + x'*G*x)

Definition at line 74 of file HessianFactor.cpp.

◆ HessianFactor() [3/12]

gtsam::HessianFactor::HessianFactor	(	Key	j,
		const Vector &	mu,
		const Matrix &	Sigma
	)

Construct a unary factor, given a mean and covariance matrix. error is 0.5*(x-mu)'inv(Sigma)(x-mu)

Definition at line 87 of file HessianFactor.cpp.

◆ HessianFactor() [4/12]

gtsam::HessianFactor::HessianFactor	(	Key	j1,
		Key	j2,
		const Matrix &	G11,
		const Matrix &	G12,
		const Vector &	g1,
		const Matrix &	G22,
		const Vector &	g2,
		double	f
	)

Construct a binary factor. Gxx are the upper-triangle blocks of the quadratic term (the Hessian matrix), gx the pieces of the linear vector term, and f the constant term. JacobianFactor error is

$0.5* (Ax-b)' M (Ax-b) = 0.5*x'A'MAx - x'A'Mb + 0.5*b'Mb$

HessianFactor error is

$0.5*(x'Gx - 2x'g + f) = 0.5*x'Gx - x'*g + 0.5*f$

So, with $ A = [A1 A2] $ and $ G=A'*M*A = [A1';A2']*M*[A1 A2] $ we have

n1*n1 G11 = A1'*M*A1
n1*n2 G12 = A1'*M*A2
n2*n2 G22 = A2'*M*A2
n1*1   g1 = A1'*M*b
n2*1   g2 = A2'*M*b
 1*1    f =  b'*M*b

Definition at line 98 of file HessianFactor.cpp.

◆ HessianFactor() [5/12]

gtsam::HessianFactor::HessianFactor	(	Key	j1,
		Key	j2,
		Key	j3,
		const Matrix &	G11,
		const Matrix &	G12,
		const Matrix &	G13,
		const Vector &	g1,
		const Matrix &	G22,
		const Matrix &	G23,
		const Vector &	g2,
		const Matrix &	G33,
		const Vector &	g3,
		double	f
	)

Construct a ternary factor. Gxx are the upper-triangle blocks of the quadratic term (the Hessian matrix), gx the pieces of the linear vector term, and f the constant term.

Definition at line 111 of file HessianFactor.cpp.

◆ HessianFactor() [6/12]

gtsam::HessianFactor::HessianFactor	(	const KeyVector &	js,
		const std::vector< Matrix > &	Gs,
		const std::vector< Vector > &	gs,
		double	f
	)

Construct an n-way factor. Gs contains the upper-triangle blocks of the quadratic term (the Hessian matrix) provided in row-order, gs the pieces of the linear vector term, and f the constant term.

Definition at line 145 of file HessianFactor.cpp.

◆ HessianFactor() [7/12]

template<typename KEYS >

gtsam::HessianFactor::HessianFactor	(	const KEYS &	keys,
		const SymmetricBlockMatrix &	augmentedInformation
	)

Constructor with an arbitrary number of keys and with the augmented information matrix specified as a block matrix.

Definition at line 25 of file HessianFactor-inl.h.

◆ HessianFactor() [8/12]

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

explicit

Construct from a JacobianFactor (or from a GaussianConditional since it derives from it)

Definition at line 217 of file HessianFactor.cpp.

◆ HessianFactor() [9/12]

gtsam::HessianFactor::HessianFactor ( const GaussianFactor & factor )

explicit

Attempt to construct from any GaussianFactor - currently supports JacobianFactor, HessianFactor, GaussianConditional, or any derived classes.

Definition at line 224 of file HessianFactor.cpp.

◆ HessianFactor() [10/12]

gtsam::HessianFactor::HessianFactor	(	const GaussianFactorGraph &	factors,
		const Scatter &	scatter
	)

explicit

Combine a set of factors into a single dense HessianFactor

Definition at line 239 of file HessianFactor.cpp.

◆ HessianFactor() [11/12]

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

inlineexplicit

Combine a set of factors into a single dense HessianFactor

Definition at line 181 of file HessianFactor.h.

◆ ~HessianFactor()

gtsam::HessianFactor::~HessianFactor ( )

inlineoverride

Destructor

Definition at line 185 of file HessianFactor.h.

◆ HessianFactor() [12/12]

gtsam::HessianFactor::HessianFactor ( const Scatter & scatter )

private

Constructor with given scatter pattern, allocating but not initializing storage.

Definition at line 62 of file HessianFactor.cpp.

Member Function Documentation

◆ Allocate()

void gtsam::HessianFactor::Allocate ( const Scatter & scatter )

private

Allocate for given scatter pattern.

Definition at line 44 of file HessianFactor.cpp.

◆ augmentedInformation()

Matrix gtsam::HessianFactor::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.

For HessianFactor, this is the same as info() except that this function returns a complete symmetric matrix whereas info() returns a matrix where only the upper triangle is valid, but should be interpreted as symmetric. This is because info() returns only a reference to the internal representation of the augmented information matrix, which stores only the upper triangle.

keys	THe ordered vector of keys for the information matrix to be updated
info	The information matrix to be updated

Public Types

Public Member Functions

Protected Attributes

Private Member Functions

Friends

Additional Inherited Members

Detailed Description

Member Typedef Documentation

◆ Base

◆ Block

◆ constBlock

◆ shared_ptr

◆ This

Constructor & Destructor Documentation

◆ HessianFactor() [1/12]

◆ HessianFactor() [2/12]

◆ HessianFactor() [3/12]

◆ HessianFactor() [4/12]

◆ HessianFactor() [5/12]

◆ HessianFactor() [6/12]

◆ HessianFactor() [7/12]

◆ HessianFactor() [8/12]

◆ HessianFactor() [9/12]

◆ HessianFactor() [10/12]

◆ HessianFactor() [11/12]

◆ ~HessianFactor()

◆ HessianFactor() [12/12]

Member Function Documentation

◆ Allocate()

◆ augmentedInformation()

◆ augmentedJacobian()

◆ clone()

◆ constantTerm() [1/2]

◆ constantTerm() [2/2]

◆ eliminateCholesky()

◆ equals()

◆ error() [1/3]

◆ error() [2/3]

◆ error() [3/3]

◆ getDim()

◆ gradient()

◆ gradientAtZero() [1/2]

◆ gradientAtZero() [2/2]

◆ hessianBlockDiagonal()

◆ hessianDiagonal()

◆ hessianDiagonalAdd()

◆ info() [1/2]

◆ info() [2/2]

◆ information()

◆ informationView()

◆ jacobian()

◆ linearTerm() [1/3]

◆ linearTerm() [2/3]

◆ linearTerm() [3/3]

◆ multiplyHessianAdd()

◆ negate()

◆ print()

◆ rows()

◆ solve()

◆ updateHessian() [1/2]

◆ updateHessian() [2/2]

Friends And Related Function Documentation

◆ NonlinearClusterTree

◆ NonlinearFactorGraph

Member Data Documentation

◆ info_