gtsam  3.2.0
gtsam
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
gtsam::SmartProjectionFactor< POSE, LANDMARK, CALIBRATION, D > Class Template Referenceabstract

Detailed Description

template<class POSE, class LANDMARK, class CALIBRATION, size_t D>
class gtsam::SmartProjectionFactor< POSE, LANDMARK, CALIBRATION, D >

SmartProjectionFactor: triangulates point TODO: why LANDMARK parameter?

+ Inheritance diagram for gtsam::SmartProjectionFactor< POSE, LANDMARK, CALIBRATION, D >:

Public Member Functions

 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.
 
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 !
 
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 !
 
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...
 
virtual Cameras cameras (const Values &values) const =0
 Cameras are computed in derived class.
 
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
 
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< 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 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< 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 Types

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 Attributes

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.
 

Constructor & Destructor Documentation

template<class POSE, class LANDMARK, class CALIBRATION, size_t D>
gtsam::SmartProjectionFactor< POSE, LANDMARK, CALIBRATION, D >::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()) 
)
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, size_t D>
void gtsam::SmartProjectionFactor< POSE, LANDMARK, CALIBRATION, D >::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 >.

template<class POSE, class LANDMARK, class CALIBRATION, size_t D>
double gtsam::SmartProjectionFactor< POSE, LANDMARK, CALIBRATION, D >::totalReprojectionError ( const Cameras &  cameras,
boost::optional< Point3 externalPoint = boost::none 
) 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.


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