A HessianFactor where all variables have the same dimension D. More...

#include <RegularHessianFactor.h>

Inheritance diagram for gtsam::RegularHessianFactor< D >:

[legend]

Public Types
typedef Eigen::Matrix< double, D, D >	MatrixD

typedef Eigen::Matrix< double, D, 1 >	VectorD

Public Types inherited from gtsam::HessianFactor
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
void	gradientAtZero (double *d) const override
	Add the gradient vector (gradient at zero) to a raw memory block `d`. More...

void	hessianDiagonal (double *d) const override
	Return the diagonal of the Hessian for this factor (Raw memory version). More...

void	multiplyHessianAdd (double alpha, const double x, double yvalues) const
	Multiply the Hessian part of the factor times a raw vector `x` and add the result to `y`. (Raw memory version) More...

void	multiplyHessianAdd (double alpha, const double x, double yvalues, const std::vector< size_t > &offsets) const
	Multiply the Hessian part of the factor times a raw vector `x` and add the result to `y`. More...

void	multiplyHessianAdd (double alpha, const VectorValues &x, VectorValues &y) const override
	Multiply the Hessian part of the factor times a VectorValues `x` and add the result to `y`. More...

	RegularHessianFactor (const GaussianFactorGraph &factors)
	Construct from a GaussianFactorGraph. More...

	RegularHessianFactor (const GaussianFactorGraph &factors, const Scatter &scatter)
	Construct from a GaussianFactorGraph combined using a Scatter. More...

template<typename KEYS >
	RegularHessianFactor (const KEYS &keys, const SymmetricBlockMatrix &augmentedInformation)
	Constructor with an arbitrary number of keys and the augmented information matrix specified as a block matrix. More...

	RegularHessianFactor (const KeyVector &js, const std::vector< Matrix > &Gs, const std::vector< Vector > &gs, double f)
	Construct an n-way factor from supplied components. More...

	RegularHessianFactor (const RegularJacobianFactor< D > &jf)
	Construct a RegularHessianFactor from a RegularJacobianFactor. More...

	RegularHessianFactor (Key j1, Key j2, const MatrixD &G11, const MatrixD &G12, const VectorD &g1, const MatrixD &G22, const VectorD &g2, double f)

	RegularHessianFactor (Key j1, Key j2, Key j3, const MatrixD &G11, const MatrixD &G12, const MatrixD &G13, const VectorD &g1, const MatrixD &G22, const MatrixD &G23, const VectorD &g2, const MatrixD &G33, const VectorD &g3, double f)

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

std::map< Key, Matrix >	hessianBlockDiagonal () const override
	Return the block diagonal of the Hessian for this factor. 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

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

Private Types
typedef Eigen::Map< const VectorD >	ConstDMap

typedef Eigen::Map< VectorD >	DMap

Private Member Functions
void	checkInvariants () const
	Check if the factor has regular block structure. More...

Private Attributes
std::vector< VectorD, Eigen::aligned_allocator< VectorD > >	y_

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)

Protected Attributes inherited from gtsam::HessianFactor
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...

Detailed Description

template<size_t D>
class gtsam::RegularHessianFactor< D >

A HessianFactor where all variables have the same dimension D.

Represents a quadratic factor $ E(x) = 0.5 x^T G x - g^T x + 0.5 f $ , corresponding to a Gaussian probability density $P(x) \propto \exp(-E(x))$ . Here, G is the Hessian (or information matrix), g is the gradient vector, and f is a constant term (negative log-likelihood at x=0).

This class is templated on the dimension D and enforces that all variables associated with this factor have this dimension during construction. It inherits from HessianFactor but provides specialized, efficient implementations for operations like multiplyHessianAdd using raw memory access, assuming a regular block structure. This can significantly improve performance in iterative optimization algorithms when dealing with many variables of the same type and dimension (e.g., Pose3 variables in SLAM).

It can be constructed directly from Hessian blocks, from a RegularJacobianFactor, or by linearizing a NonlinearFactorGraph.

Template Parameters

D	The dimension of each variable block involved in the factor.

Definition at line 57 of file RegularHessianFactor.h.

Member Typedef Documentation

◆ ConstDMap

template<size_t D>

typedef Eigen::Map<const VectorD> gtsam::RegularHessianFactor< D >::ConstDMap

private

Definition at line 167 of file RegularHessianFactor.h.

◆ DMap

template<size_t D>

typedef Eigen::Map<VectorD> gtsam::RegularHessianFactor< D >::DMap

private

Definition at line 166 of file RegularHessianFactor.h.

◆ MatrixD

template<size_t D>

typedef Eigen::Matrix<double, D, D> gtsam::RegularHessianFactor< D >::MatrixD

Definition at line 62 of file RegularHessianFactor.h.

◆ VectorD

template<size_t D>

typedef Eigen::Matrix<double, D, 1> gtsam::RegularHessianFactor< D >::VectorD

Definition at line 61 of file RegularHessianFactor.h.

Constructor & Destructor Documentation

◆ RegularHessianFactor() [1/7]

template<size_t D>

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

inline

Construct an n-way factor from supplied components.

Represents the energy function $E(x) = 0.5 \sum_{i,j} x_i^T G_{ij} x_j - \sum_i g_i^T x_i + 0.5 f$ . Note that the keys in js must correspond order-wise to the blocks in Gs and gs.

Parameters

js	Vector of keys involved in the factor.
Gs	Vector of upper-triangular Hessian blocks (row-major order, e.g., G11, G12, G13, G22, G23, G33 for a ternary factor).
gs	Vector of gradient vector blocks (-J^T b).
f	Constant term $0.5f = 0.5\\|b\\|^2$ .

Definition at line 73 of file RegularHessianFactor.h.

◆ RegularHessianFactor() [2/7]

template<size_t D>

gtsam::RegularHessianFactor< D >::RegularHessianFactor	(	Key	j1,
		Key	j2,
		const MatrixD &	G11,
		const MatrixD &	G12,
		const VectorD &	g1,
		const MatrixD &	G22,
		const VectorD &	g2,
		double	f
	)

inline

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.

Definition at line 83 of file RegularHessianFactor.h.

◆ RegularHessianFactor() [3/7]

template<size_t D>

gtsam::RegularHessianFactor< D >::RegularHessianFactor	(	Key	j1,
		Key	j2,
		Key	j3,
		const MatrixD &	G11,
		const MatrixD &	G12,
		const MatrixD &	G13,
		const VectorD &	g1,
		const MatrixD &	G22,
		const MatrixD &	G23,
		const VectorD &	g2,
		const MatrixD &	G33,
		const VectorD &	g3,
		double	f
	)

inline

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 92 of file RegularHessianFactor.h.

◆ RegularHessianFactor() [4/7]

template<size_t D>

template<typename KEYS >

gtsam::RegularHessianFactor< D >::RegularHessianFactor	(	const KEYS &	keys,
		const SymmetricBlockMatrix &	augmentedInformation
	)

inline

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

The augmented information matrix contains the Hessian blocks G and the gradient blocks g, arranged as $ [ G, -g ; -g^T, f ] $ .

Parameters

keys	Container of keys specifying the variables involved and their order.
augmentedInformation	The augmented information matrix [H -g; -g' f], laid out as a SymmetricBlockMatrix.

Definition at line 108 of file RegularHessianFactor.h.

◆ RegularHessianFactor() [5/7]

template<size_t D>

gtsam::RegularHessianFactor< D >::RegularHessianFactor ( const RegularJacobianFactor< D > & jf )

inline

Construct a RegularHessianFactor from a RegularJacobianFactor.

Computes $ G = J^T J $ , $ g = J^T b $ , and $ f = b^T b $ . Requires that the JacobianFactor also has regular block sizes matching D.

Parameters

jf	The RegularJacobianFactor to convert.

Definition at line 120 of file RegularHessianFactor.h.

◆ RegularHessianFactor() [6/7]

template<size_t D>

gtsam::RegularHessianFactor< D >::RegularHessianFactor	(	const GaussianFactorGraph &	factors,
		const Scatter &	scatter
	)

inline

Construct from a GaussianFactorGraph combined using a Scatter.

This is typically used internally by optimizers. It sums the Hessian contributions from multiple factors in the graph.

Parameters

factors	The graph containing factors to combine.
scatter	A Scatter structure indicating the layout of variables.

Definition at line 131 of file RegularHessianFactor.h.

◆ RegularHessianFactor() [7/7]

template<size_t D>

gtsam::RegularHessianFactor< D >::RegularHessianFactor ( const GaussianFactorGraph & factors )

inline

Construct from a GaussianFactorGraph.

This is typically used internally by optimizers. It sums the Hessian contributions from multiple factors in the graph. Assumes keys are ordered 0..n-1.

Parameters

factors The graph containing factors to combine.

Definition at line 143 of file RegularHessianFactor.h.

Member Function Documentation

◆ checkInvariants()

template<size_t D>

void gtsam::RegularHessianFactor< D >::checkInvariants ( ) const

inlineprivate

Check if the factor has regular block structure.

Verifies that the dimensions of the stored augmented information matrix info_ correspond to the expected size based on the number of keys and the fixed block dimension D. Throws std::invalid_argument if the structure is not regular. This is called internally by constructors.

Definition at line 157 of file RegularHessianFactor.h.

◆ gradientAtZero()

template<size_t D>

void gtsam::RegularHessianFactor< D >::gradientAtZero ( double * d ) const

inlineoverridevirtual

Add the gradient vector $ -g $ (gradient at zero) to a raw memory block d.

Adds the gradient vector $ -g $ (where the factor represents $ 0.5 x^T G x - g^T x + 0.5 f $ ) to the pre-allocated raw memory block d. Assumes d has the standard contiguous layout d + keys_[i] * D.

Parameters

[in,out] d Raw pointer to the memory block where the gradient should be added.

Reimplemented from gtsam::HessianFactor.

Definition at line 320 of file RegularHessianFactor.h.

◆ hessianDiagonal()

template<size_t D>

void gtsam::RegularHessianFactor< D >::hessianDiagonal ( double * d ) const

inlineoverridevirtual

Return the diagonal of the Hessian for this factor (Raw memory version).

Adds the diagonal elements of the Hessian matrix $ H = G $ associated with this factor to the pre-allocated raw memory block d. Assumes d has the standard contiguous layout d + keys_[i] * D.

Parameters

[in,out] d Raw pointer to the memory block where Hessian diagonals should be added.

Reimplemented from gtsam::HessianFactor.

Definition at line 302 of file RegularHessianFactor.h.

◆ multiplyHessianAdd() [1/3]

template<size_t D>

void gtsam::RegularHessianFactor< D >::multiplyHessianAdd	(	double	alpha,
		const double *	x,
		double *	yvalues
	)		const

inline

Multiply the Hessian part of the factor times a raw vector x and add the result to y. (Raw memory version)

Computes y += alpha * H * x, optimized for regular block structure. Assumes x and yvalues are pre-allocated, contiguous memory blocks where variable j corresponds to memory chunk x + keys_[j] * D and yvalues + keys_[j] * D.

Parameters

	alpha	Scalar multiplier.
	x	Raw pointer to the input vector data.
[in,out]	yvalues	Raw pointer to the output vector data (accumulates).

Definition at line 202 of file RegularHessianFactor.h.

◆ multiplyHessianAdd() [2/3]

template<size_t D>

void gtsam::RegularHessianFactor< D >::multiplyHessianAdd	(	double	alpha,
		const double *	x,
		double *	yvalues,
		const std::vector< size_t > &	offsets
	)		const

inline

Multiply the Hessian part of the factor times a raw vector x and add the result to y.

Computes y += alpha * H * x, optimized for regular block structure, but handles potentially non-contiguous or scattered memory layouts for x and yvalues as defined by the offsets vector. offsets[k] should give the starting index of variable k in the raw memory arrays. offsets[k+1] - offsets[k] should equal D.

Parameters

	alpha	Scalar multiplier.
	x	Raw pointer to the input vector data.
[in,out]	yvalues	Raw pointer to the output vector data (accumulates).
	offsets	Vector mapping variable keys to their starting index in `x` and `yvalues`.

Definition at line 252 of file RegularHessianFactor.h.

◆ multiplyHessianAdd() [3/3]

template<size_t D>

void gtsam::RegularHessianFactor< D >::multiplyHessianAdd	(	double	alpha,
		const VectorValues &	x,
		VectorValues &	y
	)		const

inlineoverridevirtual

Multiply the Hessian part of the factor times a VectorValues x and add the result to y.

Computes y += alpha * H * x.

Parameters

	alpha	Scalar multiplier.
	x	VectorValues containing the vector to multiply with.
[in,out]	y	VectorValues to store the result (accumulates).

Reimplemented from gtsam::HessianFactor.

Definition at line 183 of file RegularHessianFactor.h.

Member Data Documentation

◆ y_

template<size_t D>

std::vector<VectorD, Eigen::aligned_allocator<VectorD> > gtsam::RegularHessianFactor< D >::y_

mutableprivate

Definition at line 172 of file RegularHessianFactor.h.

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

RegularHessianFactor.h

Public Types

Public Member Functions

Private Types

Private Member Functions

Private Attributes

Additional Inherited Members

Detailed Description

template<size_t D> class gtsam::RegularHessianFactor< D >

Member Typedef Documentation

◆ ConstDMap

◆ DMap

◆ MatrixD

◆ VectorD

Constructor & Destructor Documentation

◆ RegularHessianFactor() [1/7]

◆ RegularHessianFactor() [2/7]

◆ RegularHessianFactor() [3/7]

◆ RegularHessianFactor() [4/7]

◆ RegularHessianFactor() [5/7]

◆ RegularHessianFactor() [6/7]

◆ RegularHessianFactor() [7/7]

Member Function Documentation

◆ checkInvariants()

◆ gradientAtZero()

◆ hessianDiagonal()

◆ multiplyHessianAdd() [1/3]

◆ multiplyHessianAdd() [2/3]

◆ multiplyHessianAdd() [3/3]

Member Data Documentation

◆ y_

template<size_t D>
class gtsam::RegularHessianFactor< D >