Loading...
Searching...
No Matches
gtsam::RegularImplicitSchurFactor< CAMERA > Class Template Reference

## Detailed Description

template<class CAMERA>
class gtsam::RegularImplicitSchurFactor< CAMERA >

RegularImplicitSchurFactor.

A specialization of a GaussianFactor to structure-less SFM, which is very fast in a conjugate gradient (CG) solver. Specifically, as measured in timeSchurFactors.cpp, it stays very fast for an increasing number of cameras. The magic is in multiplyHessianAdd, which does the Hessian-vector multiply at the core of CG, and implements y += F'alpha(I - E*P*E')*F*x where

- F is the 2mx6m Jacobian of the m 2D measurements wrpt m 6DOF poses
- E is the 2mx3 Jacabian of the m 2D measurements wrpt a 3D point
- P is the covariance on the point The equation above implicitly executes the Schur complement by removing the information E*P*E' from the Hessian. It is also very fast as we do not use the full 6m*6m F matrix, but rather only it's m 6x6 diagonal blocks.

Inheritance diagram for gtsam::RegularImplicitSchurFactor< CAMERA >:

## Public Member Functions

| RegularImplicitSchurFactor ()
| Constructor.
| RegularImplicitSchurFactor (const KeyVector &keys, const FBlocks &Fs, const Matrix &E, const Matrix &P, const Vector &b)
| Construct from blocks of F, E, inv(E'*E), and RHS vector b.
| ~RegularImplicitSchurFactor () override
| Destructor.
const FBlocks & | Fs () const
const Matrix & | E () const
const Vector & | b () const
const Matrix & | getPointCovariance () const
void | print (const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
| print
bool | equals (const GaussianFactor &lf, double tol) const override
| equals
DenseIndex | getDim (const_iterator variable) const override
| Degrees of freedom of camera.
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).
Matrix | augmentedJacobian () const override
| Return a dense \( [ \;A\;b\; ] \in \mathbb{R}^{m \times n+1} \) Jacobian matrix, augmented with b with the noise models baked into A and b.
std::pair< Matrix, Vector > | jacobian () const override
| Return the dense Jacobian \( A \) and right-hand-side \( b \), with the noise models baked into A and b.
Matrix | augmentedInformation () const override
| Compute full augmented information matrix
Matrix | information () const override
| Compute full information matrix
void | hessianDiagonalAdd (VectorValues &d) const override
| Add the diagonal of the Hessian for this factor to existing VectorValues.
void | hessianDiagonal (double *d) const override
| add the contribution of this factor to the diagonal of the hessian d(output) = d(input) + deltaHessianFactor
std::map< Key, Matrix > | hessianBlockDiagonal () const override
| Return the block diagonal of the Hessian for this factor.
GaussianFactor::shared_ptr | clone () const override
| Clone a factor (make a deep copy).
GaussianFactor::shared_ptr | negate () const override
| Construct the corresponding anti-factor to negate information stored stored in this factor.
void | projectError2 (const Error2s &e1, Error2s &e2) const
| Calculate corrected error Q*(e-ZDim*b) = (I - E*P*E')*(e-ZDim*b).
double | error (const VectorValues &x) const override
double | errorJF (const VectorValues &x) const
void | projectError (const Error2s &e1, Error2s &e2) const
| Calculate corrected error Q*e = (I - E*P*E')*e.
void | multiplyHessianAdd (double alpha, const double *x, double *y) const
| double* Hessian-vector multiply, i.e.
void | multiplyHessianAdd (double alpha, const double *x, double *y, std::vector< size_t > keys) const
void | multiplyHessianAdd (double alpha, const VectorValues &x, VectorValues &y) const override
| Hessian-vector multiply, i.e.
void | multiplyHessianDummy (double alpha, const VectorValues &x, VectorValues &y) const
| Dummy version to measure overhead of key access.
VectorValues | gradientAtZero () const override
| Calculate gradient, which is -F'Q*b, see paper.
void | gradientAtZero (double *d) const override
| Calculate gradient, which is -F'Q*b, see paper - RAW MEMORY ACCESS.
Vector | gradient (Key key, const VectorValues &x) const override
| Gradient wrt a key at any values.
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.
virtual | ~GaussianFactor ()
| Destructor.
double | error (const HybridValues &c) const override
| All factor types need to implement an error function.
VectorValues | hessianDiagonal () const
| Return the diagonal of the Hessian for this factor.
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.

## Static Public Member Functions

static void | multiplyHessianAdd (const Matrix &F, const Matrix &E, const Matrix &PointCovariance, double alpha, const Vector &x, Vector &y)
Static Public Member Functions inherited from gtsam::GaussianFactor
template<typename CONTAINER>
static DenseIndex | Slot (const CONTAINER &keys, Key key)

## Public Attributes

Error2s | e1
| Scratch space for multiplyHessianAdd.
Error2s | e2

## Public Types

typedef RegularImplicitSchurFactor | This
| Typedef to this class.
typedef boost::shared_ptr< This > | shared_ptr
| shared_ptr to this class
typedef std::vector< Vector2, Eigen::aligned_allocator< Vector2 > > | Error2s
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 KeyVector::iterator | iterator
| Iterator over keys.
typedef KeyVector::const_iterator | const_iterator
| Const iterator over keys.

## Protected Types

typedef CameraSet< CAMERA > | Set
typedef CAMERA::Measurement | Z
typedef Eigen::Matrix< double, ZDim, D > | MatrixZD
| type of an F block
typedef Eigen::Matrix< double, D, D > | MatrixDD
| camera Hessian
typedef std::vector< MatrixZD, Eigen::aligned_allocator< MatrixZD > > | FBlocks

## Protected Attributes

FBlocks | FBlocks_
| All ZDim*D F blocks (one for each camera).
const Matrix | PointCovariance_
| the 3*3 matrix P = inv(E'E) (2*2 if degenerate)
const Matrix | E_
| The 2m*3 E Jacobian with respect to the point.
const Vector | b_
| 2m-dimensional RHS vector
Protected Attributes inherited from gtsam::Factor
KeyVector | keys_
| The keys involved in this factor.

## Static Protected Attributes

static const int | D = traits<CAMERA>::dimension
| Camera dimension.
static const int | ZDim = traits<Z>::dimension
| Measurement dimension.

## Additional Inherited Members

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

## Constructor & Destructor Documentation

## RegularImplicitSchurFactor()

template<class CAMERA>

gtsam::RegularImplicitSchurFactor< CAMERA >::RegularImplicitSchurFactor | ( | const KeyVector & | keys,
|  | const FBlocks & | Fs,
|  | const Matrix & | E,
|  | const Matrix & | P,
|  | const Vector & | b )

inline

Construct from blocks of F, E, inv(E'*E), and RHS vector b.

Construct a new RegularImplicitSchurFactor object.

Parameters

  keys | keys corresponding to cameras
  Fs | All ZDim*D F blocks (one for each camera)
  E | Jacobian of measurements wrpt point.
  P | point covariance matrix
  b | RHS vector

## Member Function Documentation

## augmentedInformation()

template<class CAMERA>

Matrix gtsam::RegularImplicitSchurFactor< CAMERA >::augmentedInformation | ( |  | ) | const

inline override virtual

Compute full augmented information matrix

Implements gtsam::GaussianFactor.

## augmentedJacobian()

template<class CAMERA>

Matrix gtsam::RegularImplicitSchurFactor< CAMERA >::augmentedJacobian | ( |  | ) | const

inline override virtual

Return a dense \( [ \;A\;b\; ] \in \mathbb{R}^{m \times n+1} \) Jacobian matrix, augmented with b with the noise models baked into A and b.

The negative log-likelihood is \( \frac{1}{2} \Vert Ax-b \Vert^2 \). See also GaussianFactorGraph::jacobian and GaussianFactorGraph::sparseJacobian.

Implements gtsam::GaussianFactor.

## clone()

template<class CAMERA>

GaussianFactor::shared_ptr gtsam::RegularImplicitSchurFactor< CAMERA >::clone | ( |  | ) | const

inline override virtual

Clone a factor (make a deep copy).

Implements gtsam::GaussianFactor.

## equals()

template<class CAMERA>

bool gtsam::RegularImplicitSchurFactor< CAMERA >::equals | ( | const GaussianFactor & | lf,
|  | double | tol ) const

inline override virtual

equals

Implements gtsam::GaussianFactor.

## error()

template<class CAMERA>

double gtsam::RegularImplicitSchurFactor< CAMERA >::error | ( | const VectorValues & | x | ) | const

inline override virtual

Reimplemented from gtsam::GaussianFactor.

## getDim()

template<class CAMERA>

DenseIndex gtsam::RegularImplicitSchurFactor< CAMERA >::getDim | ( | const_iterator | variable | ) | const

inline override virtual

Degrees of freedom of camera.

Implements gtsam::GaussianFactor.

## gradient()

template<class CAMERA>

Vector gtsam::RegularImplicitSchurFactor< CAMERA >::gradient | ( | Key | key,
|  | const VectorValues & | x ) const

inline override virtual

Gradient wrt a key at any values.

Implements gtsam::GaussianFactor.

## gradientAtZero() [1/2]

template<class CAMERA>

VectorValues gtsam::RegularImplicitSchurFactor< CAMERA >::gradientAtZero | ( |  | ) | const

inline override virtual

Calculate gradient, which is -F'Q*b, see paper.

Implements gtsam::GaussianFactor.

## gradientAtZero() [2/2]

template<class CAMERA>

void gtsam::RegularImplicitSchurFactor< CAMERA >::gradientAtZero | ( | double * | d | ) | const

inline override virtual

Calculate gradient, which is -F'Q*b, see paper - RAW MEMORY ACCESS.

Implements gtsam::GaussianFactor.

## hessianBlockDiagonal()

template<class CAMERA>

std::map< Key, Matrix > gtsam::RegularImplicitSchurFactor< CAMERA >::hessianBlockDiagonal | ( |  | ) | const

inline override virtual

Return the block diagonal of the Hessian for this factor.

Implements gtsam::GaussianFactor.

## hessianDiagonal()

template<class CAMERA>

void gtsam::RegularImplicitSchurFactor< CAMERA >::hessianDiagonal | ( | double * | d | ) | const

inline override virtual

add the contribution of this factor to the diagonal of the hessian d(output) = d(input) + deltaHessianFactor

Implements gtsam::GaussianFactor.

## hessianDiagonalAdd()

template<class CAMERA>

void gtsam::RegularImplicitSchurFactor< CAMERA >::hessianDiagonalAdd | ( | VectorValues & | d | ) | const

inline override virtual

Add the diagonal of the Hessian for this factor to existing VectorValues.

Implements gtsam::GaussianFactor.

## information()

template<class CAMERA>

Matrix gtsam::RegularImplicitSchurFactor< CAMERA >::information | ( |  | ) | const

inline override virtual

Compute full information matrix

Implements gtsam::GaussianFactor.

## jacobian()

template<class CAMERA>

std::pair< Matrix, Vector > gtsam::RegularImplicitSchurFactor< CAMERA >::jacobian | ( |  | ) | const

inline override virtual

Return the dense Jacobian \( A \) and right-hand-side \( b \), with the noise models baked into A and b.

The negative log-likelihood is \( \frac{1}{2} \Vert Ax-b \Vert^2 \). See also GaussianFactorGraph::augmentedJacobian and GaussianFactorGraph::sparseJacobian.

Implements gtsam::GaussianFactor.

## multiplyHessianAdd() [1/2]

template<class CAMERA>

void gtsam::RegularImplicitSchurFactor< CAMERA >::multiplyHessianAdd | ( | double | alpha,
|  | const double * | x,
|  | double * | y ) const

inline

double* Hessian-vector multiply, i.e.

y += F'alpha(I - E*P*E')*F*x RAW memory access! Assumes keys start at 0 and go to M-1, and x and and y are laid out that way

## multiplyHessianAdd() [2/2]

template<class CAMERA>

void gtsam::RegularImplicitSchurFactor< CAMERA >::multiplyHessianAdd | ( | double | alpha,
|  | const VectorValues & | x,
|  | VectorValues & | y ) const

inline override virtual

Hessian-vector multiply, i.e.

y += F'alpha(I - E*P*E')*F*x

Implements gtsam::GaussianFactor.

## negate()

template<class CAMERA>

GaussianFactor::shared_ptr gtsam::RegularImplicitSchurFactor< CAMERA >::negate | ( |  | ) | const

inline override virtual

Construct the corresponding anti-factor to negate information stored stored in this factor.

Returns
  a HessianFactor with negated Hessian matrices

Implements gtsam::GaussianFactor.

## print()

template<class CAMERA>

void gtsam::RegularImplicitSchurFactor< CAMERA >::print | ( | const std::string & | s = "",
|  | const KeyFormatter & | keyFormatter = DefaultKeyFormatter ) const

inline override virtual

print

Implements gtsam::GaussianFactor.

## updateHessian()

template<class CAMERA>

void gtsam::RegularImplicitSchurFactor< CAMERA >::updateHessian | ( | const KeyVector & | keys,
|  | SymmetricBlockMatrix * | info ) const

inline override virtual

Update an information matrix by adding the information corresponding to this factor (used internally during elimination).

Parameters

  scatter | A mapping from variable index to slot index in this HessianFactor
  info | The information matrix to be updated

Implements gtsam::GaussianFactor.

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

- /build/gtsam-Xg031d/gtsam-4.2.0+dfsg/gtsam/slam/RegularImplicitSchurFactor.h