Public Types | Public Member Functions | Private Types | Private Member Functions | Private Attributes | List of all members
gtsam::RegularHessianFactor< D > Class Template Reference

#include <RegularHessianFactor.h>

Inheritance diagram for gtsam::RegularHessianFactor< D >:
Inheritance graph
[legend]

Public Types

typedef Eigen::Matrix< double, D, DMatrixD
 
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< Thisshared_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< Thisshared_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 gradient at zero to d TODO: is it really the goal to add ?? More...
 
void hessianDiagonal (double *d) const override
 
void multiplyHessianAdd (double alpha, const VectorValues &x, VectorValues &y) const override
 
void multiplyHessianAdd (double alpha, const double *x, double *yvalues) const
 
void multiplyHessianAdd (double alpha, const double *x, double *yvalues, std::vector< size_t > offsets) const
 Raw memory version, with offsets TODO document reasoning. More...
 
 RegularHessianFactor (const KeyVector &js, const std::vector< Matrix > &Gs, const std::vector< Vector > &gs, double f)
 
 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)
 
template<typename KEYS >
 RegularHessianFactor (const KEYS &keys, const SymmetricBlockMatrix &augmentedInformation)
 
 RegularHessianFactor (const RegularJacobianFactor< D > &jf)
 Construct from RegularJacobianFactor. More...
 
 RegularHessianFactor (const GaussianFactorGraph &factors, const Scatter &scatter)
 Construct from a GaussianFactorGraph. More...
 
 RegularHessianFactor (const GaussianFactorGraph &factors)
 Construct from a GaussianFactorGraph. More...
 
- Public Member Functions inherited from gtsam::HessianFactor
Matrix augmentedInformation () const override
 
Matrix augmentedJacobian () const override
 
GaussianFactor::shared_ptr clone () const override
 
double constantTerm () const
 
double & constantTerm ()
 
std::shared_ptr< GaussianConditionaleliminateCholesky (const Ordering &keys)
 
bool equals (const GaussianFactor &lf, double tol=1e-9) const override
 
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, MatrixhessianBlockDiagonal () 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 (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)
 
 HessianFactor (const KeyVector &js, const std::vector< Matrix > &Gs, const std::vector< Vector > &gs, double f)
 
template<typename KEYS >
 HessianFactor (const KEYS &keys, const SymmetricBlockMatrix &augmentedInformation)
 
 HessianFactor (const JacobianFactor &cg)
 
 HessianFactor (const GaussianFactor &factor)
 
 HessianFactor (const GaussianFactorGraph &factors, const Scatter &scatter)
 
 HessianFactor (const GaussianFactorGraph &factors)
 
const SymmetricBlockMatrixinfo () const
 Return underlying information matrix. More...
 
SymmetricBlockMatrixinfo ()
 
Matrix information () const override
 
Eigen::SelfAdjointView< SymmetricBlockMatrix::constBlock, Eigen::UpperinformationView () const
 Return self-adjoint view onto the information matrix (NOT augmented). More...
 
std::pair< Matrix, Vectorjacobian () const override
 Return (dense) matrix associated with factor. More...
 
SymmetricBlockMatrix::constBlock linearTerm (const_iterator j) const
 
SymmetricBlockMatrix::constBlock linearTerm () const
 
SymmetricBlockMatrix::Block linearTerm ()
 
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 KeyVectorkeys () 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...
 
KeyVectorkeys ()
 
iterator begin ()
 
iterator end ()
 

Private Types

typedef Eigen::Map< const VectorDConstDMap
 
typedef Eigen::Map< VectorDDMap
 

Private Member Functions

void checkInvariants ()
 Check invariants after construction. More...
 

Private Attributes

std::vector< VectorDy_
 

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 >

Definition at line 28 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 102 of file RegularHessianFactor.h.

◆ DMap

template<size_t D>
typedef Eigen::Map<VectorD> gtsam::RegularHessianFactor< D >::DMap
private

Definition at line 101 of file RegularHessianFactor.h.

◆ MatrixD

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

Definition at line 33 of file RegularHessianFactor.h.

◆ VectorD

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

Definition at line 32 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. 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 39 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 49 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 58 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 with the augmented information matrix specified as a block matrix.

Definition at line 68 of file RegularHessianFactor.h.

◆ RegularHessianFactor() [5/7]

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

Construct from RegularJacobianFactor.

Definition at line 75 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.

Definition at line 79 of file RegularHessianFactor.h.

◆ RegularHessianFactor() [7/7]

template<size_t D>
gtsam::RegularHessianFactor< D >::RegularHessianFactor ( const GaussianFactorGraph factors)
inline

Construct from a GaussianFactorGraph.

Definition at line 86 of file RegularHessianFactor.h.

Member Function Documentation

◆ checkInvariants()

template<size_t D>
void gtsam::RegularHessianFactor< D >::checkInvariants ( )
inlineprivate

Check invariants after construction.

Definition at line 94 of file RegularHessianFactor.h.

◆ gradientAtZero()

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

Add gradient at zero to d TODO: is it really the goal to add ??

Reimplemented from gtsam::HessianFactor.

Definition at line 196 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)

Reimplemented from gtsam::HessianFactor.

Definition at line 185 of file RegularHessianFactor.h.

◆ multiplyHessianAdd() [1/3]

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

y += alpha * A'*A*x

Reimplemented from gtsam::HessianFactor.

Definition at line 112 of file RegularHessianFactor.h.

◆ multiplyHessianAdd() [2/3]

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

y += alpha * A'*A*x

Definition at line 118 of file RegularHessianFactor.h.

◆ multiplyHessianAdd() [3/3]

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

Raw memory version, with offsets TODO document reasoning.

Definition at line 150 of file RegularHessianFactor.h.

Member Data Documentation

◆ y_

template<size_t D>
std::vector<VectorD> gtsam::RegularHessianFactor< D >::y_
mutableprivate

Definition at line 107 of file RegularHessianFactor.h.


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


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:47:08