gtsam
3.2.0
gtsam

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 sumsquareerror at the mean, when \( x = \mu \).
Indeed, the negative loglikelihood 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 sumofsquares 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 semidefinite, 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 upperleft n x n blocks of HessianFactor::matrix_ store the upperright triangle of G, the upperrightmost column of length n of HessianFactor::matrix_ stores g, and the lowerright entry of HessianFactor::matrix_ stores f, i.e.
Blocks can be accessed as follows:
Public Member Functions  
HessianFactor ()  
default constructor for I/O  
HessianFactor (Key j, const Matrix &G, const Vector &g, double f)  
Construct a unary factor. More...  
HessianFactor (Key j, const Vector &mu, const Matrix &Sigma)  
Construct a unary factor, given a mean and covariance matrix. More...  
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. More...  
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. More...  
HessianFactor (const std::vector< Key > &js, const std::vector< Matrix > &Gs, const std::vector< Vector > &gs, double f)  
Construct an nway factor. More...  
template<typename KEYS >  
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. More...  
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. More...  
HessianFactor (const GaussianFactorGraph &factors, boost::optional< const Scatter & > scatter=boost::none)  
Combine a set of factors into a single dense HessianFactor.  
virtual  ~HessianFactor () 
Destructor.  
virtual GaussianFactor::shared_ptr  clone () const 
Clone this HessianFactor.  
virtual void  print (const std::string &s="", const KeyFormatter &formatter=DefaultKeyFormatter) const 
Print the factor for debugging and testing (implementing Testable)  
virtual bool  equals (const GaussianFactor &lf, double tol=1e9) const 
Compare to another factor for testing (implementing Testable)  
virtual double  error (const VectorValues &c) const 
Evaluate the factor error f(x), see above. More...  
virtual DenseIndex  getDim (const_iterator variable) const 
0.5*[x 1]'H[x 1] (also see constructor documentation) More...  
size_t  rows () const 
Return the number of columns and rows of the Hessian matrix, including the information vector. More...  
virtual GaussianFactor::shared_ptr  negate () const 
Construct the corresponding antifactor to negate information stored stored in this factor. More...  
virtual bool  empty () const 
Check if the factor is empty. More...  
constBlock  info (const_iterator j1, const_iterator j2) const 
Return a view of the block at (j1,j2) of the uppertriangular part of the information matrix \( H \), no data is copied. More...  
Block  info (iterator j1, iterator j2) 
Return a view of the block at (j1,j2) of the uppertriangular part of the information matrix \( H \), no data is copied. More...  
SymmetricBlockMatrix::constBlock  info () const 
Return the uppertriangular part of the full augmented information matrix, as described above. More...  
SymmetricBlockMatrix::Block  info () 
Return the uppertriangular part of the full augmented information matrix, as described above. More...  
double  constantTerm () const 
Return the constant term \( f \) as described above. More...  
double &  constantTerm () 
Return the constant term \( f \) as described above. More...  
constBlock::OffDiagonal::ColXpr  linearTerm (const_iterator j) const 
Return the part of linear term \( g \) as described above corresponding to the requested variable. More...  
Block::OffDiagonal::ColXpr  linearTerm (iterator j) 
Return the part of linear term \( g \) as described above corresponding to the requested variable. More...  
constBlock::OffDiagonal::ColXpr  linearTerm () const 
Return the complete linear term \( g \) as described above. More...  
Block::OffDiagonal::ColXpr  linearTerm () 
Return the complete linear term \( g \) as described above. More...  
virtual Matrix  augmentedInformation () const 
Return the augmented information matrix represented by this GaussianFactor. More...  
virtual Matrix  information () const 
Return the nonaugmented information matrix represented by this GaussianFactor.  
virtual VectorValues  hessianDiagonal () const 
Return the diagonal of the Hessian for this factor.  
virtual void  hessianDiagonal (double *d) const 
Return the diagonal of the Hessian for this factor (raw memory version)  
virtual std::map< Key, Matrix >  hessianBlockDiagonal () const 
Return the block diagonal of the Hessian for this factor.  
virtual std::pair< Matrix, Vector >  jacobian () const 
Return (dense) matrix associated with factor. More...  
virtual Matrix  augmentedJacobian () const 
Return (dense) matrix associated with factor The returned system is an augmented matrix: [A b]. More...  
const SymmetricBlockMatrix &  matrixObject () const 
Return the full augmented Hessian matrix of this factor as a SymmetricBlockMatrix object. More...  
void  updateATA (const JacobianFactor &update, const Scatter &scatter) 
Update the factor by adding the information from the JacobianFactor (used internally during elimination). More...  
void  updateATA (const HessianFactor &update, const Scatter &scatter) 
Update the factor by adding the information from the HessianFactor (used internally during elimination). More...  
void  multiplyHessianAdd (double alpha, const VectorValues &x, VectorValues &y) const 
y += alpha * A'*A*x  
void  multiplyHessianAdd (double alpha, const double *x, double *y, std::vector< size_t > keys) const 
y += alpha * A'*A*x  
void  multiplyHessianAdd (double alpha, const double *x, double *y) const 
y += alpha * A'*A*x  
VectorValues  gradientAtZero () const 
eta for Hessian  
virtual void  gradientAtZero (double *d) const 
A'*b for Jacobian, eta for Hessian (raw memory version)  
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. More...  
virtual  ~GaussianFactor () 
Destructor.  
Public Member Functions inherited from gtsam::Factor  
Key  front () const 
First key.  
Key  back () const 
Last key.  
const_iterator  find (Key key) const 
find  
const FastVector< Key > &  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 
void  print (const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const 
print  
void  printKeys (const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const 
print only keys  
FastVector< Key > &  keys () 
iterator  begin () 
Iterator at beginning of involved variable keys.  
iterator  end () 
Iterator at end of involved variable keys.  
Public Types  
typedef GaussianFactor  Base 
Typedef to base class.  
typedef HessianFactor  This 
Typedef to this class.  
typedef boost::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 boost::shared_ptr< This >  shared_ptr 
shared_ptr to this class  
typedef Factor  Base 
Our base class.  
Public Types inherited from gtsam::Factor  
typedef FastVector< Key >::iterator  iterator 
Iterator over keys.  
typedef FastVector< Key > ::const_iterator  const_iterator 
Const iterator over keys.  
Protected Attributes  
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  
FastVector< Key >  keys_ 
The keys involved in this factor.  
Friends  
class  boost::serialization::access 
Serialization function.  
GTSAM_EXPORT std::pair < boost::shared_ptr < GaussianConditional > , boost::shared_ptr < HessianFactor > >  EliminateCholesky (const GaussianFactorGraph &factors, const Ordering &keys) 
GTSAM_EXPORT std::pair < boost::shared_ptr < GaussianConditional > , boost::shared_ptr < GaussianFactor > >  EliminatePreferCholesky (const GaussianFactorGraph &factors, const Ordering &keys) 
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)
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*(xmu)'inv(Sigma)(xmu)
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 uppertriangle 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* (Axb)' M (Axb) = 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
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 uppertriangle blocks of the quadratic term (the Hessian matrix), gx the pieces of the linear vector term, and f the constant term.
gtsam::HessianFactor::HessianFactor  (  const std::vector< Key > &  js, 
const std::vector< Matrix > &  Gs,  
const std::vector< Vector > &  gs,  
double  f  
) 
Construct an nway factor.
Gs contains the uppertriangle blocks of the quadratic term (the Hessian matrix) provided in roworder, gs the pieces of the linear vector term, and f the constant term.
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.

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

virtual 
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 lowerright 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.
Implements gtsam::GaussianFactor.

virtual 
Return (dense) matrix associated with factor The returned system is an augmented matrix: [A b].
set  weight to use whitening to bake in weights 
Implements gtsam::GaussianFactor.

inline 
Return the constant term \( f \) as described above.

inline 
Return the constant term \( f \) as described above.

inlinevirtual 

virtual 
Evaluate the factor error f(x), see above.
Implements gtsam::GaussianFactor.

inlinevirtual 
0.5*[x 1]'H[x 1] (also see constructor documentation)
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?
variable  An iterator pointing to the slot in this factor. You can use, for example, begin() + 2 to get the 3rd variable in this factor. 
Implements gtsam::GaussianFactor.

inline 
Return a view of the block at (j1,j2) of the uppertriangular part of the information matrix \( H \), no data is copied.
See HessianFactor class documentation above to explain that only the uppertriangular part of the information matrix is stored and returned by this function.
j1  Which block row to get, as an iterator pointing to the slot in this factor. You can use, for example, begin() + 2 to get the 3rd variable in this factor. 
j2  Which block column to get, as an iterator pointing to the slot in this factor. You can use, for example, begin() + 2 to get the 3rd variable in this factor. 
Return a view of the block at (j1,j2) of the uppertriangular part of the information matrix \( H \), no data is copied.
See HessianFactor class documentation above to explain that only the uppertriangular part of the information matrix is stored and returned by this function.
j1  Which block row to get, as an iterator pointing to the slot in this factor. You can use, for example, begin() + 2 to get the 3rd variable in this factor. 
j2  Which block column to get, as an iterator pointing to the slot in this factor. You can use, for example, begin() + 2 to get the 3rd variable in this factor. 

inline 
Return the uppertriangular part of the full augmented information matrix, as described above.
See HessianFactor class documentation above to explain that only the uppertriangular part of the information matrix is stored and returned by this function.

inline 
Return the uppertriangular part of the full augmented information matrix, as described above.
See HessianFactor class documentation above to explain that only the uppertriangular part of the information matrix is stored and returned by this function.

virtual 
Return (dense) matrix associated with factor.
ordering  of variables needed for matrix column order 
set  weight to true to bake in the weights 
Implements gtsam::GaussianFactor.

inline 
Return the part of linear term \( g \) as described above corresponding to the requested variable.
j  Which block row to get, as an iterator pointing to the slot in this factor. You can use, for example, begin() + 2 to get the 3rd variable in this factor. 

inline 
Return the part of linear term \( g \) as described above corresponding to the requested variable.
j  Which block row to get, as an iterator pointing to the slot in this factor. You can use, for example, begin() + 2 to get the 3rd variable in this factor. 

inline 
Return the complete linear term \( g \) as described above.

inline 
Return the complete linear term \( g \) as described above.

inline 
Return the full augmented Hessian matrix of this factor as a SymmetricBlockMatrix object.

virtual 
Construct the corresponding antifactor to negate information stored stored in this factor.
Implements gtsam::GaussianFactor.

inline 
Return the number of columns and rows of the Hessian matrix, including the information vector.
void gtsam::HessianFactor::updateATA  (  const JacobianFactor &  update, 
const Scatter &  scatter  
) 
Update the factor by adding the information from the JacobianFactor (used internally during elimination).
update  The JacobianFactor containing the new information to add 
scatter  A mapping from variable index to slot index in this HessianFactor 
void gtsam::HessianFactor::updateATA  (  const HessianFactor &  update, 
const Scatter &  scatter  
) 
Update the factor by adding the information from the HessianFactor (used internally during elimination).
update  The HessianFactor containing the new information to add 
scatter  A mapping from variable index to slot index in this HessianFactor 