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.

Inheritance diagram for gtsam::RegularHessianFactor< D >:

Public Member Functions
	RegularHessianFactor (const KeyVector &js, const std::vector< Matrix > &Gs, const std::vector< Vector > &gs, double f)
	Construct an n-way factor from supplied components.
	RegularHessianFactor (Key j1, Key j2, const MatrixD &G11, const MatrixD &G12, const VectorD &g1, const MatrixD &G22, const VectorD &g2, double f)
	Construct a binary factor.
	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)
	Construct a ternary factor.
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.
	RegularHessianFactor (const RegularJacobianFactor< D > &jf)
	Construct a RegularHessianFactor from a RegularJacobianFactor.
	RegularHessianFactor (const GaussianFactorGraph &factors, const Scatter &scatter)
	Construct from a GaussianFactorGraph combined using a Scatter.
	RegularHessianFactor (const GaussianFactorGraph &factors)
	Construct from a GaussianFactorGraph.
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.
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.
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.
void	hessianDiagonal (double *d) const override
	Return the diagonal of the Hessian for this factor (Raw memory version).
void	gradientAtZero (double *d) const override
	Add the gradient vector \( -g \) (gradient at zero) to a raw memory block d.
Public Member Functions inherited from gtsam::HessianFactor
	HessianFactor ()
	default constructor for I/O
	HessianFactor (Key j, const Matrix &G, const Vector &g, double f)
	Construct a unary factor.
	HessianFactor (Key j, const Vector &mu, const Matrix &Sigma)
	Construct a unary factor, given a mean and covariance matrix.
	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.
	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.
	HessianFactor (const KeyVector &js, const std::vector< Matrix > &Gs, const std::vector< Vector > &gs, double f)
	Construct an n-way factor.
template<typename KEYS>
	HessianFactor (const KEYS &keys, const SymmetricBlockMatrix &augmentedInformation)
	Construct from keys and a copied augmented information block matrix.
template<typename KEYS>
	HessianFactor (const KEYS &keys, SymmetricBlockMatrix &&augmentedInformation)
	Construct from keys and take ownership of an augmented information block matrix.
	HessianFactor (const JacobianFactor &cg)
	Construct from a JacobianFactor (or from a GaussianConditional since it derives from it).
	HessianFactor (const GaussianFactor &factor)
	Attempt to construct from any GaussianFactor - currently supports JacobianFactor, HessianFactor, GaussianConditional, or any derived classes.
	HessianFactor (const GaussianFactorGraph &factors, const Scatter &scatter)
	Combine a set of factors into a single dense HessianFactor.
	HessianFactor (const GaussianFactorGraph &factors)
	Combine a set of factors into a single dense HessianFactor.
	~HessianFactor () override
	Destructor.
GaussianFactor::shared_ptr	clone () const override
	Clone this HessianFactor.
void	print (const std::string &s="", const KeyFormatter &formatter=DefaultKeyFormatter) const override
	Print the factor for debugging and testing (implementing Testable).
bool	equals (const GaussianFactor &lf, double tol=1e-9) const override
	Compare to another factor for testing (implementing Testable).
double	error (const VectorValues &c) const override
	Evaluate the factor error f(x).
double	deltaError (const VectorValues &c, double oldError=nullptr, double newError=nullptr) const override
	Compute the change in error from zero to c, optionally returning the old and new errors.
DenseIndex	getDim (const_iterator variable) const override
	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?
size_t	rows () const
	Return the number of columns and rows of the Hessian matrix, including the information vector.
GaussianFactor::shared_ptr	negate () const override
	Construct the corresponding anti-factor to negate information stored stored in this factor.
double	constantTerm () const
	Return the constant term \( f \) as described above.
double &	constantTerm ()
	Return the constant term \( f \) as described above.
SymmetricBlockMatrix::constBlock	linearTerm (const_iterator j) const
	Return the part of linear term \( g \) as described above corresponding to the requested variable.
SymmetricBlockMatrix::constBlock	linearTerm () const
	Return the complete linear term \( g \) as described above.
SymmetricBlockMatrix::Block	linearTerm ()
	Return the complete linear term \( g \) as described above.
const SymmetricBlockMatrix &	info () const
	Return underlying information matrix.
SymmetricBlockMatrix &	info ()
	Return non-const information matrix.
Matrix	augmentedInformation () const override
	Return the augmented information matrix represented by this GaussianFactor.
Eigen::SelfAdjointView< SymmetricBlockMatrix::constBlock, Eigen::Upper >	informationView () const
	Return self-adjoint view onto the information matrix (NOT augmented).
Matrix	information () const override
	Return the non-augmented information matrix represented by this GaussianFactor.
void	hessianDiagonalAdd (VectorValues &d) const override
	Add the current diagonal to a VectorValues instance.
std::map< Key, Matrix >	hessianBlockDiagonal () const override
	Return the block diagonal of the Hessian for this factor.
std::pair< Matrix, Vector >	jacobian () const override
	Return (dense) matrix associated with factor.
Matrix	augmentedJacobian () const override
	Return (dense) matrix associated with factor The returned system is an augmented matrix: [A b].
void	updateHessian (const KeyVector &keys, SymmetricBlockMatrix *info) const override
	Update an information matrix by adding the information corresponding to this factor (used internally during elimination).
void	updateHessian (const KeyVector &keys, SymmetricBlockMatrix *info, DenseIndex beginCol, DenseIndex endCol) const override
	Update an information matrix by adding the information corresponding to this factor (used internally during elimination), restricted to a range of block columns, useful for parallelization.
void	updateHessian (HessianFactor *other) const
	Update another Hessian factor.
VectorValues	gradientAtZero () const override
	eta for Hessian
Vector	gradient (Key key, const VectorValues &x) const override
	Compute the gradient at a key: \( \grad f(x_i) = \sum_j G_ij*x_j - g_i \).
std::shared_ptr< GaussianConditional >	eliminateCholesky (const Ordering &keys)
	In-place elimination that returns a conditional on (ordered) keys specified, and leaves this factor to be on the remaining keys (separator) only.
VectorValues	solve ()
	Solve the system A'A delta = A'b in-place, return delta as VectorValues.
double	error (const HybridValues &c) const override
	All factor types need to implement an error function.
VectorValues	hessianDiagonal () const
	Using the base method.
Public Member Functions inherited from gtsam::GaussianFactor
	GaussianFactor ()
	Default constructor creates empty factor.
template<typename CONTAINER>
	GaussianFactor (const CONTAINER &keys)
	Construct from container of keys.
VectorValues	hessianDiagonal () const
	Return the diagonal of the Hessian for this factor.
virtual bool	isJacobian () const
	Fast check for JacobianFactor-based types.
Public Member Functions inherited from gtsam::Factor
virtual	~Factor ()=default
	Default destructor.
bool	empty () const
	Whether the factor is empty (involves zero variables).
Key	front () const
	First key.
Key	back () const
	Last key.
const_iterator	find (Key key) const
	find
const KeyVector &	keys () const
	Access the factor's involved variable keys.
const_iterator	begin () const
	Iterator at beginning of involved variable keys.
const_iterator	end () const
	Iterator at end of involved variable keys.
size_t	size () const
virtual void	printKeys (const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
	print only keys
bool	equals (const This &other, double tol=1e-9) const
	check equality
KeyVector &	keys ()
iterator	begin ()
	Iterator at beginning of involved variable keys.
iterator	end ()
	Iterator at end of involved variable keys.

Public Types
typedef Eigen::Matrix< double, D, 1 >	VectorD
typedef Eigen::Matrix< double, D, D >	MatrixD
Public Types inherited from gtsam::HessianFactor
typedef GaussianFactor	Base
	Typedef to base class.
typedef HessianFactor	This
	Typedef to this class.
typedef std::shared_ptr< This >	shared_ptr
	A shared_ptr to this class.
typedef SymmetricBlockMatrix::Block	Block
	A block from the Hessian matrix.
typedef SymmetricBlockMatrix::constBlock	constBlock
	A block from the Hessian matrix (const version).
Public Types inherited from gtsam::GaussianFactor
typedef GaussianFactor	This
	This class.
typedef std::shared_ptr< This >	shared_ptr
	shared_ptr to this class
typedef Factor	Base
	Our base class.
Public Types inherited from gtsam::Factor
typedef KeyVector::iterator	iterator
	Iterator over keys.
typedef KeyVector::const_iterator	const_iterator
	Const iterator over keys.

Additional Inherited Members
template<typename CONTAINER>
static DenseIndex	Slot (const CONTAINER &keys, Key key)
	Factor ()
	Default constructor for I/O.
template<typename CONTAINER>
	Factor (const CONTAINER &keys)
	Construct factor from container of keys.
template<typename ITERATOR>
	Factor (ITERATOR first, ITERATOR last)
	Construct factor from iterator keys.
template<typename CONTAINER>
static Factor	FromKeys (const CONTAINER &keys)
	Construct factor from container of keys.
template<typename ITERATOR>
static Factor	FromIterators (ITERATOR first, ITERATOR last)
	Construct factor from iterator keys.
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].
Protected Attributes inherited from gtsam::Factor
KeyVector	keys_
	The keys involved in this factor.

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 \).

◆ 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.

◆ 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.

◆ 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.

◆ 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.

◆ 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.

◆ 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.

Member Function Documentation

◆ 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.

◆ 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.

◆ 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).

◆ 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.

◆ 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.

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

/tmp/gtsam-4.3.0-doxygen.rsXPUS/source/gtsam/linear/RegularHessianFactor.h

Detailed Description

Public Member Functions

Public Types

Additional Inherited Members

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

◆ gradientAtZero()

◆ hessianDiagonal()

◆ multiplyHessianAdd() [1/3]

◆ multiplyHessianAdd() [2/3]

◆ multiplyHessianAdd() [3/3]