Public Member Functions | Private Types | List of all members
gtsam::JacobianFactorSVD< D, ZDim > Class Template Reference

#include <JacobianFactorSVD.h>

Inheritance diagram for gtsam::JacobianFactorSVD< D, ZDim >:
Inheritance graph
[legend]

Public Member Functions

 JacobianFactorSVD ()
 Default constructor. More...
 
 JacobianFactorSVD (const KeyVector &keys, const SharedDiagonal &model=SharedDiagonal())
 Empty constructor with keys. More...
 
 JacobianFactorSVD (const KeyVector &keys, const std::vector< MatrixZD, Eigen::aligned_allocator< MatrixZD > > &Fblocks, const Matrix &Enull, const Vector &b, const SharedDiagonal &model=SharedDiagonal())
 Construct a new JacobianFactorSVD object, createing a reduced-rank Jacobian factor on the CameraSet. More...
 
- Public Member Functions inherited from gtsam::RegularJacobianFactor< D >
VectorValues gradientAtZero () const override
 Expose base class gradientAtZero. More...
 
void gradientAtZero (double *d) const override
 Raw memory access version of gradientAtZero. More...
 
VectorValues hessianDiagonal () const
 Using the base method. More...
 
void hessianDiagonal (double *d) const override
 Raw memory access version of hessianDiagonal. More...
 
virtual void hessianDiagonal (double *d) const=0
 Using the base method. More...
 
void multiplyHessianAdd (double alpha, const double *x, double *y) const
 double* Hessian-vector multiply, i.e. y += A'*(A*x) RAW memory access! Assumes keys start at 0 and go to M-1, and x and and y are laid out that way More...
 
void multiplyHessianAdd (double alpha, const double *x, double *y, const std::vector< size_t > &accumulatedDims) const
 
void multiplyHessianAdd (double alpha, const VectorValues &x, VectorValues &y) const override
 
void multiplyHessianAdd (double alpha, const VectorValues &x, VectorValues &y) const override
 
Vector operator* (const double *x) const
 double* Matrix-vector multiply, i.e. y = A*x RAW memory access! Assumes keys start at 0 and go to M-1, and x is laid out that way More...
 
 RegularJacobianFactor ()
 Default constructor. More...
 
template<typename KEYS >
 RegularJacobianFactor (const KEYS &keys, const VerticalBlockMatrix &augmentedMatrix, const SharedDiagonal &sigmas=SharedDiagonal())
 
template<typename TERMS >
 RegularJacobianFactor (const TERMS &terms, const Vector &b, const SharedDiagonal &model=SharedDiagonal())
 
void transposeMultiplyAdd (double alpha, const Vector &e, double *x) const
 double* Transpose Matrix-vector multiply, i.e. x += A'*e RAW memory access! Assumes keys start at 0 and go to M-1, and y is laid out that way More...
 
- Public Member Functions inherited from gtsam::JacobianFactor
Matrix augmentedInformation () const override
 
Matrix augmentedJacobian () const override
 
Matrix augmentedJacobianUnweighted () const
 
GaussianFactor::shared_ptr clone () const override
 
size_t cols () const
 
std::pair< std::shared_ptr< GaussianConditional >, shared_ptreliminate (const Ordering &keys)
 
bool equals (const GaussianFactor &lf, double tol=1e-9) const override
 assert equality up to a tolerance More...
 
double error (const HybridValues &c) const override
 
virtual double error (const VectorValues &c) const
 
double error (const VectorValues &c) const override
 
Vector error_vector (const VectorValues &c) const
 
SharedDiagonalget_model ()
 
const SharedDiagonalget_model () const
 
ABlock getA ()
 
constABlock getA () const
 
ABlock getA (const Key &key)
 
constABlock getA (const_iterator variable) const
 
ABlock getA (iterator variable)
 
BVector getb ()
 
const constBVector getb () const
 
DenseIndex getDim (const_iterator variable) const override
 
Vector gradient (Key key, const VectorValues &x) const override
 Compute the gradient wrt a key at any values. 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...
 
Matrix information () const override
 
bool isConstrained () const
 
std::pair< Matrix, Vectorjacobian () const override
 Returns (dense) A,b pair associated with factor, bakes in the weights. More...
 
 JacobianFactor ()
 
 JacobianFactor (const GaussianFactor &gf)
 
 JacobianFactor (const GaussianFactorGraph &graph)
 
 JacobianFactor (const GaussianFactorGraph &graph, const Ordering &ordering)
 
 JacobianFactor (const GaussianFactorGraph &graph, const Ordering &ordering, const VariableSlots &p_variableSlots)
 
 JacobianFactor (const GaussianFactorGraph &graph, const VariableSlots &p_variableSlots)
 
 JacobianFactor (const HessianFactor &hf)
 
 JacobianFactor (const JacobianFactor &jf)
 
template<typename KEYS >
 JacobianFactor (const KEYS &keys, const VerticalBlockMatrix &augmentedMatrix, const SharedDiagonal &sigmas=SharedDiagonal())
 
template<typename TERMS >
 JacobianFactor (const TERMS &terms, const Vector &b, const SharedDiagonal &model=SharedDiagonal())
 
 JacobianFactor (const Vector &b_in)
 
 JacobianFactor (Key i1, const Matrix &A1, const Vector &b, const SharedDiagonal &model=SharedDiagonal())
 
 JacobianFactor (Key i1, const Matrix &A1, Key i2, const Matrix &A2, const Vector &b, const SharedDiagonal &model=SharedDiagonal())
 
 JacobianFactor (Key i1, const Matrix &A1, Key i2, const Matrix &A2, Key i3, const Matrix &A3, const Vector &b, const SharedDiagonal &model=SharedDiagonal())
 
std::pair< Matrix, VectorjacobianUnweighted () const
 Returns (dense) A,b pair associated with factor, does not bake in weights. More...
 
VerticalBlockMatrixmatrixObject ()
 
const VerticalBlockMatrixmatrixObject () const
 
void multiplyHessianAdd (double alpha, const double *x, double *y, const std::vector< size_t > &accumulatedDims) const
 
GaussianFactor::shared_ptr negate () const override
 
Vector operator* (const VectorValues &x) const
 
void print (const std::string &s="", const KeyFormatter &formatter=DefaultKeyFormatter) const override
 print with optional string More...
 
size_t rows () const
 
void setModel (bool anyConstrained, const Vector &sigmas)
 
std::shared_ptr< GaussianConditionalsplitConditional (size_t nrFrontals)
 
void transposeMultiplyAdd (double alpha, const Vector &e, VectorValues &x) const
 
Vector unweighted_error (const VectorValues &c) const
 
void updateHessian (const KeyVector &keys, SymmetricBlockMatrix *info) const override
 
JacobianFactor whiten () const
 
 ~JacobianFactor () 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 RegularJacobianFactor< DBase
 
typedef std::pair< Key, MatrixKeyMatrix
 
typedef Eigen::Matrix< double, ZDim, DMatrixZD
 

Additional Inherited Members

- Public Types inherited from gtsam::JacobianFactor
typedef VerticalBlockMatrix::Block ABlock
 
typedef GaussianFactor Base
 Typedef to base class. More...
 
typedef ABlock::ColXpr BVector
 
typedef VerticalBlockMatrix::constBlock constABlock
 
typedef constABlock::ConstColXpr constBVector
 
typedef std::shared_ptr< Thisshared_ptr
 shared_ptr to this class More...
 
typedef JacobianFactor 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...
 
- 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::JacobianFactor
template<typename TERMS >
void fillTerms (const TERMS &terms, const Vector &b, const SharedDiagonal &noiseModel)
 Internal function to fill blocks and set dimensions. More...
 
- 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::JacobianFactor
VerticalBlockMatrix Ab_
 
noiseModel::Diagonal::shared_ptr model_
 
- Protected Attributes inherited from gtsam::Factor
KeyVector keys_
 The keys involved in this factor. More...
 

Detailed Description

template<size_t D, size_t ZDim>
class gtsam::JacobianFactorSVD< D, ZDim >

JacobianFactor for Schur complement that uses the "Nullspace Trick" by Mourikis et al.

This trick is equivalent to the Schur complement, but can be faster. In essence, the linear factor |E*dp + F*dX - b|, where p is point and X are poses, is multiplied by Enull, a matrix that spans the left nullspace of E, i.e., The mx3 matrix is analyzed with SVD as E = [Erange Enull]*S*V (mxm * mx3 * 3x3) where Enull is an m x (m-3) matrix Then Enull'*E*dp = 0, and |Enull'*E*dp + Enull'*F*dX - Enull'*b| == |Enull'*F*dX - Enull'*b| Normally F is m x 6*numKeys, and Enull'*F yields an (m-3) x 6*numKeys matrix.

The code below assumes that F is block diagonal and is given as a vector of ZDim*D blocks. Example: m = 4 (2 measurements), Enull = 4*1, F = 4*12 (for D=6) Then Enull'*F = 1*4 * 4*12 = 1*12, but each 1*6 piece can be computed as a 1x2 * 2x6 multiplication.

Definition at line 29 of file JacobianFactorSVD.h.

Member Typedef Documentation

◆ Base

template<size_t D, size_t ZDim>
typedef RegularJacobianFactor<D> gtsam::JacobianFactorSVD< D, ZDim >::Base
private

Definition at line 31 of file JacobianFactorSVD.h.

◆ KeyMatrix

template<size_t D, size_t ZDim>
typedef std::pair<Key, Matrix> gtsam::JacobianFactorSVD< D, ZDim >::KeyMatrix
private

Definition at line 33 of file JacobianFactorSVD.h.

◆ MatrixZD

template<size_t D, size_t ZDim>
typedef Eigen::Matrix<double, ZDim, D> gtsam::JacobianFactorSVD< D, ZDim >::MatrixZD
private

Definition at line 32 of file JacobianFactorSVD.h.

Constructor & Destructor Documentation

◆ JacobianFactorSVD() [1/3]

template<size_t D, size_t ZDim>
gtsam::JacobianFactorSVD< D, ZDim >::JacobianFactorSVD ( )
inline

Default constructor.

Definition at line 38 of file JacobianFactorSVD.h.

◆ JacobianFactorSVD() [2/3]

template<size_t D, size_t ZDim>
gtsam::JacobianFactorSVD< D, ZDim >::JacobianFactorSVD ( const KeyVector keys,
const SharedDiagonal model = SharedDiagonal() 
)
inline

Empty constructor with keys.

Definition at line 42 of file JacobianFactorSVD.h.

◆ JacobianFactorSVD() [3/3]

template<size_t D, size_t ZDim>
gtsam::JacobianFactorSVD< D, ZDim >::JacobianFactorSVD ( const KeyVector keys,
const std::vector< MatrixZD, Eigen::aligned_allocator< MatrixZD > > &  Fblocks,
const Matrix Enull,
const Vector b,
const SharedDiagonal model = SharedDiagonal() 
)
inline

Construct a new JacobianFactorSVD object, createing a reduced-rank Jacobian factor on the CameraSet.

Parameters
keyskeys associated with F blocks.
FblocksCameraSet derivatives, ZDim*D blocks of block-diagonal F
Enulla reduced point derivative
bright-hand side
modelnoise model

Definition at line 64 of file JacobianFactorSVD.h.


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


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:51:38