Public Types | Public Member Functions | Static Public Member Functions | Public Attributes | Static Public Attributes | Protected Attributes | Private Types | List of all members
gtsam::SmartFactorBase< CAMERA > Class Template Reference

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. More...

#include <SmartFactorBase.h>

Inheritance diagram for gtsam::SmartFactorBase< CAMERA >:
Inheritance graph
[legend]

Public Types

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

Public Member Functions

void add (const Z &measured, const Key &key)
 
void add (const ZVector &measurements, const KeyVector &cameraKeys)
 Add a bunch of measurements, together with the camera keys. More...
 
template<class SFM_TRACK >
void add (const SFM_TRACK &trackToAdd)
 
Pose3 body_P_sensor () const
 
virtual Cameras cameras (const Values &values) const
 Collect all cameras: important that in key order. More...
 
template<class POINT >
void computeJacobians (FBlocks &Fs, Matrix &E, Vector &b, const Cameras &cameras, const POINT &point) const
 
template<class POINT >
void computeJacobiansSVD (FBlocks &Fs, Matrix &Enull, Vector &b, const Cameras &cameras, const POINT &point) const
 
virtual void correctForMissingMeasurements (const Cameras &cameras, Vector &ue, typename Cameras::FBlocks *Fs=nullptr, Matrix *E=nullptr) const
 
template<class ... OptArgs>
void correctForMissingMeasurements (const Cameras &cameras, Vector &ue, OptArgs &&... optArgs) const
 
std::shared_ptr< RegularHessianFactor< Dim > > createHessianFactor (const Cameras &cameras, const Point3 &point, const double lambda=0.0, bool diagonalDamping=false) const
 Linearize to a Hessianfactor. More...
 
std::shared_ptr< JacobianFactorQ< Dim, ZDim > > createJacobianQFactor (const Cameras &cameras, const Point3 &point, double lambda=0.0, bool diagonalDamping=false) const
 Return Jacobians as JacobianFactorQ. More...
 
std::shared_ptr< JacobianFactorcreateJacobianSVDFactor (const Cameras &cameras, const Point3 &point, double lambda=0.0) const
 
std::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. More...
 
size_t dim () const override
 Return the dimension (number of rows!) of the factor. More...
 
bool equals (const NonlinearFactor &p, double tol=1e-9) const override
 equals More...
 
const ZVectormeasured () const
 Return the 2D measurements (ZDim, in general). More...
 
void print (const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
 
 SmartFactorBase ()
 Default Constructor, for serialization. More...
 
 SmartFactorBase (const SharedNoiseModel &sharedNoiseModel, std::optional< Pose3 > body_P_sensor={}, size_t expectedNumberCameras=10)
 Construct with given noise model and optional arguments. More...
 
template<class POINT >
double totalReprojectionError (const Cameras &cameras, const POINT &point) const
 
template<class POINT >
Vector unwhitenedError (const Cameras &cameras, const POINT &point, typename Cameras::FBlocks *Fs=nullptr, Matrix *E=nullptr) const
 
template<class POINT , class ... OptArgs, typename = std::enable_if_t<sizeof...(OptArgs)!=0>>
Vector unwhitenedError (const Cameras &cameras, const POINT &point, OptArgs &&... optArgs) const
 
void updateAugmentedHessian (const Cameras &cameras, const Point3 &point, const double lambda, bool diagonalDamping, SymmetricBlockMatrix &augmentedHessian, const KeyVector allKeys) const
 
template<class POINT >
Vector whitenedError (const Cameras &cameras, const POINT &point) const
 
void whitenJacobians (FBlocks &F, Matrix &E, Vector &b) const
 Whiten the Jacobians computed by computeJacobians using noiseModel_. More...
 
 ~SmartFactorBase () override
 Virtual destructor, subclasses from NonlinearFactor. More...
 
- Public Member Functions inherited from gtsam::NonlinearFactor
 NonlinearFactor ()
 
template<typename CONTAINER >
 NonlinearFactor (const CONTAINER &keys)
 
virtual double error (const Values &c) const
 
double error (const HybridValues &c) const override
 
virtual bool active (const Values &) const
 
virtual std::shared_ptr< GaussianFactorlinearize (const Values &c) const =0
 
virtual shared_ptr clone () const
 
virtual shared_ptr rekey (const std::map< Key, Key > &rekey_mapping) const
 
virtual shared_ptr rekey (const KeyVector &new_keys) const
 
virtual bool sendable () const
 
- Public Member Functions inherited from gtsam::Factor
virtual ~Factor ()=default
 Default destructor. More...
 
bool empty () const
 Whether the factor is empty (involves zero variables). More...
 
Key front () const
 First key. More...
 
Key back () const
 Last key. More...
 
const_iterator find (Key key) const
 find More...
 
const KeyVectorkeys () const
 Access the factor's involved variable keys. More...
 
const_iterator begin () const
 
const_iterator end () const
 
size_t size () const
 
virtual void printKeys (const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
 print only keys More...
 
bool equals (const This &other, double tol=1e-9) const
 check equality More...
 
KeyVectorkeys ()
 
iterator begin ()
 
iterator end ()
 

Static Public Member Functions

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

Public Attributes

GTSAM_MAKE_ALIGNED_OPERATOR_NEW typedef std::shared_ptr< Thisshared_ptr
 shorthand for a smart pointer to a factor. More...
 

Static Public Attributes

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

Protected Attributes

std::optional< Pose3body_P_sensor_
 Pose of the camera in the body frame. More...
 
FBlocks Fs
 
ZVector measured_
 
SharedIsotropic noiseModel_
 
- Protected Attributes inherited from gtsam::Factor
KeyVector keys_
 The keys involved in this factor. More...
 

Private Types

typedef NonlinearFactor Base
 
typedef SmartFactorBase< CAMERA > This
 
typedef CAMERA::Measurement Z
 
typedef CAMERA::MeasurementVector ZVector
 

Additional Inherited Members

- Protected Types inherited from gtsam::NonlinearFactor
typedef Factor Base
 
typedef NonlinearFactor This
 
- Protected Member Functions inherited from gtsam::Factor
 Factor ()
 
template<typename CONTAINER >
 Factor (const CONTAINER &keys)
 
template<typename ITERATOR >
 Factor (ITERATOR first, ITERATOR last)
 
- Static Protected Member Functions inherited from gtsam::Factor
template<typename CONTAINER >
static Factor FromKeys (const CONTAINER &keys)
 
template<typename ITERATOR >
static Factor FromIterators (ITERATOR first, ITERATOR last)
 

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
CAMERAshould behave like a PinholeCamera.

Definition at line 51 of file SmartFactorBase.h.

Member Typedef Documentation

◆ Base

template<class CAMERA>
typedef NonlinearFactor gtsam::SmartFactorBase< CAMERA >::Base
private

Definition at line 54 of file SmartFactorBase.h.

◆ Cameras

template<class CAMERA>
typedef CameraSet<CAMERA> gtsam::SmartFactorBase< CAMERA >::Cameras

The CameraSet data structure is used to refer to a set of cameras.

Definition at line 95 of file SmartFactorBase.h.

◆ FBlocks

template<class CAMERA>
typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> > gtsam::SmartFactorBase< CAMERA >::FBlocks

Definition at line 64 of file SmartFactorBase.h.

◆ MatrixZD

template<class CAMERA>
typedef Eigen::Matrix<double, ZDim, Dim> gtsam::SmartFactorBase< CAMERA >::MatrixZD

Definition at line 63 of file SmartFactorBase.h.

◆ This

template<class CAMERA>
typedef SmartFactorBase<CAMERA> gtsam::SmartFactorBase< CAMERA >::This
private

Definition at line 55 of file SmartFactorBase.h.

◆ Z

template<class CAMERA>
typedef CAMERA::Measurement gtsam::SmartFactorBase< CAMERA >::Z
private

Definition at line 56 of file SmartFactorBase.h.

◆ ZVector

template<class CAMERA>
typedef CAMERA::MeasurementVector gtsam::SmartFactorBase< CAMERA >::ZVector
private

Definition at line 57 of file SmartFactorBase.h.

Constructor & Destructor Documentation

◆ SmartFactorBase() [1/2]

template<class CAMERA>
gtsam::SmartFactorBase< CAMERA >::SmartFactorBase ( )
inline

Default Constructor, for serialization.

Definition at line 98 of file SmartFactorBase.h.

◆ SmartFactorBase() [2/2]

template<class CAMERA>
gtsam::SmartFactorBase< CAMERA >::SmartFactorBase ( const SharedNoiseModel sharedNoiseModel,
std::optional< Pose3 body_P_sensor = {},
size_t  expectedNumberCameras = 10 
)
inline

Construct with given noise model and optional arguments.

Definition at line 101 of file SmartFactorBase.h.

◆ ~SmartFactorBase()

template<class CAMERA>
gtsam::SmartFactorBase< CAMERA >::~SmartFactorBase ( )
inlineoverride

Virtual destructor, subclasses from NonlinearFactor.

Definition at line 119 of file SmartFactorBase.h.

Member Function Documentation

◆ add() [1/3]

template<class CAMERA>
void gtsam::SmartFactorBase< CAMERA >::add ( const Z measured,
const Key key 
)
inline

Add a new measurement and pose/camera key.

Parameters
measuredis the 2m dimensional projection of a single landmark
keyis the index corresponding to the camera observing the landmark

Definition at line 127 of file SmartFactorBase.h.

◆ add() [2/3]

template<class CAMERA>
void gtsam::SmartFactorBase< CAMERA >::add ( const ZVector measurements,
const KeyVector cameraKeys 
)
inline

Add a bunch of measurements, together with the camera keys.

Definition at line 137 of file SmartFactorBase.h.

◆ add() [3/3]

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.

Definition at line 149 of file SmartFactorBase.h.

◆ body_P_sensor()

template<class CAMERA>
Pose3 gtsam::SmartFactorBase< CAMERA >::body_P_sensor ( ) const
inline

Definition at line 439 of file SmartFactorBase.h.

◆ cameras()

template<class CAMERA>
virtual Cameras gtsam::SmartFactorBase< CAMERA >::cameras ( const Values values) const
inlinevirtual

◆ 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.

Definition at line 315 of file SmartFactorBase.h.

◆ 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.

Definition at line 330 of file SmartFactorBase.h.

◆ correctForMissingMeasurements() [1/2]

template<class CAMERA>
virtual void gtsam::SmartFactorBase< CAMERA >::correctForMissingMeasurements ( const Cameras cameras,
Vector ue,
typename Cameras::FBlocks Fs = nullptr,
Matrix E = nullptr 
) const
inlinevirtual

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.

Reimplemented in gtsam::SmartStereoProjectionFactor.

Definition at line 258 of file SmartFactorBase.h.

◆ correctForMissingMeasurements() [2/2]

template<class CAMERA>
template<class ... OptArgs>
void gtsam::SmartFactorBase< CAMERA >::correctForMissingMeasurements ( const Cameras cameras,
Vector ue,
OptArgs &&...  optArgs 
) const
inline

An overload of correctForMissingMeasurements. This allows end users to provide optional arguments that are l-value references to the matrices and vectors that will be used to store the results instead of pointers.

Definition at line 270 of file SmartFactorBase.h.

◆ createHessianFactor()

template<class CAMERA>
std::shared_ptr<RegularHessianFactor<Dim> > gtsam::SmartFactorBase< CAMERA >::createHessianFactor ( const Cameras cameras,
const Point3 point,
const double  lambda = 0.0,
bool  diagonalDamping = false 
) const
inline

Linearize to a Hessianfactor.

Definition at line 346 of file SmartFactorBase.h.

◆ createJacobianQFactor()

template<class CAMERA>
std::shared_ptr<JacobianFactorQ<Dim, ZDim> > gtsam::SmartFactorBase< CAMERA >::createJacobianQFactor ( const Cameras cameras,
const Point3 point,
double  lambda = 0.0,
bool  diagonalDamping = false 
) const
inline

Return Jacobians as JacobianFactorQ.

Definition at line 399 of file SmartFactorBase.h.

◆ createJacobianSVDFactor()

template<class CAMERA>
std::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

Definition at line 416 of file SmartFactorBase.h.

◆ createRegularImplicitSchurFactor()

template<class CAMERA>
std::shared_ptr<RegularImplicitSchurFactor<CAMERA> > gtsam::SmartFactorBase< CAMERA >::createRegularImplicitSchurFactor ( const Cameras cameras,
const Point3 point,
double  lambda = 0.0,
bool  diagonalDamping = false 
) const
inline

Return Jacobians as RegularImplicitSchurFactor with raw access.

Definition at line 386 of file SmartFactorBase.h.

◆ dim()

template<class CAMERA>
size_t gtsam::SmartFactorBase< CAMERA >::dim ( ) const
inlineoverridevirtual

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

Implements gtsam::NonlinearFactor.

Definition at line 157 of file SmartFactorBase.h.

◆ equals()

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

◆ FillDiagonalF()

template<class CAMERA>
static void gtsam::SmartFactorBase< CAMERA >::FillDiagonalF ( const FBlocks Fs,
Matrix F 
)
inlinestatic

Create BIG block-diagonal matrix F from Fblocks.

Definition at line 430 of file SmartFactorBase.h.

◆ measured()

template<class CAMERA>
const ZVector& gtsam::SmartFactorBase< CAMERA >::measured ( ) const
inline

Return the 2D measurements (ZDim, in general).

Definition at line 160 of file SmartFactorBase.h.

◆ PointCov()

template<class CAMERA>
static Matrix gtsam::SmartFactorBase< CAMERA >::PointCov ( const Matrix E)
inlinestatic

Computes Point Covariance P from the "point Jacobian" E.

Definition at line 304 of file SmartFactorBase.h.

◆ print()

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

print

Parameters
soptional string naming the factor
keyFormatteroptional formatter useful for printing Symbols

Reimplemented from gtsam::NonlinearFactor.

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

Definition at line 175 of file SmartFactorBase.h.

◆ 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

Definition at line 297 of file SmartFactorBase.h.

◆ unwhitenedError() [1/2]

template<class CAMERA>
template<class POINT >
Vector gtsam::SmartFactorBase< CAMERA >::unwhitenedError ( const Cameras cameras,
const POINT &  point,
typename Cameras::FBlocks Fs = nullptr,
Matrix E = nullptr 
) const
inline

Compute reprojection errors [h(x)-z] = [cameras.project(p)-z] and derivatives. This is the error before the noise model is applied. The templated version described above must finally get resolved to this function.

Definition at line 208 of file SmartFactorBase.h.

◆ unwhitenedError() [2/2]

template<class CAMERA>
template<class POINT , class ... OptArgs, typename = std::enable_if_t<sizeof...(OptArgs)!=0>>
Vector gtsam::SmartFactorBase< CAMERA >::unwhitenedError ( const Cameras cameras,
const POINT &  point,
OptArgs &&...  optArgs 
) const
inline

An overload of unwhitenedError. This allows end users to provide optional arguments that are l-value references to the matrices and vectors that will be used to store the results instead of pointers.

Definition at line 247 of file SmartFactorBase.h.

◆ 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

Definition at line 366 of file SmartFactorBase.h.

◆ whitenedError()

template<class CAMERA>
template<class POINT >
Vector gtsam::SmartFactorBase< CAMERA >::whitenedError ( const Cameras cameras,
const POINT &  point 
) const
inline

Calculate vector of re-projection errors [h(x)-z] = [cameras.project(p) - z], with the noise model applied.

Definition at line 281 of file SmartFactorBase.h.

◆ whitenJacobians()

template<class CAMERA>
void gtsam::SmartFactorBase< CAMERA >::whitenJacobians ( FBlocks F,
Matrix E,
Vector b 
) const
inline

Whiten the Jacobians computed by computeJacobians using noiseModel_.

Definition at line 377 of file SmartFactorBase.h.

Member Data Documentation

◆ body_P_sensor_

template<class CAMERA>
std::optional<Pose3> gtsam::SmartFactorBase< CAMERA >::body_P_sensor_
protected

Pose of the camera in the body frame.

Definition at line 83 of file SmartFactorBase.h.

◆ Dim

template<class CAMERA>
const int gtsam::SmartFactorBase< CAMERA >::Dim = traits<CAMERA>::dimension
static

Camera dimension.

Definition at line 61 of file SmartFactorBase.h.

◆ Fs

template<class CAMERA>
FBlocks gtsam::SmartFactorBase< CAMERA >::Fs
mutableprotected

Definition at line 86 of file SmartFactorBase.h.

◆ 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.

Definition at line 80 of file SmartFactorBase.h.

◆ 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.

Definition at line 73 of file SmartFactorBase.h.

◆ shared_ptr

template<class CAMERA>
GTSAM_MAKE_ALIGNED_OPERATOR_NEW typedef std::shared_ptr<This> gtsam::SmartFactorBase< CAMERA >::shared_ptr

shorthand for a smart pointer to a factor.

Definition at line 92 of file SmartFactorBase.h.

◆ ZDim

template<class CAMERA>
const int gtsam::SmartFactorBase< CAMERA >::ZDim = traits<Z>::dimension
static

Measurement dimension.

Definition at line 62 of file SmartFactorBase.h.


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


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:47:09