#include <RegularImplicitSchurFactor.h>

Inheritance diagram for gtsam::RegularImplicitSchurFactor< CAMERA >:

Collaboration diagram for gtsam::RegularImplicitSchurFactor< CAMERA >:

Public Types
typedef RegularImplicitSchurFactor	This
	Typedef to this class.

typedef std::shared_ptr< This >	shared_ptr
	shared_ptr to this class

typedef std::vector< Vector2, Eigen::aligned_allocator< Vector2 > >	Error2s

typedef Factor	Base
	Our base class.

typedef KeyVector::iterator	iterator
	Iterator over keys.

typedef KeyVector::const_iterator	const_iterator
	Const iterator over keys.

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

	~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

Matrix	augmentedJacobian () const override

std::pair< Matrix, Vector >	jacobian () const override

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

GaussianFactor::shared_ptr	negate () const override

void	projectError2 (const Error2s &e1, Error2s &e2) const
	Calculate corrected error Q(e-ZDimb) = (I - EPE')(e-ZDimb)

double	error (const VectorValues &x) const override

double	errorJF (const VectorValues &x) const

void	projectError (const Error2s &e1, Error2s &e2) const
	Calculate corrected error Qe = (I - EPE')e.

void	multiplyHessianAdd (double alpha, const double x, double y) const
	double* Hessian-vector multiply, i.e. y += F'alpha(I - EPE')Fx RAW memory access! Assumes keys start at 0 and go to M-1, and x and and y are laid out that way

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. y += F'alpha(I - EPE')Fx.

void	multiplyHessianDummy (double alpha, const VectorValues &x, VectorValues &y) const
	Dummy version to measure overhead of key access.

VectorValues	gradientAtZero () const override

void	gradientAtZero (double *d) const override

Vector	gradient (Key key, const VectorValues &x) const override
	Gradient wrt a key at any values.

Testable
bool	equals (const This &other, double tol=1e-9) const
	check equality

virtual void	printKeys (const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
	print only keys

Standard Interface
double	error (const HybridValues &c) const override

VectorValues	hessianDiagonal () const
	Return the diagonal of the Hessian for this factor.

Standard Interface
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

const_iterator	end () const

size_t	size () const

Advanced Interface
KeyVector &	keys ()

iterator	begin ()

iterator	end ()

Static Public Member Functions
static void	multiplyHessianAdd (const Matrix &F, const Matrix &E, const Matrix &PointCovariance, double alpha, const Vector &x, Vector &y)

Advanced Interface
template<typename CONTAINER >
static DenseIndex	Slot (const CONTAINER &keys, Key key)

Public Attributes
Error2s	e1
	Scratch space for multiplyHessianAdd.

Error2s	e2

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

Static Protected Member Functions
Standard Constructors
template<typename CONTAINER >
static Factor	FromKeys (const CONTAINER &keys)

template<typename ITERATOR >
static Factor	FromIterators (ITERATOR first, ITERATOR last)

Protected Attributes
FBlocks	FBlocks_
	All ZDim*D F blocks (one for each camera)

const Matrix	PointCovariance_
	the 33 matrix P = inv(E'E) (22 if degenerate)

const Matrix	E_
	The 2m*3 E Jacobian with respect to the point.

const Vector	b_
	2m-dimensional RHS vector

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.

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.

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

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

◆ begin() [1/2]

const_iterator gtsam::Factor::begin ( ) const

inlineinherited

Iterator at beginning of involved variable keys

◆ begin() [2/2]

iterator gtsam::Factor::begin ( )

inlineinherited

Iterator at beginning of involved variable keys

◆ clone()

template<class CAMERA >

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

inlineoverridevirtual

Clone a factor (make a deep copy)

Implements gtsam::GaussianFactor.

◆ end() [1/2]

const_iterator gtsam::Factor::end ( ) const

inlineinherited

Iterator at end of involved variable keys

◆ end() [2/2]

iterator gtsam::Factor::end ( )

inlineinherited

Iterator at end of involved variable keys

◆ error()

double gtsam::GaussianFactor::error ( const HybridValues & c ) const

overridevirtualinherited

All factor types need to implement an error function. In factor graphs, this is the negative log-likelihood.

Reimplemented from gtsam::Factor.

◆ FromIterators()

template<typename ITERATOR >

static Factor gtsam::Factor::FromIterators	(	ITERATOR	first,
		ITERATOR	last
	)

inlinestaticprotectedinherited

Construct factor from iterator keys. This is called internally from derived factor static factor methods, as a workaround for not being able to call the protected constructors above.

◆ FromKeys()

template<typename CONTAINER >

static Factor gtsam::Factor::FromKeys ( const CONTAINER & keys )

inlinestaticprotectedinherited

Construct factor from container of keys. This is called internally from derived factor static factor methods, as a workaround for not being able to call the protected constructors above.

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

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

◆ keys()

KeyVector& gtsam::Factor::keys ( )

inlineinherited

Returns: keys involved in this factor

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

◆ size()

size_t gtsam::Factor::size ( ) const

inlineinherited

Returns: the number of variables involved in this factor

◆ 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:

/home/docs/checkouts/readthedocs.org/user_builds/gtsam-jlblanco-docs/checkouts/latest/gtsam/slam/RegularImplicitSchurFactor.h

Public Types

Public Member Functions

Static Public Member Functions

Public Attributes

Protected Types

Static Protected Member Functions

Protected Attributes

Static Protected Attributes

Detailed Description

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

Constructor & Destructor Documentation

◆ RegularImplicitSchurFactor()

Member Function Documentation

◆ augmentedJacobian()

◆ begin() [1/2]

◆ begin() [2/2]

◆ clone()

◆ end() [1/2]

◆ end() [2/2]

◆ error()

◆ FromIterators()

◆ FromKeys()

◆ gradientAtZero() [1/2]

◆ gradientAtZero() [2/2]

◆ jacobian()

◆ keys()

◆ negate()

◆ size()

◆ updateHessian()

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