Loading...
Searching...
No Matches
gtsam::SmartFactorBase< CAMERA > Class Template Reference

## Detailed Description

template<class CAMERA>
class gtsam::SmartFactorBase< CAMERA >

Base class for smart factors.

This base class has no internal point, but it has a measurement, noise model and an optional sensor pose. This class mainly computes the derivatives and returns them as a variety of factors. The methods take a CameraSet<CAMERA> argument and the value of a point, which is kept in the derived class.

Template Parameters

  CAMERA | should behave like a PinholeCamera.

Inheritance diagram for gtsam::SmartFactorBase< CAMERA >:

## Public Member Functions

| SmartFactorBase ()
| Default Constructor, for serialization.
| SmartFactorBase (const SharedNoiseModel &sharedNoiseModel, boost::optional< Pose3 > body_P_sensor=boost::none, size_t expectedNumberCameras=10)
| Construct with given noise model and optional arguments.
| ~SmartFactorBase () override
| Virtual destructor, subclasses from NonlinearFactor.
void | add (const Z &measured, const Key &key)
| Add a new measurement and pose/camera key.
void | add (const ZVector &measurements, const KeyVector &cameraKeys)
| Add a bunch of measurements, together with the camera keys.
template<class SFM_TRACK>
void | add (const SFM_TRACK &trackToAdd)
| Add an entire SfM_track (collection of cameras observing a single point).
size_t | dim () const override
| Return the dimension (number of rows!) of the factor.
const ZVector & | measured () const
| Return the 2D measurements (ZDim, in general).
virtual Cameras | cameras (const Values &values) const
| Collect all cameras: important that in key order.
void | print (const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
| print
bool | equals (const NonlinearFactor &p, double tol=1e-9) const override
| equals
template<class POINT>
Vector | unwhitenedError (const Cameras &cameras, const POINT &point, boost::optional< typename Cameras::FBlocks & > Fs=boost::none, boost::optional< Matrix & > E=boost::none) const
| Compute reprojection errors [h(x)-z] = [cameras.project(p)-z] and derivatives.
virtual void | correctForMissingMeasurements (const Cameras &cameras, Vector &ue, boost::optional< typename Cameras::FBlocks & > Fs=boost::none, boost::optional< Matrix & > E=boost::none) const
| This corrects the Jacobians for the case in which some 2D measurement is missing (nan).
template<class POINT>
Vector | whitenedError (const Cameras &cameras, const POINT &point) const
| Calculate vector of re-projection errors [h(x)-z] = [cameras.project(p) - z], with the noise model applied.
template<class POINT>
double | totalReprojectionError (const Cameras &cameras, const POINT &point) const
| Calculate the error of the factor.
template<class POINT>
void | computeJacobians (FBlocks &Fs, Matrix &E, Vector &b, const Cameras &cameras, const POINT &point) const
| Compute F, E, and b (called below in both vanilla and SVD versions), where F is a vector of derivatives wrpt the cameras, and E the stacked derivatives with respect to the point.
template<class POINT>
void | computeJacobiansSVD (FBlocks &Fs, Matrix &Enull, Vector &b, const Cameras &cameras, const POINT &point) const
| SVD version that produces smaller Jacobian matrices by doing an SVD decomposition on E, and returning the left nulkl-space of E.
boost::shared_ptr< RegularHessianFactor< Dim > > | createHessianFactor (const Cameras &cameras, const Point3 &point, const double lambda=0.0, bool diagonalDamping=false) const
| Linearize to a Hessianfactor.
void | updateAugmentedHessian (const Cameras &cameras, const Point3 &point, const double lambda, bool diagonalDamping, SymmetricBlockMatrix &augmentedHessian, const KeyVector allKeys) const
| Add the contribution of the smart factor to a pre-allocated Hessian, using sparse linear algebra.
void | whitenJacobians (FBlocks &F, Matrix &E, Vector &b) const
| Whiten the Jacobians computed by computeJacobians using noiseModel_.
boost::shared_ptr< RegularImplicitSchurFactor< CAMERA > > | createRegularImplicitSchurFactor (const Cameras &cameras, const Point3 &point, double lambda=0.0, bool diagonalDamping=false) const
| Return Jacobians as RegularImplicitSchurFactor with raw access.
boost::shared_ptr< JacobianFactorQ< Dim, ZDim > > | createJacobianQFactor (const Cameras &cameras, const Point3 &point, double lambda=0.0, bool diagonalDamping=false) const
| Return Jacobians as JacobianFactorQ.
boost::shared_ptr< JacobianFactor > | createJacobianSVDFactor (const Cameras &cameras, const Point3 &point, double lambda=0.0) const
| Return Jacobians as JacobianFactorSVD.
Pose3 | body_P_sensor () const
| 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
double | error (const HybridValues &c) const override
| All factor types need to implement an error function.
virtual bool | active (const Values &) const
| Checks whether a factor should be used based on a set of values.
virtual boost::shared_ptr< GaussianFactor > | linearize (const Values &c) const =0
| linearize to a GaussianFactor
virtual shared_ptr | clone () const
| Creates a shared_ptr clone of the factor - needs to be specialized to allow for subclasses.
virtual 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.
virtual shared_ptr | rekey (const KeyVector &new_keys) const
| Clones a factor and fully replaces its keys.
virtual bool | sendable () const
| Should the factor be evaluated in the same thread as the caller This is to enable factors that has shared states (like the Python GIL lock).
Public Member Functions inherited from gtsam::Factor
virtual | ~Factor ()=default
| Default destructor.
bool | empty () const
| Whether the factor is empty (involves zero variables).
Key | front () const
| First key.
Key | back () const
| Last key.
const_iterator | find (Key key) const
| find
const KeyVector & | 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
virtual void | printKeys (const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
| print only keys
bool | equals (const This &other, double tol=1e-9) const
| check equality
KeyVector & | keys ()
iterator | begin ()
| Iterator at beginning of involved variable keys.
iterator | end ()
| Iterator at end of involved variable keys.

## Static Public Member Functions

static Matrix | PointCov (const Matrix &E)
| Computes Point Covariance P from the "point Jacobian" E.
static void | FillDiagonalF (const FBlocks &Fs, Matrix &F)
| Create BIG block-diagonal matrix F from Fblocks.

## Public Attributes

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

## Static Public Attributes

static const int | Dim = traits<CAMERA>::dimension
| Camera dimension.
static const int | ZDim = traits<Z>::dimension
| Measurement dimension.

## Public Types

typedef Eigen::Matrix< double, ZDim, Dim > | MatrixZD
typedef std::vector< MatrixZD, Eigen::aligned_allocator< MatrixZD > > | FBlocks
typedef CameraSet< CAMERA > | Cameras
| The CameraSet data structure is used to refer to a set of cameras.
Public Types inherited from gtsam::NonlinearFactor
typedef boost::shared_ptr< This > | shared_ptr
Public Types inherited from gtsam::Factor
typedef KeyVector::iterator | iterator
| Iterator over keys.
typedef KeyVector::const_iterator | const_iterator
| Const iterator over keys.

## Protected Attributes

SharedIsotropic | noiseModel_
| As of Feb 22, 2015, the noise model is the same for all measurements and is isotropic.
ZVector | measured_
| Measurements for each of the m views.
boost::optional< Pose3 > | body_P_sensor_
| Pose of the camera in the body frame.
FBlocks | Fs
Protected Attributes inherited from gtsam::Factor
KeyVector | keys_
| The keys involved in this factor.

## Friends

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

## Additional Inherited Members

Protected Types inherited from gtsam::NonlinearFactor
typedef Factor | Base
typedef NonlinearFactor | This
| Factor ()
| Default constructor for I/O.
template<typename CONTAINER>
| Factor (const CONTAINER &keys)
| Construct factor from container of keys.
template<typename ITERATOR>
| Factor (ITERATOR first, ITERATOR last)
| Construct factor from iterator keys.
template<typename CONTAINER>
static Factor | FromKeys (const CONTAINER &keys)
| Construct factor from container of keys.
template<typename ITERATOR>
static Factor | FromIterators (ITERATOR first, ITERATOR last)
| Construct factor from iterator keys.

## Member Function Documentation

## add() [1/2]

template<class CAMERA>
template<class SFM_TRACK>

void gtsam::SmartFactorBase< CAMERA >::add | ( | const SFM_TRACK & | trackToAdd | ) |

inline

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

The noise is assumed to be the same for all measurements.

## add() [2/2]

template<class CAMERA>

void gtsam::SmartFactorBase< CAMERA >::add | ( | const Z & | measured,
|  | const Key & | key )

inline

Add a new measurement and pose/camera key.

Parameters

  measured | is the 2m dimensional projection of a single landmark
  key | is the index corresponding to the camera observing the landmark

## cameras()

template<class CAMERA>

virtual Cameras gtsam::SmartFactorBase< CAMERA >::cameras | ( | const Values & | values | ) | const

inline virtual

Collect all cameras: important that in key order.

Reimplemented in gtsam::SmartProjectionPoseFactor< CALIBRATION >, and gtsam::SmartProjectionRigFactor< CAMERA >.

## computeJacobians()

template<class CAMERA>
template<class POINT>

void gtsam::SmartFactorBase< CAMERA >::computeJacobians | ( | FBlocks & | Fs,
|  | Matrix & | E,
|  | Vector & | b,
|  | const Cameras & | cameras,
|  | const POINT & | point ) const

inline

Compute F, E, and b (called below in both vanilla and SVD versions), where F is a vector of derivatives wrpt the cameras, and E the stacked derivatives with respect to the point.

The value of cameras/point are passed as parameters.

## computeJacobiansSVD()

template<class CAMERA>
template<class POINT>

void gtsam::SmartFactorBase< CAMERA >::computeJacobiansSVD | ( | FBlocks & | Fs,
|  | Matrix & | Enull,
|  | Vector & | b,
|  | const Cameras & | cameras,
|  | const POINT & | point ) const

inline

SVD version that produces smaller Jacobian matrices by doing an SVD decomposition on E, and returning the left nulkl-space of E.

See JacobianFactorSVD for more documentation.

## correctForMissingMeasurements()

template<class CAMERA>

virtual void gtsam::SmartFactorBase< CAMERA >::correctForMissingMeasurements | ( | const Cameras & | cameras,
|  | Vector & | ue,
|  | boost::optional< typename Cameras::FBlocks & > | Fs = boost::none,
|  | boost::optional< Matrix & > | E = boost::none ) const

inline virtual

This corrects the Jacobians for the case in which some 2D measurement is missing (nan).

In practice, this does not do anything in the monocular case, but it is implemented in the stereo version.

## createJacobianSVDFactor()

template<class CAMERA>

boost::shared_ptr< JacobianFactor > gtsam::SmartFactorBase< CAMERA >::createJacobianSVDFactor | ( | const Cameras & | cameras,
|  | const Point3 & | point,
|  | double | lambda = 0.0 ) const

inline

Return Jacobians as JacobianFactorSVD.

TODO(dellaert): lambda is currently ignored

## dim()

template<class CAMERA>

size_t gtsam::SmartFactorBase< CAMERA >::dim | ( |  | ) | const

inline override virtual

Return the dimension (number of rows!) of the factor.

Implements gtsam::NonlinearFactor.

## equals()

template<class CAMERA>

bool gtsam::SmartFactorBase< CAMERA >::equals | ( | const NonlinearFactor & | p,
|  | double | tol = 1e-9 ) const

inline override virtual

equals

Reimplemented from gtsam::NonlinearFactor.

Reimplemented in gtsam::SmartProjectionFactor< CAMERA >, gtsam::SmartProjectionFactor< Camera >, gtsam::SmartProjectionFactor< PinholePose< CALIBRATION > >, gtsam::SmartProjectionPoseFactor< CALIBRATION >, and gtsam::SmartProjectionRigFactor< CAMERA >.

## print()

template<class CAMERA>

void gtsam::SmartFactorBase< CAMERA >::print | ( | const std::string & | s = "",
|  | const KeyFormatter & | keyFormatter = DefaultKeyFormatter ) const

inline override virtual

print

Parameters

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

Reimplemented from gtsam::NonlinearFactor.

Reimplemented in gtsam::SmartProjectionFactor< CAMERA >, gtsam::SmartProjectionFactor< Camera >, gtsam::SmartProjectionFactor< PinholePose< CALIBRATION > >, gtsam::SmartProjectionPoseFactor< CALIBRATION >, and gtsam::SmartProjectionRigFactor< CAMERA >.

## totalReprojectionError()

template<class CAMERA>
template<class POINT>

double gtsam::SmartFactorBase< CAMERA >::totalReprojectionError | ( | const Cameras & | cameras,
|  | const POINT & | 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. Will be used in "error(Values)" function required by NonlinearFactor base class

## unwhitenedError()

template<class CAMERA>
template<class POINT>

Vector gtsam::SmartFactorBase< CAMERA >::unwhitenedError | ( | const Cameras & | cameras,
|  | const POINT & | point,
|  | boost::optional< typename Cameras::FBlocks & > | Fs = boost::none,
|  | boost::optional< Matrix & > | E = boost::none ) const

inline

Compute reprojection errors [h(x)-z] = [cameras.project(p)-z] and derivatives.

This is the error before the noise model is applied.

## updateAugmentedHessian()

template<class CAMERA>

void gtsam::SmartFactorBase< CAMERA >::updateAugmentedHessian | ( | const Cameras & | cameras,
|  | const Point3 & | point,
|  | const double | lambda,
|  | bool | diagonalDamping,
|  | SymmetricBlockMatrix & | augmentedHessian,
|  | const KeyVector | allKeys ) const

inline

Add the contribution of the smart factor to a pre-allocated Hessian, using sparse linear algebra.

More efficient than the creation of the Hessian without preallocation of the SymmetricBlockMatrix

## Member Data Documentation

## measured_

template<class CAMERA>

ZVector gtsam::SmartFactorBase< CAMERA >::measured_

protected

Measurements for each of the m views.

We keep a copy of the measurements for I/O and computing the error. The order is kept the same as the keys that we use to create the factor.

## noiseModel_

template<class CAMERA>

SharedIsotropic gtsam::SmartFactorBase< CAMERA >::noiseModel_

protected

As of Feb 22, 2015, the noise model is the same for all measurements and is isotropic.

This allows for moving most calculations of Schur complement etc. to be easily moved to CameraSet, and also agrees pragmatically with what is normally done.

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

- /build/gtsam-RkN8wG/gtsam-4.2.2+dfsg/gtsam/slam/SmartFactorBase.h