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

inlineoverridevirtual

Compute full augmented information matrix

Implements gtsam::GaussianFactor.

augmentedJacobian()

template<class CAMERA>

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

inlineoverridevirtual

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

inlineoverridevirtual

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

inlineoverridevirtual

equals

Implements gtsam::GaussianFactor.

error()

template<class CAMERA>

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

inlineoverridevirtual

Reimplemented from gtsam::GaussianFactor.

getDim()

template<class CAMERA>

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

inlineoverridevirtual

Degrees of freedom of camera.

Implements gtsam::GaussianFactor.

gradient()

template<class CAMERA>

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

inlineoverridevirtual

Gradient wrt a key at any values.

Implements gtsam::GaussianFactor.

gradientAtZero() [1/2]

template<class CAMERA>

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

inlineoverridevirtual

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

inlineoverridevirtual

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

inlineoverridevirtual

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

inlineoverridevirtual

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

inlineoverridevirtual

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

inlineoverridevirtual

Compute full information matrix

Implements gtsam::GaussianFactor.

jacobian()

template<class CAMERA>

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

inlineoverridevirtual

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

inlineoverridevirtual

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

inlineoverridevirtual

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

inlineoverridevirtual

print

Implements gtsam::GaussianFactor.

updateHessian()

template<class CAMERA>

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

inlineoverridevirtual

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:

• /tmp/gtsam-4.2-docs.H5EUbA/src/gtsam/slam/RegularImplicitSchurFactor.h