#include <RegularImplicitSchurFactor.h>

Inheritance diagram for gtsam::RegularImplicitSchurFactor< CAMERA >:

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

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

typedef RegularImplicitSchurFactor	This
	Typedef to this class. More...

Public Types inherited from gtsam::GaussianFactor
typedef Factor	Base
	Our base class. More...

typedef std::shared_ptr< This >	shared_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...

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

Matrix	augmentedJacobian () const override

const Vector &	b () const

GaussianFactor::shared_ptr	clone () const override

const Matrix &	E () const

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

double	error (const VectorValues &x) const override

double	errorJF (const VectorValues &x) const

const FBlocks &	Fs () const

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

const Matrix &	getPointCovariance () const

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

VectorValues	gradientAtZero () const override

void	gradientAtZero (double *d) const override

std::map< Key, Matrix >	hessianBlockDiagonal () const override
	Return the block diagonal of the Hessian for this factor. 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...

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

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

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

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

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

GaussianFactor::shared_ptr	negate () const override

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

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

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

	RegularImplicitSchurFactor ()
	Constructor. More...

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

void	updateHessian (const KeyVector &keys, SymmetricBlockMatrix *info) const override

	~RegularImplicitSchurFactor () override
	Destructor. More...

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 KeyVector &	keys () 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...

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)

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

Error2s	e2

Protected Types
typedef std::vector< MatrixZD, Eigen::aligned_allocator< MatrixZD > >	FBlocks

typedef Eigen::Matrix< double, D, D >	MatrixDD
	camera Hessian More...

typedef Eigen::Matrix< double, ZDim, D >	MatrixZD
	type of an F block More...

typedef CameraSet< CAMERA >	Set

typedef CAMERA::Measurement	Z

Protected Attributes
const Vector	b_
	2m-dimensional RHS vector More...

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

FBlocks	FBlocks_
	All ZDim*D F blocks (one for each camera) More...

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

Protected Attributes inherited from gtsam::Factor
KeyVector	keys_
	The keys involved in this factor. More...

Static Protected Attributes
static const int	D = traits<CAMERA>::dimension
	Camera dimension. More...

static const int	ZDim = traits<Z>::dimension
	Measurement dimension. More...

Additional Inherited Members
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)

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.

Definition at line 39 of file RegularImplicitSchurFactor.h.

Member Typedef Documentation

◆ Error2s

template<class CAMERA>

typedef std::vector<Vector2, Eigen::aligned_allocator<Vector2> > gtsam::RegularImplicitSchurFactor< CAMERA >::Error2s

Definition at line 282 of file RegularImplicitSchurFactor.h.

◆ FBlocks

template<class CAMERA>

typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> > gtsam::RegularImplicitSchurFactor< CAMERA >::FBlocks

protected

Definition at line 56 of file RegularImplicitSchurFactor.h.

◆ MatrixDD

template<class CAMERA>

typedef Eigen::Matrix<double, D, D> gtsam::RegularImplicitSchurFactor< CAMERA >::MatrixDD

protected

camera Hessian

Definition at line 55 of file RegularImplicitSchurFactor.h.

◆ MatrixZD

template<class CAMERA>

typedef Eigen::Matrix<double, ZDim, D> gtsam::RegularImplicitSchurFactor< CAMERA >::MatrixZD

protected

type of an F block

Definition at line 54 of file RegularImplicitSchurFactor.h.

◆ Set

template<class CAMERA>

typedef CameraSet<CAMERA> gtsam::RegularImplicitSchurFactor< CAMERA >::Set

protected

Definition at line 48 of file RegularImplicitSchurFactor.h.

◆ shared_ptr

template<class CAMERA>

typedef std::shared_ptr<This> gtsam::RegularImplicitSchurFactor< CAMERA >::shared_ptr

shared_ptr to this class

Definition at line 43 of file RegularImplicitSchurFactor.h.

◆ This

template<class CAMERA>

typedef RegularImplicitSchurFactor gtsam::RegularImplicitSchurFactor< CAMERA >::This

Typedef to this class.

Definition at line 42 of file RegularImplicitSchurFactor.h.

◆ Z

template<class CAMERA>

typedef CAMERA::Measurement gtsam::RegularImplicitSchurFactor< CAMERA >::Z

protected

Definition at line 50 of file RegularImplicitSchurFactor.h.

Constructor & Destructor Documentation

◆ RegularImplicitSchurFactor() [1/2]

template<class CAMERA>

gtsam::RegularImplicitSchurFactor< CAMERA >::RegularImplicitSchurFactor ( )

inline

Constructor.

Definition at line 66 of file RegularImplicitSchurFactor.h.

◆ RegularImplicitSchurFactor() [2/2]

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

Definition at line 80 of file RegularImplicitSchurFactor.h.

◆ ~RegularImplicitSchurFactor()

template<class CAMERA>

gtsam::RegularImplicitSchurFactor< CAMERA >::~RegularImplicitSchurFactor ( )

inlineoverride

Destructor.

Definition at line 85 of file RegularImplicitSchurFactor.h.

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

Public Types

Public Member Functions

Static Public Member Functions

Public Attributes

Protected Types

Protected Attributes

Static Protected Attributes

Additional Inherited Members

Detailed Description

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

Member Typedef Documentation

◆ Error2s

◆ FBlocks

◆ MatrixDD

◆ MatrixZD

◆ Set

◆ shared_ptr

◆ This

◆ Z

Constructor & Destructor Documentation

◆ RegularImplicitSchurFactor() [1/2]

◆ RegularImplicitSchurFactor() [2/2]

◆ ~RegularImplicitSchurFactor()

Member Function Documentation

◆ augmentedInformation()

◆ augmentedJacobian()

◆ b()

◆ clone()

◆ E()

◆ equals()

◆ error()

◆ errorJF()

◆ Fs()

◆ getDim()

◆ getPointCovariance()

◆ gradient()

◆ gradientAtZero() [1/2]

◆ gradientAtZero() [2/2]

◆ hessianBlockDiagonal()

◆ hessianDiagonal()

◆ hessianDiagonalAdd()

◆ information()

◆ jacobian()

◆ multiplyHessianAdd() [1/4]

◆ multiplyHessianAdd() [2/4]

◆ multiplyHessianAdd() [3/4]

◆ multiplyHessianAdd() [4/4]

◆ multiplyHessianDummy()

◆ negate()

◆ print()

◆ projectError()

◆ projectError2()

◆ updateHessian()

Member Data Documentation

◆ b_

◆ D

◆ e1

◆ e2

◆ E_

◆ FBlocks_

◆ PointCovariance_

◆ ZDim

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