Base class for smart factors This base class has no internal point, but it has a measurement, noise model and an optional sensor pose. This class mainly computes the derivatives and returns them as a variety of factors. The methods take a Cameras argument, which should behave like PinholeCamera, and the value of a point, which is kept in the base class. More...

#include <SmartFactorBase.h>

Inheritance diagram for gtsam::SmartFactorBase< CAMERA >:

[legend]

Public Types
typedef CameraSet< CAMERA >	Cameras
	We use the new CameraSte data structure to refer to a set of cameras. More...

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

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

Public Types inherited from gtsam::NonlinearFactor
typedef boost::shared_ptr< This >	shared_ptr

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
void	add (const Z &measured, const Key &key)

void	add (const ZVector &measurements, const KeyVector &cameraKeys)

template<class SFM_TRACK >
void	add (const SFM_TRACK &trackToAdd)

Pose3	body_P_sensor () const

virtual Cameras	cameras (const Values &values) const
	Collect all cameras: important that in key order. More...

template<class POINT >
void	computeJacobians (FBlocks &Fs, Matrix &E, Vector &b, const Cameras &cameras, const POINT &point) const

template<class POINT >
void	computeJacobiansSVD (FBlocks &Fs, Matrix &Enull, Vector &b, const Cameras &cameras, const POINT &point) const
	SVD version. More...

virtual void	correctForMissingMeasurements (const Cameras &cameras, Vector &ue, boost::optional< typename Cameras::FBlocks & > Fs=boost::none, boost::optional< Matrix & > E=boost::none) const

boost::shared_ptr< RegularHessianFactor< Dim > >	createHessianFactor (const Cameras &cameras, const Point3 &point, const double lambda=0.0, bool diagonalDamping=false) const
	Linearize to a Hessianfactor. More...

boost::shared_ptr< JacobianFactorQ< Dim, ZDim > >	createJacobianQFactor (const Cameras &cameras, const Point3 &point, double lambda=0.0, bool diagonalDamping=false) const

boost::shared_ptr< JacobianFactor >	createJacobianSVDFactor (const Cameras &cameras, const Point3 &point, double lambda=0.0) const

boost::shared_ptr< RegularImplicitSchurFactor< CAMERA > >	createRegularImplicitSchurFactor (const Cameras &cameras, const Point3 &point, double lambda=0.0, bool diagonalDamping=false) const
	Return Jacobians as RegularImplicitSchurFactor with raw access. More...

size_t	dim () const override
	get the dimension (number of rows!) of the factor More...

bool	equals (const NonlinearFactor &p, double tol=1e-9) const override
	equals More...

const ZVector &	measured () const

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

	SmartFactorBase ()
	Default Constructor, for serialization. More...

	SmartFactorBase (const SharedNoiseModel &sharedNoiseModel, boost::optional< Pose3 > body_P_sensor=boost::none, size_t expectedNumberCameras=10)
	Constructor. More...

template<class POINT >
double	totalReprojectionError (const Cameras &cameras, const POINT &point) const

template<class POINT >
Vector	unwhitenedError (const Cameras &cameras, const POINT &point, boost::optional< typename Cameras::FBlocks & > Fs=boost::none, boost::optional< Matrix & > E=boost::none) const

void	updateAugmentedHessian (const Cameras &cameras, const Point3 &point, const double lambda, bool diagonalDamping, SymmetricBlockMatrix &augmentedHessian, const KeyVector allKeys) const

template<class POINT >
Vector	whitenedError (const Cameras &cameras, const POINT &point) const

void	whitenJacobians (FBlocks &F, Matrix &E, Vector &b) const
	Whiten the Jacobians computed by computeJacobians using noiseModel_. More...

	~SmartFactorBase () override
	Virtual destructor, subclasses from NonlinearFactor. More...

Public Member Functions inherited from gtsam::NonlinearFactor
	NonlinearFactor ()

template<typename CONTAINER >
	NonlinearFactor (const CONTAINER &keys)

virtual	~NonlinearFactor ()

virtual double	error (const Values &c) const =0

virtual bool	active (const Values &) const

virtual boost::shared_ptr< GaussianFactor >	linearize (const Values &c) const =0

virtual shared_ptr	clone () const

shared_ptr	rekey (const std::map< Key, Key > &rekey_mapping) const

shared_ptr	rekey (const KeyVector &new_keys) const

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	FillDiagonalF (const FBlocks &Fs, Matrix &F)
	Create BIG block-diagonal matrix F from Fblocks. More...

static Matrix	PointCov (Matrix &E)
	Computes Point Covariance P from E. More...

Public Attributes
GTSAM_MAKE_ALIGNED_OPERATOR_NEW typedef boost::shared_ptr< This >	shared_ptr
	shorthand for a smart pointer to a factor More...

Static Public Attributes
static const int	Dim = traits<CAMERA>::dimension
	Camera dimension. More...

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

Protected Attributes
boost::optional< Pose3 >	body_P_sensor_
	Pose of the camera in the body frame. More...

FBlocks	Fs

ZVector	measured_

SharedIsotropic	noiseModel_

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

Private Types
typedef NonlinearFactor	Base

typedef SmartFactorBase< CAMERA >	This

typedef CAMERA::Measurement	Z

typedef CAMERA::MeasurementVector	ZVector

Private Member Functions
template<class ARCHIVE >
void	serialize (ARCHIVE &ar, const unsigned int)

Friends
class	boost::serialization::access
	Serialization function. More...

Additional Inherited Members
Protected Types inherited from gtsam::NonlinearFactor
typedef Factor	Base

typedef NonlinearFactor	This

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::SmartFactorBase< CAMERA >

Base class for smart factors This base class has no internal point, but it has a measurement, noise model and an optional sensor pose. This class mainly computes the derivatives and returns them as a variety of factors. The methods take a Cameras argument, which should behave like PinholeCamera, and the value of a point, which is kept in the base class.

Definition at line 48 of file SmartFactorBase.h.

Member Typedef Documentation

template<class CAMERA>

typedef NonlinearFactor gtsam::SmartFactorBase< CAMERA >::Base

private

Definition at line 51 of file SmartFactorBase.h.

template<class CAMERA>

typedef CameraSet<CAMERA> gtsam::SmartFactorBase< CAMERA >::Cameras

We use the new CameraSte data structure to refer to a set of cameras.

Definition at line 91 of file SmartFactorBase.h.

template<class CAMERA>

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

Definition at line 61 of file SmartFactorBase.h.

template<class CAMERA>

typedef Eigen::Matrix<double, ZDim, Dim> gtsam::SmartFactorBase< CAMERA >::MatrixZD

Definition at line 60 of file SmartFactorBase.h.

template<class CAMERA>

typedef SmartFactorBase<CAMERA> gtsam::SmartFactorBase< CAMERA >::This

private

Definition at line 52 of file SmartFactorBase.h.

template<class CAMERA>

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

private

Definition at line 53 of file SmartFactorBase.h.

template<class CAMERA>

typedef CAMERA::MeasurementVector gtsam::SmartFactorBase< CAMERA >::ZVector

private

Definition at line 54 of file SmartFactorBase.h.

Constructor & Destructor Documentation

template<class CAMERA>

gtsam::SmartFactorBase< CAMERA >::SmartFactorBase ( )

inline

Default Constructor, for serialization.

Definition at line 94 of file SmartFactorBase.h.

template<class CAMERA>

gtsam::SmartFactorBase< CAMERA >::SmartFactorBase	(	const SharedNoiseModel &	sharedNoiseModel,
		boost::optional< Pose3 >	body_P_sensor = `boost::none`,
		size_t	expectedNumberCameras = `10`
	)

inline

Constructor.

Definition at line 97 of file SmartFactorBase.h.

template<class CAMERA>

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

inlineoverride

Virtual destructor, subclasses from NonlinearFactor.

Definition at line 115 of file SmartFactorBase.h.

Member Function Documentation

template<class CAMERA>

void gtsam::SmartFactorBase< CAMERA >::add	(	const Z &	measured,
		const Key &	key
	)

inline

Add a new measurement and pose/camera key

Parameters

measured	is the 2m dimensional projection of a single landmark
key	is the index corresponding to the camera observing the landmark

Definition at line 123 of file SmartFactorBase.h.

template<class CAMERA>

void gtsam::SmartFactorBase< CAMERA >::add	(	const ZVector &	measurements,
		const KeyVector &	cameraKeys
	)

inline

Add a bunch of measurements, together with the camera keys

Definition at line 135 of file SmartFactorBase.h.

template<class CAMERA>

template<class SFM_TRACK >

void gtsam::SmartFactorBase< CAMERA >::add ( const SFM_TRACK & trackToAdd )

inline

Adds an entire SfM_track (collection of cameras observing a single point). The noise is assumed to be the same for all measurements

Definition at line 147 of file SmartFactorBase.h.

template<class CAMERA>

Pose3 gtsam::SmartFactorBase< CAMERA >::body_P_sensor ( ) const

inline

Definition at line 397 of file SmartFactorBase.h.

template<class CAMERA>

virtual Cameras gtsam::SmartFactorBase< CAMERA >::cameras ( const Values & values ) const

inlinevirtual

Collect all cameras: important that in key order.

Reimplemented in gtsam::SmartStereoProjectionPoseFactor, and gtsam::SmartProjectionPoseFactor< CALIBRATION >.

Definition at line 165 of file SmartFactorBase.h.

template<class CAMERA>

template<class POINT >

void gtsam::SmartFactorBase< CAMERA >::computeJacobians	(	FBlocks &	Fs,
		Matrix &	E,
		Vector &	b,
		const Cameras &	cameras,
		const POINT &	point
	)		const

inline

Compute F, E, and b (called below in both vanilla and SVD versions), where F is a vector of derivatives wrpt the cameras, and E the stacked derivatives with respect to the point. The value of cameras/point are passed as parameters. TODO: Kill this obsolete method

Definition at line 275 of file SmartFactorBase.h.

template<class CAMERA>

template<class POINT >

void gtsam::SmartFactorBase< CAMERA >::computeJacobiansSVD	(	FBlocks &	Fs,
		Matrix &	Enull,
		Vector &	b,
		const Cameras &	cameras,
		const POINT &	point
	)		const

inline

SVD version.

Definition at line 286 of file SmartFactorBase.h.

template<class CAMERA>

virtual void gtsam::SmartFactorBase< CAMERA >::correctForMissingMeasurements	(	const Cameras &	cameras,
		Vector &	ue,
		boost::optional< typename Cameras::FBlocks & >	Fs = `boost::none`,
		boost::optional< Matrix & >	E = `boost::none`
	)		const

inlinevirtual

This corrects the Jacobians for the case in which some pixel measurement is missing (nan) In practice, this does not do anything in the monocular case, but it is implemented in the stereo version

Reimplemented in gtsam::SmartStereoProjectionFactor.

Definition at line 235 of file SmartFactorBase.h.

template<class CAMERA>

boost::shared_ptr<RegularHessianFactor<Dim> > gtsam::SmartFactorBase< CAMERA >::createHessianFactor	(	const Cameras &	cameras,
		const Point3 &	point,
		const double	lambda = `0.0`,
		bool	diagonalDamping = `false`
	)		const

inline

Linearize to a Hessianfactor.

Definition at line 302 of file SmartFactorBase.h.

template<class CAMERA>

boost::shared_ptr<JacobianFactorQ<Dim, ZDim> > gtsam::SmartFactorBase< CAMERA >::createJacobianQFactor	(	const Cameras &	cameras,
		const Point3 &	point,
		double	lambda = `0.0`,
		bool	diagonalDamping = `false`
	)		const

inline

Return Jacobians as JacobianFactorQ

Definition at line 357 of file SmartFactorBase.h.

template<class CAMERA>

boost::shared_ptr<JacobianFactor> gtsam::SmartFactorBase< CAMERA >::createJacobianSVDFactor	(	const Cameras &	cameras,
		const Point3 &	point,
		double	lambda = `0.0`
	)		const

inline

Return Jacobians as JacobianFactorSVD TODO lambda is currently ignored

Definition at line 374 of file SmartFactorBase.h.

template<class CAMERA>

boost::shared_ptr<RegularImplicitSchurFactor<CAMERA> > gtsam::SmartFactorBase< CAMERA >::createRegularImplicitSchurFactor	(	const Cameras &	cameras,
		const Point3 &	point,
		double	lambda = `0.0`,
		bool	diagonalDamping = `false`
	)		const

inline

Return Jacobians as RegularImplicitSchurFactor with raw access.

Definition at line 342 of file SmartFactorBase.h.

template<class CAMERA>

size_t gtsam::SmartFactorBase< CAMERA >::dim ( ) const

inlineoverridevirtual

get the dimension (number of rows!) of the factor

Implements gtsam::NonlinearFactor.

Definition at line 155 of file SmartFactorBase.h.

template<class CAMERA>

bool gtsam::SmartFactorBase< CAMERA >::equals	(	const NonlinearFactor &	p,
		double	tol = `1e-9`
	)		const

inlineoverridevirtual

equals

Reimplemented from gtsam::NonlinearFactor.

Reimplemented in gtsam::SmartProjectionFactor< CAMERA >, and gtsam::SmartProjectionFactor< PinholePose< CALIBRATION > >.

Definition at line 190 of file SmartFactorBase.h.

template<class CAMERA>

static void gtsam::SmartFactorBase< CAMERA >::FillDiagonalF	(	const FBlocks &	Fs,
		Matrix &	F
	)

inlinestatic

Create BIG block-diagonal matrix F from Fblocks.

Definition at line 388 of file SmartFactorBase.h.

template<class CAMERA>

const ZVector& gtsam::SmartFactorBase< CAMERA >::measured ( ) const

inline

return the measurements

Definition at line 160 of file SmartFactorBase.h.

template<class CAMERA>

static Matrix gtsam::SmartFactorBase< CAMERA >::PointCov ( Matrix & E )

inlinestatic

Computes Point Covariance P from E.

Definition at line 264 of file SmartFactorBase.h.

template<class CAMERA>

void gtsam::SmartFactorBase< CAMERA >::print	(	const std::string &	s = `""`,
		const KeyFormatter &	keyFormatter = `DefaultKeyFormatter`
	)		const

inlineoverridevirtual

print

Parameters

s	optional string naming the factor
keyFormatter	optional formatter useful for printing Symbols

Reimplemented from gtsam::NonlinearFactor.

Reimplemented in gtsam::SmartProjectionFactor< CAMERA >, and gtsam::SmartProjectionFactor< PinholePose< CALIBRATION > >.

Definition at line 177 of file SmartFactorBase.h.

template<class CAMERA>

template<class ARCHIVE >

void gtsam::SmartFactorBase< CAMERA >::serialize	(	ARCHIVE &	ar,
		const unsigned int
	)

inlineprivate

Definition at line 409 of file SmartFactorBase.h.

template<class CAMERA>

template<class POINT >

double gtsam::SmartFactorBase< CAMERA >::totalReprojectionError	(	const Cameras &	cameras,
		const POINT &	point
	)		const

inline

Calculate the error of the factor. This is the log-likelihood, e.g. $0.5(h(x)-z)^2/\sigma^2$ in case of Gaussian. In this class, we take the raw prediction error $ h(x)-z $ , ask the noise model to transform it to $(h(x)-z)^2/\sigma^2$ , and then multiply by 0.5. Will be used in "error(Values)" function required by NonlinearFactor base class

Definition at line 257 of file SmartFactorBase.h.

template<class CAMERA>

template<class POINT >

Vector gtsam::SmartFactorBase< CAMERA >::unwhitenedError	(	const Cameras &	cameras,
		const POINT &	point,
		boost::optional< typename Cameras::FBlocks & >	Fs = `boost::none`,
		boost::optional< Matrix & >	E = `boost::none`
	)		const

inline

Compute reprojection errors [h(x)-z] = [cameras.project(p)-z] and derivatives

Definition at line 205 of file SmartFactorBase.h.

template<class CAMERA>

void gtsam::SmartFactorBase< CAMERA >::updateAugmentedHessian	(	const Cameras &	cameras,
		const Point3 &	point,
		const double	lambda,
		bool	diagonalDamping,
		SymmetricBlockMatrix &	augmentedHessian,
		const KeyVector	allKeys
	)		const

inline

Add the contribution of the smart factor to a pre-allocated Hessian, using sparse linear algebra. More efficient than the creation of the Hessian without preallocation of the SymmetricBlockMatrix

Definition at line 322 of file SmartFactorBase.h.

template<class CAMERA>

template<class POINT >

Vector gtsam::SmartFactorBase< CAMERA >::whitenedError	(	const Cameras &	cameras,
		const POINT &	point
	)		const

inline

Calculate vector of re-projection errors [h(x)-z] = [cameras.project(p) - z] Noise model applied

Definition at line 243 of file SmartFactorBase.h.

template<class CAMERA>

void gtsam::SmartFactorBase< CAMERA >::whitenJacobians	(	FBlocks &	F,
		Matrix &	E,
		Vector &	b
	)		const

inline

Whiten the Jacobians computed by computeJacobians using noiseModel_.

Definition at line 333 of file SmartFactorBase.h.

Friends And Related Function Documentation

template<class CAMERA>

friend class boost::serialization::access

friend

Serialization function.

Definition at line 407 of file SmartFactorBase.h.

Member Data Documentation

template<class CAMERA>

boost::optional<Pose3> gtsam::SmartFactorBase< CAMERA >::body_P_sensor_

protected

Pose of the camera in the body frame.

Definition at line 79 of file SmartFactorBase.h.

template<class CAMERA>

const int gtsam::SmartFactorBase< CAMERA >::Dim = traits<CAMERA>::dimension

static

Camera dimension.

Definition at line 58 of file SmartFactorBase.h.

template<class CAMERA>

FBlocks gtsam::SmartFactorBase< CAMERA >::Fs

mutableprotected

Definition at line 82 of file SmartFactorBase.h.

template<class CAMERA>

ZVector gtsam::SmartFactorBase< CAMERA >::measured_

protected

2D measurement and noise model for each of the m views We keep a copy of measurements for I/O and computing the error. The order is kept the same as the keys that we use to create the factor.

Definition at line 77 of file SmartFactorBase.h.

template<class CAMERA>

SharedIsotropic gtsam::SmartFactorBase< CAMERA >::noiseModel_

protected

As of Feb 22, 2015, the noise model is the same for all measurements and is isotropic. This allows for moving most calculations of Schur complement etc to be moved to CameraSet very easily, and also agrees pragmatically with what is normally done.

Definition at line 70 of file SmartFactorBase.h.

template<class CAMERA>

GTSAM_MAKE_ALIGNED_OPERATOR_NEW typedef boost::shared_ptr<This> gtsam::SmartFactorBase< CAMERA >::shared_ptr

shorthand for a smart pointer to a factor

Definition at line 88 of file SmartFactorBase.h.

template<class CAMERA>

const int gtsam::SmartFactorBase< CAMERA >::ZDim = traits<Z>::dimension

static

Measurement dimension.

Definition at line 59 of file SmartFactorBase.h.

The documentation for this class was generated from the following file:

SmartFactorBase.h

Public Types

Public Member Functions

Static Public Member Functions

Public Attributes

Static Public Attributes

Protected Attributes

Private Types

Private Member Functions

Friends

Additional Inherited Members

Detailed Description

template<class CAMERA> class gtsam::SmartFactorBase< CAMERA >

Member Typedef Documentation

Constructor & Destructor Documentation

Member Function Documentation

Friends And Related Function Documentation

Member Data Documentation

template<class CAMERA>
class gtsam::SmartFactorBase< CAMERA >