Loading...
Searching...
No Matches
gtsam::SmartProjectionPoseFactor< CALIBRATION > Class Template Reference
SLAM

## Detailed Description

template<class CALIBRATION>
class gtsam::SmartProjectionPoseFactor< CALIBRATION >

If you are using the factor, please cite: L.

Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating conditionally independent sets in factor graphs: a unifying perspective based on smart factors, Int. Conf. on Robotics and Automation (ICRA), 2014. This factor assumes that camera calibration is fixed, and that the calibration is the same for all cameras involved in this factor. The factor only constrains poses (variable dimension is 6). This factor requires that values contains the involved poses (Pose3). If the calibration should be optimized, as well, use SmartProjectionFactor instead!

Inheritance diagram for gtsam::SmartProjectionPoseFactor< CALIBRATION >:

## Public Member Functions

| SmartProjectionPoseFactor ()
| Default constructor, only for serialization.
| SmartProjectionPoseFactor (const SharedNoiseModel &sharedNoiseModel, const boost::shared_ptr< CALIBRATION > K, const SmartProjectionParams &params=SmartProjectionParams())
| Constructor.
| SmartProjectionPoseFactor (const SharedNoiseModel &sharedNoiseModel, const boost::shared_ptr< CALIBRATION > K, const boost::optional< Pose3 > body_P_sensor, const SmartProjectionParams &params=SmartProjectionParams())
| Constructor.
| ~SmartProjectionPoseFactor () override
| Virtual destructor.
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
double | error (const Values &values) const override
| error calculates the error of the factor.
const boost::shared_ptr< CALIBRATION > | calibration () const
| return calibration shared pointers
Base::Cameras | cameras (const Values &values) const override
| Collect all cameras involved in this factor.
Public Member Functions inherited from gtsam::SmartProjectionFactor< PinholePose< CALIBRATION > >
| SmartProjectionFactor ()
| Default constructor, only for serialization.
| ~SmartProjectionFactor () override
| Virtual destructor.
bool | decideIfTriangulate (const Cameras &cameras) const
| Check if the new linearization point is the same as the one used for previous triangulation.
TriangulationResult | triangulateSafe (const Cameras &cameras) const
| Call gtsam::triangulateSafe iff we need to re-triangulate.
bool | triangulateForLinearize (const Cameras &cameras) const
| Possibly re-triangulate before calculating Jacobians.
boost::shared_ptr< RegularHessianFactor< Base::Dim > > | createHessianFactor (const Cameras &cameras, const double lambda=0.0, bool diagonalDamping=false) const
| Create a Hessianfactor that is an approximation of error(p).
boost::shared_ptr< RegularImplicitSchurFactor< PinholePose< CALIBRATION > > > | createRegularImplicitSchurFactor (const Cameras &cameras, double lambda) const
boost::shared_ptr< JacobianFactorQ< Base::Dim, 2 > > | createJacobianQFactor (const Cameras &cameras, double lambda) const
| Create JacobianFactorQ factor.
boost::shared_ptr< JacobianFactor > | createJacobianSVDFactor (const Cameras &cameras, double lambda) const
| Different (faster) way to compute a JacobianFactorSVD factor.
virtual boost::shared_ptr< RegularHessianFactor< Base::Dim > > | linearizeToHessian (const Values &values, double lambda=0.0) const
| Linearize to a Hessianfactor.
virtual boost::shared_ptr< RegularImplicitSchurFactor< PinholePose< CALIBRATION > > > | linearizeToImplicit (const Values &values, double lambda=0.0) const
| Linearize to an Implicit Schur factor.
virtual boost::shared_ptr< JacobianFactorQ< Base::Dim, 2 > > | linearizeToJacobian (const Values &values, double lambda=0.0) const
| Linearize to a JacobianfactorQ.
boost::shared_ptr< GaussianFactor > | linearizeDamped (const Cameras &cameras, const double lambda=0.0) const
| Linearize to Gaussian Factor.
boost::shared_ptr< GaussianFactor > | linearize (const Values &values) const override
| linearize
bool | triangulateAndComputeE (Matrix &E, const Cameras &cameras) const
| Triangulate and compute derivative of error with respect to point.
void | computeJacobiansWithTriangulatedPoint (typename Base::FBlocks &Fs, 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.
bool | triangulateAndComputeJacobians (typename Base::FBlocks &Fs, Matrix &E, Vector &b, const Values &values) const
| Version that takes values, and creates the point.
bool | triangulateAndComputeJacobiansSVD (typename Base::FBlocks &Fs, Matrix &Enull, Vector &b, const Values &values) const
| takes values
Vector | reprojectionErrorAfterTriangulation (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.
TriangulationResult | point () const
| return the landmark
bool | isValid () const
| Is result valid?
bool | isDegenerate () const
| return the degenerate state
bool | isPointBehindCamera () const
| return the cheirality status flag
bool | isOutlier () const
| return the outlier state
bool | isFarPoint () const
| return the farPoint state
Public Member Functions inherited from gtsam::SmartFactorBase< PinholePose< CALIBRATION > >
| SmartFactorBase ()
| Default Constructor, for serialization.
| ~SmartFactorBase () override
| Virtual destructor, subclasses from NonlinearFactor.
void | add (const Z &measured, const Key &key)
| Add a new measurement and pose/camera key.
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).
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).
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.
double | totalReprojectionError (const Cameras &cameras, const POINT &point) const
| Calculate the error of the factor.
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.
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< PinholePose< CALIBRATION > > > | 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.
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 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.

## Public Types

typedef boost::shared_ptr< This > | shared_ptr
| shorthand for a smart pointer to a factor
Public Types inherited from gtsam::SmartProjectionFactor< PinholePose< CALIBRATION > >
typedef boost::shared_ptr< This > | shared_ptr
| shorthand for a smart pointer to a factor
typedef PinholePose< CALIBRATION > | Camera
| shorthand for a set of cameras
typedef CameraSet< PinholePose< CALIBRATION > > | Cameras
Public Types inherited from gtsam::SmartFactorBase< PinholePose< CALIBRATION > >
typedef Eigen::Matrix< double, ZDim, Dim > | MatrixZD
typedef std::vector< MatrixZD, Eigen::aligned_allocator< MatrixZD > > | FBlocks
typedef CameraSet< PinholePose< CALIBRATION > > | 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

boost::shared_ptr< CALIBRATION > | K_
| calibration object (one for all cameras)
Protected Attributes inherited from gtsam::SmartProjectionFactor< PinholePose< CALIBRATION > >
SmartProjectionParams | params_
TriangulationResult | result_
| result from triangulateSafe
std::vector< Pose3, Eigen::aligned_allocator< Pose3 > > | cameraPosesTriangulation_
| current triangulation poses
Protected Attributes inherited from gtsam::SmartFactorBase< PinholePose< CALIBRATION > >
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

Static Public Member Functions inherited from gtsam::SmartFactorBase< PinholePose< CALIBRATION > >
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 inherited from gtsam::SmartFactorBase< PinholePose< CALIBRATION > >
GTSAM_MAKE_ALIGNED_OPERATOR_NEW typedef boost::shared_ptr< This > | shared_ptr
| shorthand for a smart pointer to a factor.
Static Public Attributes inherited from gtsam::SmartFactorBase< PinholePose< CALIBRATION > >
static const int | Dim
| Camera dimension.
static const int | ZDim
| Measurement dimension.
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.

## Constructor & Destructor Documentation

## SmartProjectionPoseFactor() [1/2]

template<class CALIBRATION>

gtsam::SmartProjectionPoseFactor< CALIBRATION >::SmartProjectionPoseFactor | ( | const SharedNoiseModel & | sharedNoiseModel,
|  | const boost::shared_ptr< CALIBRATION > | K,
|  | const SmartProjectionParams & | params = SmartProjectionParams() )

inline

Constructor.

Parameters

  sharedNoiseModel | isotropic noise model for the 2D feature measurements
  K | (fixed) calibration, assumed to be the same for all cameras
  params | parameters for the smart projection factors

## SmartProjectionPoseFactor() [2/2]

template<class CALIBRATION>

gtsam::SmartProjectionPoseFactor< CALIBRATION >::SmartProjectionPoseFactor | ( | const SharedNoiseModel & | sharedNoiseModel,
|  | const boost::shared_ptr< CALIBRATION > | K,
|  | const boost::optional< Pose3 > | body_P_sensor,
|  | const SmartProjectionParams & | params = SmartProjectionParams() )

inline

Constructor.

Parameters

  sharedNoiseModel | isotropic noise model for the 2D feature measurements
  K | (fixed) calibration, assumed to be the same for all cameras
  body_P_sensor | pose of the camera in the body frame (optional)
  params | parameters for the smart projection factors

## Member Function Documentation

## cameras()

template<class CALIBRATION>

Base::Cameras gtsam::SmartProjectionPoseFactor< CALIBRATION >::cameras | ( | const Values & | values | ) | const

inline override virtual

Collect all cameras involved in this factor.

Parameters

  values | Values structure which must contain camera poses corresponding to keys involved in this factor

Returns
  vector of Values

Reimplemented from gtsam::SmartFactorBase< PinholePose< CALIBRATION > >.

## equals()

template<class CALIBRATION>

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

inline override virtual

equals

Reimplemented from gtsam::SmartProjectionFactor< PinholePose< CALIBRATION > >.

## error()

template<class CALIBRATION>

double gtsam::SmartProjectionPoseFactor< CALIBRATION >::error | ( | const Values & | values | ) | const

inline override virtual

error calculates the error of the factor.

Reimplemented from gtsam::SmartProjectionFactor< PinholePose< CALIBRATION > >.

## print()

template<class CALIBRATION>

void gtsam::SmartProjectionPoseFactor< CALIBRATION >::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::SmartProjectionFactor< PinholePose< CALIBRATION > >.

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

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