#include <RegularImplicitSchurFactor.h>

Inheritance diagram for gtsam::RegularImplicitSchurFactor< CAMERA >:

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

typedef boost::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 boost::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	empty () const override

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

double	error (const VectorValues &x) const override

double	errorJF (const VectorValues &x) const

std::vector< MatrixZD, Eigen::aligned_allocator< MatrixZD > > &	FBlocks () 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 std::vector< MatrixZD, Eigen::aligned_allocator< MatrixZD > > &FBlocks, 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)

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

virtual	~GaussianFactor ()

Public Member Functions inherited from gtsam::Factor
virtual	~Factor ()=default
	Default destructor. 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...

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

const std::vector< MatrixZD, Eigen::aligned_allocator< MatrixZD > >	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)

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

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

Definition at line 25 of file RegularImplicitSchurFactor.h.

Member Typedef Documentation

template<class CAMERA>

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

Definition at line 263 of file RegularImplicitSchurFactor.h.

template<class CAMERA>

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

protected

camera hessian

Definition at line 41 of file RegularImplicitSchurFactor.h.

template<class CAMERA>

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

protected

type of an F block

Definition at line 40 of file RegularImplicitSchurFactor.h.

template<class CAMERA>

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

protected

Definition at line 34 of file RegularImplicitSchurFactor.h.

template<class CAMERA>

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

shared_ptr to this class

Definition at line 29 of file RegularImplicitSchurFactor.h.

template<class CAMERA>

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

Typedef to this class.

Definition at line 28 of file RegularImplicitSchurFactor.h.

template<class CAMERA>

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

protected

Definition at line 36 of file RegularImplicitSchurFactor.h.

Constructor & Destructor Documentation

template<class CAMERA>

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

inline

Constructor.

Definition at line 51 of file RegularImplicitSchurFactor.h.

template<class CAMERA>

gtsam::RegularImplicitSchurFactor< CAMERA >::RegularImplicitSchurFactor	(	const KeyVector &	keys,
		const std::vector< MatrixZD, Eigen::aligned_allocator< MatrixZD > > &	FBlocks,
		const Matrix &	E,
		const Matrix &	P,
		const Vector &	b
	)

inline

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

Definition at line 55 of file RegularImplicitSchurFactor.h.

template<class CAMERA>

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

inlineoverride

Destructor.

Definition at line 62 of file RegularImplicitSchurFactor.h.

Member Function Documentation

template<class CAMERA>

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

inlineoverridevirtual

Compute full augmented information matrix

Implements gtsam::GaussianFactor.

Definition at line 132 of file RegularImplicitSchurFactor.h.

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.

Definition at line 120 of file RegularImplicitSchurFactor.h.

template<class CAMERA>

const Vector& gtsam::RegularImplicitSchurFactor< CAMERA >::b ( ) const

inline

Definition at line 73 of file RegularImplicitSchurFactor.h.

template<class CAMERA>

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

inlineoverridevirtual

Clone a factor (make a deep copy)

Implements gtsam::GaussianFactor.

Reimplemented in gtsam::DummyFactor< CAMERA >.

Definition at line 233 of file RegularImplicitSchurFactor.h.

template<class CAMERA>

const Matrix& gtsam::RegularImplicitSchurFactor< CAMERA >::E ( ) const

inline

Definition at line 69 of file RegularImplicitSchurFactor.h.

template<class CAMERA>

bool gtsam::RegularImplicitSchurFactor< CAMERA >::empty ( ) const

inlineoverridevirtual

Test whether the factor is empty

Implements gtsam::GaussianFactor.

Definition at line 240 of file RegularImplicitSchurFactor.h.

template<class CAMERA>

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

inlineoverridevirtual

equals

Implements gtsam::GaussianFactor.

Definition at line 95 of file RegularImplicitSchurFactor.h.

template<class CAMERA>

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

inlineoverridevirtual

Print for testable

Implements gtsam::GaussianFactor.

Definition at line 295 of file RegularImplicitSchurFactor.h.

template<class CAMERA>

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

inline

Definition at line 317 of file RegularImplicitSchurFactor.h.

template<class CAMERA>

std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& gtsam::RegularImplicitSchurFactor< CAMERA >::FBlocks ( ) const

inline

Definition at line 65 of file RegularImplicitSchurFactor.h.

template<class CAMERA>

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

inlineoverridevirtual

Degrees of freedom of camera.

Implements gtsam::GaussianFactor.

Definition at line 111 of file RegularImplicitSchurFactor.h.

template<class CAMERA>

const Matrix& gtsam::RegularImplicitSchurFactor< CAMERA >::getPointCovariance ( ) const

inline

Definition at line 77 of file RegularImplicitSchurFactor.h.

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.

Definition at line 480 of file RegularImplicitSchurFactor.h.

template<class CAMERA>

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

inlineoverridevirtual

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

Implements gtsam::GaussianFactor.

Definition at line 438 of file RegularImplicitSchurFactor.h.

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.

Definition at line 460 of file RegularImplicitSchurFactor.h.

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.

Definition at line 208 of file RegularImplicitSchurFactor.h.

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.

Definition at line 182 of file RegularImplicitSchurFactor.h.

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.

Definition at line 151 of file RegularImplicitSchurFactor.h.

template<class CAMERA>

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

inlineoverridevirtual

Compute full information matrix

Implements gtsam::GaussianFactor.

Definition at line 140 of file RegularImplicitSchurFactor.h.

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.

Definition at line 125 of file RegularImplicitSchurFactor.h.

template<class CAMERA>

static void gtsam::RegularImplicitSchurFactor< CAMERA >::multiplyHessianAdd	(	const Matrix &	F,
		const Matrix &	E,
		const Matrix &	PointCovariance,
		double	alpha,
		const Vector &	x,
		Vector &	y
	)

inlinestatic

Definition at line 253 of file RegularImplicitSchurFactor.h.

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

Definition at line 361 of file RegularImplicitSchurFactor.h.

template<class CAMERA>