#include <JacobianFactorSVD.h>

Inheritance diagram for gtsam::JacobianFactorSVD< D, ZDim >:

[legend]

Public Member Functions
	JacobianFactorSVD ()
	Default constructor. More...

	JacobianFactorSVD (const KeyVector &keys, const SharedDiagonal &model=SharedDiagonal())
	Empty constructor with keys. More...

	JacobianFactorSVD (const KeyVector &keys, const std::vector< MatrixZD, Eigen::aligned_allocator< MatrixZD > > &Fblocks, const Matrix &Enull, const Vector &b, const SharedDiagonal &model=SharedDiagonal())
	Constructor Takes the CameraSet derivatives (as ZDim*D blocks of block-diagonal F) and a reduced point derivative, Enull and creates a reduced-rank Jacobian factor on the CameraSet. More...

Public Member Functions inherited from gtsam::RegularJacobianFactor< D >
VectorValues	gradientAtZero () const override
	Expose base class gradientAtZero. More...

void	gradientAtZero (double *d) const override
	Raw memory access version of gradientAtZero. More...

void	hessianDiagonal (double *d) const override
	Raw memory access version of hessianDiagonal. More...

void	multiplyHessianAdd (double alpha, const VectorValues &x, VectorValues &y) const override

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

Vector	operator* (const double *x) const
	double* Matrix-vector multiply, i.e. y = A*x RAW memory access! Assumes keys start at 0 and go to M-1, and x is laid out that way More...

	RegularJacobianFactor ()
	Default constructor. More...

template<typename TERMS >
	RegularJacobianFactor (const TERMS &terms, const Vector &b, const SharedDiagonal &model=SharedDiagonal())

template<typename KEYS >
	RegularJacobianFactor (const KEYS &keys, const VerticalBlockMatrix &augmentedMatrix, const SharedDiagonal &sigmas=SharedDiagonal())

void	transposeMultiplyAdd (double alpha, const Vector &e, double *x) const
	double* Transpose Matrix-vector multiply, i.e. x += A'*e RAW memory access! Assumes keys start at 0 and go to M-1, and y is laid out that way More...

Public Member Functions inherited from gtsam::JacobianFactor
Matrix	augmentedInformation () const override

Matrix	augmentedJacobian () const override

Matrix	augmentedJacobianUnweighted () const

GaussianFactor::shared_ptr	clone () const override

size_t	cols () const

std::pair< boost::shared_ptr< GaussianConditional >, shared_ptr >	eliminate (const Ordering &keys)

bool	empty () const override

bool	equals (const GaussianFactor &lf, double tol=1e-9) const override

double	error (const VectorValues &c) const override

Vector	error_vector (const VectorValues &c) const

const SharedDiagonal &	get_model () const

SharedDiagonal &	get_model ()

constABlock	getA (const_iterator variable) const

constABlock	getA () const

ABlock	getA (iterator variable)

ABlock	getA ()

const constBVector	getb () const

BVector	getb ()

DenseIndex	getDim (const_iterator variable) const override

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

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

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

Matrix	information () const override

bool	isConstrained () const

std::pair< Matrix, Vector >	jacobian () const override
	Returns (dense) A,b pair associated with factor, bakes in the weights. More...

	JacobianFactor (const GaussianFactor &gf)

	JacobianFactor (const JacobianFactor &jf)

	JacobianFactor (const HessianFactor &hf)

	JacobianFactor ()

	JacobianFactor (const Vector &b_in)

	JacobianFactor (Key i1, const Matrix &A1, const Vector &b, const SharedDiagonal &model=SharedDiagonal())

	JacobianFactor (Key i1, const Matrix &A1, Key i2, const Matrix &A2, const Vector &b, const SharedDiagonal &model=SharedDiagonal())

	JacobianFactor (Key i1, const Matrix &A1, Key i2, const Matrix &A2, Key i3, const Matrix &A3, const Vector &b, const SharedDiagonal &model=SharedDiagonal())

template<typename TERMS >
	JacobianFactor (const TERMS &terms, const Vector &b, const SharedDiagonal &model=SharedDiagonal())

template<typename KEYS >
	JacobianFactor (const KEYS &keys, const VerticalBlockMatrix &augmentedMatrix, const SharedDiagonal &sigmas=SharedDiagonal())

	JacobianFactor (const GaussianFactorGraph &graph)

	JacobianFactor (const GaussianFactorGraph &graph, const VariableSlots &p_variableSlots)

	JacobianFactor (const GaussianFactorGraph &graph, const Ordering &ordering)

	JacobianFactor (const GaussianFactorGraph &graph, const Ordering &ordering, const VariableSlots &p_variableSlots)

std::pair< Matrix, Vector >	jacobianUnweighted () const
	Returns (dense) A,b pair associated with factor, does not bake in weights. More...

const VerticalBlockMatrix &	matrixObject () const

VerticalBlockMatrix &	matrixObject ()

void	multiplyHessianAdd (double alpha, const double x, double y, const std::vector< size_t > &accumulatedDims) const

GaussianFactor::shared_ptr	negate () const override

Vector	operator* (const VectorValues &x) const

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

size_t	rows () const

void	setModel (bool anyConstrained, const Vector &sigmas)

boost::shared_ptr< GaussianConditional >	splitConditional (size_t nrFrontals)

void	transposeMultiplyAdd (double alpha, const Vector &e, VectorValues &x) const

Vector	unweighted_error (const VectorValues &c) const

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

JacobianFactor	whiten () const

	~JacobianFactor () override

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

Private Types
typedef RegularJacobianFactor< D >	Base

typedef std::pair< Key, Matrix >	KeyMatrix

typedef Eigen::Matrix< double, ZDim, D >	MatrixZD

Additional Inherited Members
Public Types inherited from gtsam::JacobianFactor
typedef VerticalBlockMatrix::Block	ABlock

typedef GaussianFactor	Base
	Typedef to base class. More...

typedef ABlock::ColXpr	BVector

typedef VerticalBlockMatrix::constBlock	constABlock

typedef constABlock::ConstColXpr	constBVector

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

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

Static Public Member Functions inherited from gtsam::GaussianFactor
template<typename CONTAINER >
static DenseIndex	Slot (const CONTAINER &keys, Key key)

Protected Member Functions inherited from gtsam::JacobianFactor
template<typename TERMS >
void	fillTerms (const TERMS &terms, const Vector &b, const SharedDiagonal &noiseModel)
	Internal function to fill blocks and set dimensions. More...

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)

Protected Attributes inherited from gtsam::JacobianFactor
VerticalBlockMatrix	Ab_

noiseModel::Diagonal::shared_ptr	model_

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

Detailed Description

template<size_t D, size_t ZDim>
class gtsam::JacobianFactorSVD< D, ZDim >

JacobianFactor for Schur complement that uses the "Nullspace Trick" by Mourikis

This trick is equivalent to the Schur complement, but can be faster. In essence, the linear factor |E*dp + F*dX - b|, where p is point and X are poses, is multiplied by Enull, a matrix that spans the left nullspace of E, i.e., The mx3 matrix is analyzed with SVD as E = [Erange Enull]*S*V (mxm * mx3 * 3x3) where Enull is an m x (m-3) matrix Then Enull'*E*dp = 0, and |Enull'*E*dp + Enull'*F*dX - Enull'*b| == |Enull'*F*dX - Enull'*b| Normally F is m x 6*numKeys, and Enull'*F yields an (m-3) x 6*numKeys matrix.

The code below assumes that F is block diagonal and is given as a vector of ZDim*D blocks. Example: m = 4 (2 measurements), Enull = 4*1, F = 4*12 (for D=6) Then Enull'*F = 1*4 * 4*12 = 1*12, but each 1*6 piece can be computed as a 1x2 * 2x6 mult

Definition at line 28 of file JacobianFactorSVD.h.