Detailed Description

template<class POSE, class CALIBRATION, size_t D>
class gtsam::SmartFactorBase< POSE, CALIBRATION, D >

Base class with no internal point, completely functional.

Inheritance diagram for gtsam::SmartFactorBase< POSE, CALIBRATION, D >:

Public Member Functions
	SmartFactorBase (boost::optional< POSE > body_P_sensor=boost::none)
	Constructor. More...

virtual	~SmartFactorBase ()
	Virtual destructor.

void	add (const Point2 &measured_i, const Key &poseKey_i, const SharedNoiseModel &noise_i)
	add a new measurement and pose key More...

void	add (std::vector< Point2 > &measurements, std::vector< Key > &poseKeys, std::vector< SharedNoiseModel > &noises)
	variant of the previous add: adds a bunch of measurements, together with the camera keys and noises

void	add (std::vector< Point2 > &measurements, std::vector< Key > &poseKeys, const SharedNoiseModel &noise)
	variant of the previous add: adds a bunch of measurements and uses the same noise model for all of them

void	add (const SfM_Track &trackToAdd, const SharedNoiseModel &noise)
	Adds an entire SfM_track (collection of cameras observing a single point). More...

const std::vector< Point2 > &	measured () const
	return the measurements

const std::vector < SharedNoiseModel > &	noise () const
	return the noise model

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

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

Vector	reprojectionError (const Cameras &cameras, const Point3 &point) const
	Calculate vector of re-projection errors, before applying noise model.

double	totalReprojectionError (const Cameras &cameras, const Point3 &point) const
	Calculate the error of the factor. More...

void	computeEP (Matrix &E, Matrix &PointCov, const Cameras &cameras, const Point3 &point) const
	Assumes non-degenerate !

double	computeJacobians (std::vector< KeyMatrix2D > &Fblocks, Matrix &E, Vector &b, const Cameras &cameras, const Point3 &point) const
	Compute F, E only (called below in both vanilla and SVD versions) Given a Point3, assumes dimensionality is 3.

double	computeJacobians (std::vector< KeyMatrix2D > &Fblocks, Matrix &E, Matrix3 &PointCov, Vector &b, const Cameras &cameras, const Point3 &point, double lambda=0.0, bool diagonalDamping=false) const
	Version that computes PointCov, with optional lambda parameter.

double	computeJacobians (Matrix &F, Matrix &E, Matrix3 &PointCov, Vector &b, const Cameras &cameras, const Point3 &point, const double lambda=0.0) const

double	computeJacobiansSVD (std::vector< KeyMatrix2D > &Fblocks, Matrix &Enull, Vector &b, const Cameras &cameras, const Point3 &point, double lambda=0.0, bool diagonalDamping=false) const
	SVD version.

double	computeJacobiansSVD (Matrix &F, Matrix &Enull, Vector &b, const Cameras &cameras, const Point3 &point) const
	Matrix version of SVD.

boost::shared_ptr < RegularHessianFactor< D > >	createHessianFactor (const Cameras &cameras, const Point3 &point, const double lambda=0.0, bool diagonalDamping=false) const
	linearize returns a Hessianfactor that is an approximation of error(p)

void	schurComplement (const std::vector< KeyMatrix2D > &Fblocks, const Matrix &E, const Matrix &PointCov, const Vector &b, std::vector< Matrix > &Gs, std::vector< Vector > &gs) const

void	sparseSchurComplement (const std::vector< KeyMatrix2D > &Fblocks, const Matrix &E, const Matrix &P, const Vector &b, SymmetricBlockMatrix &augmentedHessian) const

void	sparseSchurComplement (const std::vector< KeyMatrix2D > &Fblocks, const Matrix &E, const Matrix &P, const Vector &b, std::vector< Matrix > &Gs, std::vector< Vector > &gs) const

void	updateAugmentedHessian (const Cameras &cameras, const Point3 &point, const double lambda, bool diagonalDamping, SymmetricBlockMatrix &augmentedHessian, const FastVector< Key > allKeys) const

void	updateSparseSchurComplement (const std::vector< KeyMatrix2D > &Fblocks, const Matrix &E, const Matrix &P, const Vector &b, const double f, const FastVector< Key > allKeys, SymmetricBlockMatrix &augmentedHessian) const

boost::shared_ptr < ImplicitSchurFactor< D > >	createImplicitSchurFactor (const Cameras &cameras, const Point3 &point, double lambda=0.0, bool diagonalDamping=false) const

boost::shared_ptr < JacobianFactorQ< D > >	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

Public Member Functions inherited from gtsam::NonlinearFactor
	NonlinearFactor ()
	Default constructor for I/O only.

template<typename CONTAINER >
	NonlinearFactor (const CONTAINER &keys)
	Constructor from a collection of the keys involved in this factor.

virtual	~NonlinearFactor ()
	Destructor.

virtual double	error (const Values &c) const =0
	Calculate the error of the factor This is typically equal to log-likelihood, e.g. More...

virtual size_t	dim () const =0
	get the dimension of the factor (number of rows on linearization)

virtual bool	active (const Values &c) const
	Checks whether a factor should be used based on a set of values. More...

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

virtual shared_ptr	clone () const
	Create a symbolic factor using the given ordering to determine the variable indices. More...

shared_ptr	rekey (const std::map< Key, Key > &rekey_mapping) const
	Creates a shared_ptr clone of the factor with different keys using a map from old->new keys.

shared_ptr	rekey (const std::vector< Key > &new_keys) const
	Clones a factor and fully replaces its keys. More...

Public Member Functions inherited from gtsam::Factor
Key	front () const
	First key.

Key	back () const
	Last key.

const_iterator	find (Key key) const
	find

const FastVector< Key > &	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

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

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

FastVector< Key > &	keys ()

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

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

Public Types
typedef boost::shared_ptr< This >	shared_ptr
	shorthand for a smart pointer to a factor

typedef PinholeCamera < CALIBRATION >	Camera
	shorthand for a pinhole camera

typedef std::vector< Camera >	Cameras

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

Public Types inherited from gtsam::Factor
typedef FastVector< Key >::iterator	iterator
	Iterator over keys.

typedef FastVector< Key > ::const_iterator	const_iterator
	Const iterator over keys.

Protected Types
typedef Eigen::Matrix< double, 2, D >	Matrix2D
	Definitions for blocks of F.

typedef Eigen::Matrix< double, D, 2 >	MatrixD2

typedef std::pair< Key, Matrix2D >	KeyMatrix2D

typedef Eigen::Matrix< double, D, D >	MatrixDD

typedef Eigen::Matrix< double, 2, 3 >	Matrix23

typedef Eigen::Matrix< double, D, 1 >	VectorD

typedef Eigen::Matrix< double, 2, 2 >	Matrix2

typedef NonlinearFactor	Base
	shorthand for base class type

typedef SmartFactorBase< POSE, CALIBRATION, D >	This
	shorthand for this class

Protected Types inherited from gtsam::NonlinearFactor
typedef Factor	Base

typedef NonlinearFactor	This

Protected Attributes
std::vector< Point2 >	measured_
	2D measurement for each of the m views

std::vector< SharedNoiseModel >	noise_
	noise model used More...

boost::optional< POSE >	body_P_sensor_
	The pose of the sensor in the body frame (one for all cameras)

Protected Attributes inherited from gtsam::Factor
FastVector< Key >	keys_
	The keys involved in this factor.

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

Constructor & Destructor Documentation

template<class POSE , class CALIBRATION , size_t D>

gtsam::SmartFactorBase< POSE, CALIBRATION, D >::SmartFactorBase ( boost::optional< POSE > body_P_sensor = boost::none )

inline

Constructor.

Parameters

body_P_sensor is the transform from body to sensor frame (default identity)

Member Function Documentation

template<class POSE , class CALIBRATION , size_t D>

void gtsam::SmartFactorBase< POSE, CALIBRATION, D >::add	(	const Point2 &	measured_i,
		const Key &	poseKey_i,
		const SharedNoiseModel &	noise_i
	)

inline

add a new measurement and pose key

Parameters

measured_i	is the 2m dimensional projection of a single landmark
poseKey	is the index corresponding to the camera observing the landmark
noise_i	is the measurement noise

template<class POSE , class CALIBRATION , size_t D>

void gtsam::SmartFactorBase< POSE, CALIBRATION, D >::add	(	const SfM_Track &	trackToAdd,
		const SharedNoiseModel &	noise
	)

inline

Adds an entire SfM_track (collection of cameras observing a single point).

The noise is assumed to be the same for all measurements

template<class POSE , class CALIBRATION , size_t D>

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

inlinevirtual

print

Parameters

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

Reimplemented from gtsam::NonlinearFactor.

Reimplemented in gtsam::SmartProjectionFactor< POSE, LANDMARK, CALIBRATION, D >, gtsam::SmartProjectionFactor< POSE, LANDMARK, CALIBRATION, 6 >, and gtsam::SmartProjectionPoseFactor< POSE, LANDMARK, CALIBRATION >.

template<class POSE , class CALIBRATION , size_t D>

void gtsam::SmartFactorBase< POSE, CALIBRATION, D >::schurComplement	(	const std::vector< KeyMatrix2D > &	Fblocks,
		const Matrix &	E,
		const Matrix &	PointCov,
		const Vector &	b,
		std::vector< Matrix > &	Gs,
		std::vector< Vector > &	gs
	)		const

inline

Compute Full F ????

template<class POSE , class CALIBRATION , size_t D>

double gtsam::SmartFactorBase< POSE, CALIBRATION, D >::totalReprojectionError	(	const Cameras &	cameras,
		const Point3 &	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. This is different from reprojectionError(cameras,point) as each point is whitened

template<class POSE , class CALIBRATION , size_t D>

void gtsam::SmartFactorBase< POSE, CALIBRATION, D >::updateSparseSchurComplement	(	const std::vector< KeyMatrix2D > &	Fblocks,
		const Matrix &	E,
		const Matrix &	P,
		const Vector &	b,
		const double	f,
		const FastVector< Key >	allKeys,
		SymmetricBlockMatrix &	augmentedHessian
	)		const

inline

< A block from the Hessian matrix

Member Data Documentation

template<class POSE , class CALIBRATION , size_t D>

std::vector<SharedNoiseModel> gtsam::SmartFactorBase< POSE, CALIBRATION, D >::noise_

protected

noise model used

(important that the order is the same as the keys that we use to create the factor)

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

/home/cbeall3/git/gtsam/gtsam/slam/SmartFactorBase.h

Detailed Description

template<class POSE, class CALIBRATION, size_t D> class gtsam::SmartFactorBase< POSE, CALIBRATION, D >

Public Member Functions

Public Types

Protected Types

Protected Attributes

Friends

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation

template<class POSE, class CALIBRATION, size_t D>
class gtsam::SmartFactorBase< POSE, CALIBRATION, D >