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

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

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 CALIBRATION , size_t D>
gtsam::SmartFactorBase< POSE, CALIBRATION, D >::SmartFactorBase ( boost::optional< POSE >  body_P_sensor = boost::none)
inline

Constructor.

Parameters
body_P_sensoris 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_iis the 2m dimensional projection of a single landmark
poseKeyis the index corresponding to the camera observing the landmark
noise_iis 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
soptional string naming the factor
keyFormatteroptional 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: