Inheritance diagram for gtsam::SmartProjectionPoseFactor< POSE, LANDMARK, CALIBRATION >:

Public Member Functions
	SmartProjectionPoseFactor (const double rankTol=1, const double linThreshold=-1, const bool manageDegeneracy=false, const bool enableEPI=false, boost::optional< POSE > body_P_sensor=boost::none, LinearizationMode linearizeTo=HESSIAN, double landmarkDistanceThreshold=1e10, double dynamicOutlierRejectionThreshold=-1)
	Constructor. More...

virtual	~SmartProjectionPoseFactor ()
	Virtual destructor.

void	add (const Point2 measured_i, const Key poseKey_i, const SharedNoiseModel noise_i, const boost::shared_ptr< CALIBRATION > K_i)
	add a new measurement and pose key More...

void	add (std::vector< Point2 > measurements, std::vector< Key > poseKeys, std::vector< SharedNoiseModel > noises, std::vector< boost::shared_ptr< CALIBRATION > > Ks)
	Variant of the previous one in which we include a set of measurements. More...

void	add (std::vector< Point2 > measurements, std::vector< Key > poseKeys, const SharedNoiseModel noise, const boost::shared_ptr< CALIBRATION > K)
	Variant of the previous one in which we include a set of measurements with the same noise and calibration. More...

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

virtual size_t	dim () const
	get the dimension of the factor

Base::Cameras	cameras (const Values &values) const
	Collect all cameras involved in this factor. More...

virtual boost::shared_ptr < GaussianFactor >	linearize (const Values &values) const
	Linearize to Gaussian Factor. More...

virtual double	error (const Values &values) const
	error calculates the error of the factor.

const std::vector < boost::shared_ptr < CALIBRATION > >	calibration () const
	return the calibration object

Public Member Functions inherited from gtsam::SmartProjectionFactor< POSE, LANDMARK, CALIBRATION, 6 >
	SmartProjectionFactor (const double rankTol, const double linThreshold, const bool manageDegeneracy, const bool enableEPI, boost::optional< POSE > body_P_sensor=boost::none, double landmarkDistanceThreshold=1e10, double dynamicOutlierRejectionThreshold=-1, SmartFactorStatePtr state=SmartFactorStatePtr(new SmartProjectionFactorState()))
	Constructor. More...

virtual	~SmartProjectionFactor ()
	Virtual destructor.

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

bool	decideIfTriangulate (const Cameras &cameras) const
	Check if the new linearization point_ is the same as the one used for previous triangulation.

bool	decideIfLinearize (const Cameras &cameras) const
	This function checks if the new linearization point_ is 'close' to the previous one used for linearization.

size_t	triangulateSafe (const Values &values) const
	triangulateSafe

size_t	triangulateSafe (const Cameras &cameras) const
	triangulateSafe

bool	triangulateForLinearize (const Cameras &cameras) const
	triangulate

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

boost::shared_ptr < ImplicitSchurFactor< D > >	createImplicitSchurFactor (const Cameras &cameras, double lambda) const

boost::shared_ptr < JacobianFactorQ< D > >	createJacobianQFactor (const Cameras &cameras, double lambda) const
	create factor

boost::shared_ptr < JacobianFactorQ< D > >	createJacobianQFactor (const Values &values, double lambda) const
	Create a factor, takes values.

boost::shared_ptr< JacobianFactor >	createJacobianSVDFactor (const Cameras &cameras, double lambda) const
	different (faster) way to compute Jacobian factor

bool	computeCamerasAndTriangulate (const Values &values, Cameras &myCameras) const
	Returns true if nonDegenerate.

bool	computeEP (Matrix &E, Matrix &PointCov, const Values &values) const
	Takes values.

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

bool	computeJacobians (std::vector< typename Base::KeyMatrix2D > &Fblocks, Matrix &E, Matrix &PointCov, Vector &b, const Values &values) const
	Version that takes values, and creates the point.

double	computeJacobians (std::vector< typename Base::KeyMatrix2D > &Fblocks, Matrix &E, Vector &b, const Cameras &cameras) const
	Compute F, E only (called below in both vanilla and SVD versions) Assumes the point has been computed Note E can be 2m3 or 2m2, in case point is degenerate.

double	computeJacobians (std::vector< typename Base::KeyMatrix2D > &Fblocks, Matrix &E, Matrix &PointCov, Vector &b, const Cameras &cameras, const double lambda=0.0) const
	Version that computes PointCov, with optional lambda parameter.

double	computeJacobians (Matrix &F, Matrix &E, Matrix3 &PointCov, Vector &b, const Cameras &cameras, const double lambda) const
	Returns Matrix, TODO: maybe should not exist -> not sparse !

bool	computeJacobiansSVD (std::vector< typename Base::KeyMatrix2D > &Fblocks, Matrix &Enull, Vector &b, const Values &values) const
	takes values

double	computeJacobiansSVD (std::vector< typename Base::KeyMatrix2D > &Fblocks, Matrix &Enull, Vector &b, const Cameras &cameras) const
	SVD version.

double	computeJacobiansSVD (Matrix &F, Matrix &Enull, Vector &b, const Cameras &cameras) const
	Returns Matrix, TODO: maybe should not exist -> not sparse !

Vector	reprojectionError (const Cameras &cameras) const
	Calculate vector of re-projection errors, before applying noise model Assumes triangulation was done and degeneracy handled.

Vector	reprojectionError (const Values &values) const
	Calculate vector of re-projection errors, before applying noise model.

double	totalReprojectionError (const Cameras &cameras, boost::optional< Point3 > externalPoint=boost::none) const
	Calculate the error of the factor. More...

boost::optional< Point3 >	point () const
	return the landmark

boost::optional< Point3 >	point (const Values &values) const
	COMPUTE the landmark.

bool	isDegenerate () const
	return the degenerate state

bool	isPointBehindCamera () const
	return the cheirality status flag

bool	verboseCheirality () const
	return chirality verbosity

bool	throwCheirality () const
	return flag for throwing cheirality exceptions

Public Member Functions inherited from gtsam::SmartFactorBase< POSE, CALIBRATION, D >
	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

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 bool	active (const Values &c) const
	Checks whether a factor should be used based on a set of values. More...

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 SmartProjectionFactor < POSE, LANDMARK, CALIBRATION, 6 >	Base
	shorthand for base class type

typedef SmartProjectionPoseFactor < POSE, LANDMARK, CALIBRATION >	This
	shorthand for this class

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

Public Types inherited from gtsam::SmartProjectionFactor< POSE, LANDMARK, CALIBRATION, 6 >
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::SmartFactorBase< POSE, CALIBRATION, D >
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 Attributes
LinearizationMode	linearizeTo_
	How to linearize the factor (HESSIAN, JACOBIAN_SVD, JACOBIAN_Q)

std::vector< boost::shared_ptr < CALIBRATION > >	K_all_
	shared pointer to calibration object (one for each camera)

Protected Attributes inherited from gtsam::SmartProjectionFactor< POSE, LANDMARK, CALIBRATION, 6 >
const double	rankTolerance_
	threshold to decide whether triangulation is degenerate_

const double	retriangulationThreshold_
	threshold to decide whether to re-triangulate

std::vector< Pose3 >	cameraPosesTriangulation_
	current triangulation poses

const bool	manageDegeneracy_
	if set to true will use the rotation-only version for degenerate cases

const bool	enableEPI_
	if set to true, will refine triangulation using LM

const double	linearizationThreshold_
	threshold to decide whether to re-linearize

std::vector< Pose3 >	cameraPosesLinearization_
	current linearization poses

Point3	point_
	Current estimate of the 3D point.

bool	degenerate_

bool	cheiralityException_

const bool	throwCheirality_
	If true, rethrows Cheirality exceptions (default: false)

const bool	verboseCheirality_
	If true, prints text for Cheirality exceptions (default: false)

boost::shared_ptr < SmartProjectionFactorState >	state_

double	landmarkDistanceThreshold_

double	dynamicOutlierRejectionThreshold_

Protected Attributes inherited from gtsam::SmartFactorBase< POSE, CALIBRATION, D >
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.

Additional Inherited Members
Protected Types inherited from gtsam::SmartProjectionFactor< POSE, LANDMARK, CALIBRATION, 6 >
typedef boost::shared_ptr < SmartProjectionFactorState >	SmartFactorStatePtr
	shorthand for smart projection factor state variable

typedef SmartFactorBase< POSE, CALIBRATION, D >	Base
	shorthand for base class type

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

Protected Types inherited from gtsam::SmartFactorBase< POSE, CALIBRATION, D >
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 Member Functions inherited from gtsam::Factor
	Factor ()
	Default constructor for I/O.

template<typename CONTAINER >
	Factor (const CONTAINER &keys)
	Construct factor from container of keys. More...

template<typename ITERATOR >
	Factor (ITERATOR first, ITERATOR last)
	Construct factor from iterator keys. More...

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

Static Protected Member Functions inherited from gtsam::Factor
template<typename CONTAINER >
static Factor	FromKeys (const CONTAINER &keys)
	Construct factor from container of keys. More...

template<typename ITERATOR >
static Factor	FromIterators (ITERATOR first, ITERATOR last)
	Construct factor from iterator keys. More...

Constructor & Destructor Documentation

template<class POSE , class LANDMARK , class CALIBRATION >

gtsam::SmartProjectionPoseFactor< POSE, LANDMARK, CALIBRATION >::SmartProjectionPoseFactor	(	const double	rankTol = `1`,
		const double	linThreshold = `-1`,
		const bool	manageDegeneracy = `false`,
		const bool	enableEPI = `false`,
		boost::optional< POSE >	body_P_sensor = `boost::none`,
		LinearizationMode	linearizeTo = `HESSIAN`,
		double	landmarkDistanceThreshold = `1e10`,
		double	dynamicOutlierRejectionThreshold = `-1`
	)

inline

Constructor.

Parameters

rankTol	tolerance used to check if point triangulation is degenerate
linThreshold	threshold on relative pose changes used to decide whether to relinearize (selective relinearization)
manageDegeneracy	is true, in presence of degenerate triangulation, the factor is converted to a rotation-only constraint, otherwise the factor is simply neglected
enableEPI	if set to true linear triangulation is refined with embedded LM iterations
body_P_sensor	is the transform from body to sensor frame (default identity)

Member Function Documentation

template<class POSE , class LANDMARK , class CALIBRATION >

void gtsam::SmartProjectionPoseFactor< POSE, LANDMARK, CALIBRATION >::add	(	const Point2	measured_i,
		const Key	poseKey_i,
		const SharedNoiseModel	noise_i,
		const boost::shared_ptr< CALIBRATION >	K_i
	)

inline

add a new measurement and pose key

Parameters

measured	is the 2m dimensional location of the projection of a single landmark in the m view (the measurement)
poseKey	is key corresponding to the camera observing the same landmark
noise_i	is the measurement noise
K_i	is the (known) camera calibration

template<class POSE , class LANDMARK , class CALIBRATION >

void gtsam::SmartProjectionPoseFactor< POSE, LANDMARK, CALIBRATION >::add	(	std::vector< Point2 >	measurements,
		std::vector< Key >	poseKeys,
		std::vector< SharedNoiseModel >	noises,
		std::vector< boost::shared_ptr< CALIBRATION > >	Ks
	)

inline

Variant of the previous one in which we include a set of measurements.

Parameters

measurements	vector of the 2m dimensional location of the projection of a single landmark in the m view (the measurement)
poseKeys	vector of keys corresponding to the camera observing the same landmark
noises	vector of measurement noises
Ks	vector of calibration objects

template<class POSE , class LANDMARK , class CALIBRATION >

void gtsam::SmartProjectionPoseFactor< POSE, LANDMARK, CALIBRATION >::add	(	std::vector< Point2 >	measurements,
		std::vector< Key >	poseKeys,
		const SharedNoiseModel	noise,
		const boost::shared_ptr< CALIBRATION >	K
	)

inline

Variant of the previous one in which we include a set of measurements with the same noise and calibration.

Parameters

mmeasurements	vector of the 2m dimensional location of the projection of a single landmark in the m view (the measurement)
poseKeys	vector of keys corresponding to the camera observing the same landmark
noise	measurement noise (same for all measurements)
K	the (known) camera calibration (same for all measurements)

template<class POSE , class LANDMARK , class CALIBRATION >

Base::Cameras gtsam::SmartProjectionPoseFactor< POSE, LANDMARK, CALIBRATION >::cameras ( const Values & values ) const

inlinevirtual

Collect all cameras involved in this factor.

Parameters

values Values structure which must contain camera poses corresponding to keys involved in this factor

Returns: vector of Values

Implements gtsam::SmartProjectionFactor< POSE, LANDMARK, CALIBRATION, 6 >.

template<class POSE , class LANDMARK , class CALIBRATION >

virtual boost::shared_ptr<GaussianFactor> gtsam::SmartProjectionPoseFactor< POSE, LANDMARK, CALIBRATION >::linearize ( const Values & values ) const

inlinevirtual

Linearize to Gaussian Factor.

Parameters

values Values structure which must contain camera poses for this factor

Returns

Implements gtsam::NonlinearFactor.

template<class POSE , class LANDMARK , class CALIBRATION >

void gtsam::SmartProjectionPoseFactor< POSE, LANDMARK, CALIBRATION >::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::SmartFactorBase< POSE, CALIBRATION, D >.

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

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

Public Member Functions

Public Types

Protected Attributes

Friends

Additional Inherited Members

Constructor & Destructor Documentation

Member Function Documentation