gtsam  3.2.0
gtsam
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
gtsam::SmartProjectionPoseFactor< POSE, LANDMARK, CALIBRATION > Class Template Reference
+ 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< JacobianFactorcreateJacobianSVDFactor (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 2m*3 or 2m*2, 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< Point3point () const
 return the landmark
 
boost::optional< Point3point (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< JacobianFactorcreateJacobianSVDFactor (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< Thisshared_ptr
 shorthand for a smart pointer to a factor
 
- Public Types inherited from gtsam::SmartProjectionFactor< POSE, LANDMARK, CALIBRATION, 6 >
typedef boost::shared_ptr< Thisshared_ptr
 shorthand for a smart pointer to a factor
 
typedef PinholeCamera
< CALIBRATION > 
Camera
 shorthand for a pinhole camera
 
typedef std::vector< CameraCameras
 
- Public Types inherited from gtsam::SmartFactorBase< POSE, CALIBRATION, D >
typedef boost::shared_ptr< Thisshared_ptr
 shorthand for a smart pointer to a factor
 
typedef PinholeCamera
< CALIBRATION > 
Camera
 shorthand for a pinhole camera
 
typedef std::vector< CameraCameras
 
- Public Types inherited from gtsam::NonlinearFactor
typedef boost::shared_ptr< Thisshared_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< Pose3cameraPosesTriangulation_
 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< Pose3cameraPosesLinearization_
 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< Point2measured_
 2D measurement for each of the m views
 
std::vector< SharedNoiseModelnoise_
 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< Keykeys_
 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, Matrix2DKeyMatrix2D
 
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
rankToltolerance used to check if point triangulation is degenerate
linThresholdthreshold on relative pose changes used to decide whether to relinearize (selective relinearization)
manageDegeneracyis true, in presence of degenerate triangulation, the factor is converted to a rotation-only constraint, otherwise the factor is simply neglected
enableEPIif set to true linear triangulation is refined with embedded LM iterations
body_P_sensoris 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
measuredis the 2m dimensional location of the projection of a single landmark in the m view (the measurement)
poseKeyis key corresponding to the camera observing the same landmark
noise_iis the measurement noise
K_iis 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
measurementsvector of the 2m dimensional location of the projection of a single landmark in the m view (the measurement)
poseKeysvector of keys corresponding to the camera observing the same landmark
noisesvector of measurement noises
Ksvector 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
mmeasurementsvector of the 2m dimensional location of the projection of a single landmark in the m view (the measurement)
poseKeysvector of keys corresponding to the camera observing the same landmark
noisemeasurement noise (same for all measurements)
Kthe (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
valuesValues 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
valuesValues 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
soptional string naming the factor
keyFormatteroptional formatter useful for printing Symbols

Reimplemented from gtsam::SmartFactorBase< POSE, CALIBRATION, D >.


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