Detailed Description

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

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

bool	equals (const GaussianFactor &lf, double tol) const override
	equals More...

DenseIndex	getDim (const_iterator variable) const override
	Degrees of freedom of camera. More...

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

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

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

Matrix	augmentedInformation () const override
	Compute full augmented information matrix More...

Matrix	information () const override
	Compute full information matrix More...

void	hessianDiagonalAdd (VectorValues &d) const override
	Add the diagonal of the Hessian for this factor to existing VectorValues. More...

void	hessianDiagonal (double *d) const override
	add the contribution of this factor to the diagonal of the hessian d(output) = d(input) + deltaHessianFactor More...

std::map< Key, Matrix >	hessianBlockDiagonal () const override
	Return the block diagonal of the Hessian for this factor. More...

GaussianFactor::shared_ptr	clone () const override
	Clone a factor (make a deep copy) More...

bool	empty () const override
	Test whether the factor is empty. More...

GaussianFactor::shared_ptr	negate () const override
	Construct the corresponding anti-factor to negate information stored stored in this factor. More...

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
	Print for testable. More...

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

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

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

void	gradientAtZero (double *d) const override
	Calculate gradient, which is -F'Q*b, see paper - RAW MEMORY ACCESS. More...

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

VectorValues	hessianDiagonal () const
	Using the base method.

virtual void	hessianDiagonal (double *d) const=0
	Using the base method. More...

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

virtual	~GaussianFactor ()
	Destructor.

void	print (const std::string &s="", const KeyFormatter &formatter=DefaultKeyFormatter) const override=0
	print More...

virtual bool	equals (const GaussianFactor &lf, double tol=1e-9) const =0
	Equals for testable. More...

virtual double	error (const VectorValues &c) const =0
	Print for testable. More...

virtual DenseIndex	getDim (const_iterator variable) const =0
	0.5(Ax-b)'D(A*x-b) More...

virtual Matrix	augmentedJacobian () const =0
	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. More...

virtual std::pair< Matrix, Vector >	jacobian () const =0
	Return the dense Jacobian \( A \) and right-hand-side \( b \), with the noise models baked into A and b. More...

virtual Matrix	augmentedInformation () const =0
	Return the augmented information matrix represented by this GaussianFactor. More...

virtual Matrix	information () const =0
	Return the non-augmented information matrix represented by this GaussianFactor. More...

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

virtual void	hessianDiagonalAdd (VectorValues &d) const =0
	Add the current diagonal to a VectorValues instance. More...

virtual void	hessianDiagonal (double *d) const =0
	Raw memory access version of hessianDiagonal. More...

virtual std::map< Key, Matrix >	hessianBlockDiagonal () const =0
	Return the block diagonal of the Hessian for this factor. More...

virtual GaussianFactor::shared_ptr	clone () const =0
	Clone a factor (make a deep copy) More...

virtual bool	empty () const =0
	Test whether the factor is empty. More...

virtual GaussianFactor::shared_ptr	negate () const =0
	Construct the corresponding anti-factor to negate information stored stored in this factor. More...

virtual void	updateHessian (const KeyVector &keys, SymmetricBlockMatrix *info) const =0
	Update an information matrix by adding the information corresponding to this factor (used internally during elimination). More...

virtual void	multiplyHessianAdd (double alpha, const VectorValues &x, VectorValues &y) const =0
	y += alpha * A'Ax More...

virtual VectorValues	gradientAtZero () const =0
	A'*b for Jacobian, eta for Hessian. More...

virtual void	gradientAtZero (double *d) const =0
	Raw memory access version of gradientAtZero. More...

virtual Vector	gradient (Key key, const VectorValues &x) const =0
	Gradient wrt a key at any values. More...

Public Member Functions inherited from gtsam::Factor
virtual	~Factor ()=default
	Default destructor.

KeyVector &	keys ()

iterator	begin ()
	Iterator at beginning of involved variable keys.

iterator	end ()
	Iterator at end of involved variable keys.

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

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

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

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
Protected Member Functions inherited from gtsam::Factor
	Factor ()
	Default constructor for I/O.

template<typename CONTAINER >
	Factor (const CONTAINER &keys)
	Construct factor from container of keys. More...

template<typename ITERATOR >
	Factor (ITERATOR first, ITERATOR last)
	Construct factor from iterator keys. More...

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

Static Protected Member Functions inherited from gtsam::Factor
template<typename CONTAINER >
static Factor	FromKeys (const CONTAINER &keys)
	Construct factor from container of keys. More...

template<typename ITERATOR >
static Factor	FromIterators (ITERATOR first, ITERATOR last)
	Construct factor from iterator keys. More...

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

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

Detailed Description

Public Member Functions

Static Public Member Functions

Public Attributes

Public Types

Protected Types

Protected Attributes

Static Protected Attributes

Additional Inherited Members

Constructor & Destructor Documentation

◆ RegularImplicitSchurFactor()

Member Function Documentation

◆ augmentedInformation()

◆ augmentedJacobian()

◆ clone()

◆ empty()

◆ equals()

◆ error()

◆ getDim()

◆ gradient()

◆ gradientAtZero() [1/2]

◆ gradientAtZero() [2/2]

◆ hessianBlockDiagonal()

◆ hessianDiagonal() [1/2]

◆ hessianDiagonal() [2/2]

◆ hessianDiagonalAdd()

◆ information()

◆ jacobian()

◆ multiplyHessianAdd() [1/2]

◆ multiplyHessianAdd() [2/2]

◆ negate()

◆ print()

◆ updateHessian()