A Gaussian factor using the canonical parameters (information form) More...
#include <HessianFactor.h>
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) |
A Gaussian factor using the canonical parameters (information form)
HessianFactor implements a general quadratic factor of the form
that stores the matrix , the vector , and the constant term .
When is positive semidefinite, this factor represents a Gaussian, in which case is the information matrix , is the information vector , and is the residual sum-square-error at the mean, when .
Indeed, the negative log-likelihood of a Gaussian is (up to a constant) with the mean and the covariance matrix. Expanding the product we get
We define the Information matrix (or Hessian) and the information vector to arrive at the canonical form of the Gaussian:
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 and stores the Jacobian and error vector , 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.
Blocks can be accessed as follows:
Definition at line 100 of file HessianFactor.h.
Typedef to base class.
Definition at line 107 of file HessianFactor.h.
A block from the Hessian matrix.
Definition at line 110 of file HessianFactor.h.
A block from the Hessian matrix (const version)
Definition at line 111 of file HessianFactor.h.
typedef std::shared_ptr<This> gtsam::HessianFactor::shared_ptr |
A shared_ptr to this class.
Definition at line 109 of file HessianFactor.h.
Typedef to this class.
Definition at line 108 of file HessianFactor.h.
gtsam::HessianFactor::HessianFactor | ( | ) |
default constructor for I/O
Definition at line 67 of file HessianFactor.cpp.
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.
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.
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
HessianFactor error is
So, with and we have
Definition at line 98 of file HessianFactor.cpp.
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.
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.
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.
|
explicit |
Construct from a JacobianFactor (or from a GaussianConditional since it derives from it)
Definition at line 217 of file HessianFactor.cpp.
|
explicit |
Attempt to construct from any GaussianFactor - currently supports JacobianFactor, HessianFactor, GaussianConditional, or any derived classes.
Definition at line 224 of file HessianFactor.cpp.
|
explicit |
Combine a set of factors into a single dense HessianFactor
Definition at line 239 of file HessianFactor.cpp.
|
inlineexplicit |
Combine a set of factors into a single dense HessianFactor
Definition at line 181 of file HessianFactor.h.
|
inlineoverride |
Destructor
Definition at line 185 of file HessianFactor.h.
|
private |
Constructor with given scatter pattern, allocating but not initializing storage.
Definition at line 62 of file HessianFactor.cpp.
|
private |
Allocate for given scatter pattern.
Definition at line 44 of file HessianFactor.cpp.
|
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 ). 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.
Definition at line 276 of file HessianFactor.cpp.
|
overridevirtual |
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.
Definition at line 321 of file HessianFactor.cpp.
|
inlineoverridevirtual |
Clone this HessianFactor
Implements gtsam::GaussianFactor.
Definition at line 188 of file HessianFactor.h.
|
inline |
Return the constant term as described above
Definition at line 237 of file HessianFactor.h.
|
inline |
Return the constant term as described above
Definition at line 229 of file HessianFactor.h.
std::shared_ptr< GaussianConditional > gtsam::HessianFactor::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. Does dense partial Cholesky.
Definition at line 459 of file HessianFactor.cpp.
|
overridevirtual |
Compare to another factor for testing (implementing Testable)
Implements gtsam::GaussianFactor.
Definition at line 267 of file HessianFactor.cpp.
|
override |
Definition at line 31 of file GaussianFactor.cpp.
double GaussianFactor::error |
Definition at line 27 of file GaussianFactor.cpp.
|
overridevirtual |
Evaluate the factor error f(x). returns 0.5*[x -1]'H[x -1] (also see constructor documentation)
Reimplemented from gtsam::GaussianFactor.
Definition at line 331 of file HessianFactor.cpp.
|
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?
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.
Definition at line 212 of file HessianFactor.h.
|
overridevirtual |
Compute the gradient at a key:
Implements gtsam::GaussianFactor.
Definition at line 438 of file HessianFactor.cpp.
|
overridevirtual |
|
overridevirtual |
Raw memory access version of gradientAtZero.
Implements gtsam::GaussianFactor.
Reimplemented in gtsam::RegularHessianFactor< D >.
Definition at line 432 of file HessianFactor.cpp.
Return the block diagonal of the Hessian for this factor.
Implements gtsam::GaussianFactor.
Definition at line 310 of file HessianFactor.cpp.
|
overridevirtual |
Raw memory access version of hessianDiagonal.
Implements gtsam::GaussianFactor.
Reimplemented in gtsam::RegularHessianFactor< D >.
Definition at line 304 of file HessianFactor.cpp.
|
overridevirtual |
Add the current diagonal to a VectorValues instance.
Implements gtsam::GaussianFactor.
Definition at line 292 of file HessianFactor.cpp.
|
inline |
Return non-const information matrix. TODO(gareth): Review the sanity of having non-const access to this.
Definition at line 268 of file HessianFactor.h.
|
inline |
Return underlying information matrix.
Definition at line 264 of file HessianFactor.h.
|
overridevirtual |
Return the non-augmented information matrix represented by this GaussianFactor.
Implements gtsam::GaussianFactor.
Definition at line 287 of file HessianFactor.cpp.
Eigen::SelfAdjointView< SymmetricBlockMatrix::constBlock, Eigen::Upper > gtsam::HessianFactor::informationView | ( | ) | const |
Return self-adjoint view onto the information matrix (NOT augmented).
Definition at line 282 of file HessianFactor.cpp.
Return (dense) matrix associated with factor.
Implements gtsam::GaussianFactor.
Definition at line 326 of file HessianFactor.cpp.
|
inline |
Return the complete linear term as described above.
Definition at line 258 of file HessianFactor.h.
|
inline |
Return the complete linear term as described above.
Definition at line 250 of file HessianFactor.h.
|
inline |
Return the part of linear term 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. |
Definition at line 243 of file HessianFactor.h.
|
overridevirtual |
y += alpha * A'*A*x
Implements gtsam::GaussianFactor.
Reimplemented in gtsam::RegularHessianFactor< D >.
Definition at line 385 of file HessianFactor.cpp.
|
overridevirtual |
Construct the corresponding anti-factor to negate information stored stored in this factor.
Implements gtsam::GaussianFactor.
Definition at line 377 of file HessianFactor.cpp.
|
overridevirtual |
Print the factor for debugging and testing (implementing Testable)
Implements gtsam::GaussianFactor.
Definition at line 255 of file HessianFactor.cpp.
|
inline |
Return the number of columns and rows of the Hessian matrix, including the information vector.
Definition at line 217 of file HessianFactor.h.
VectorValues gtsam::HessianFactor::solve | ( | ) |
Solve the system A'*A delta = A'*b in-place, return delta as VectorValues.
Definition at line 490 of file HessianFactor.cpp.
|
overridevirtual |
Update an information matrix by adding the information corresponding to this factor (used internally during elimination).
keys | THe ordered vector of keys for the information matrix to be updated |
info | The information matrix to be updated |
Implements gtsam::GaussianFactor.
Definition at line 348 of file HessianFactor.cpp.
|
inline |
Update another Hessian factor
other | the HessianFactor to be updated |
Definition at line 327 of file HessianFactor.h.
|
friend |
Definition at line 364 of file HessianFactor.h.
|
friend |
Definition at line 363 of file HessianFactor.h.
|
protected |
The full augmented information matrix, s.t. the quadratic error is 0.5*[x -1]'H[x -1].
Definition at line 103 of file HessianFactor.h.