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

## Detailed Description

template<class CAMERA>
class gtsam::SmartProjectionRigFactor< CAMERA >

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 (but each measurement can be taken by a different camera in the rig, hence can have a different extrinsic and intrinsic calibration). The factor only constrains poses (variable dimension is 6 for each pose). This factor requires that values contains the involved poses (Pose3). If all measurements share the same calibration (i.e., are from the same camera), use SmartProjectionPoseFactor instead! If the calibration should be optimized, as well, use SmartProjectionFactor instead!

Inheritance diagram for gtsam::SmartProjectionRigFactor< CAMERA >:

## Public Member Functions

| SmartProjectionRigFactor ()
| Default constructor, only for serialization.
| SmartProjectionRigFactor (const SharedNoiseModel &sharedNoiseModel, const boost::shared_ptr< Cameras > &cameraRig, const SmartProjectionParams &params=SmartProjectionParams())
| Constructor.
| ~SmartProjectionRigFactor () override=default
| Virtual destructor.
void | add (const MEASUREMENT &measured, const Key &poseKey, const size_t &cameraId=0)
| add a new measurement, corresponding to an observation from pose "poseKey" and taken from the camera in the rig identified by "cameraId"
void | add (const MEASUREMENTS &measurements, const KeyVector &poseKeys, const FastVector< size_t > &cameraIds=FastVector< size_t >())
| Variant of the previous "add" function in which we include multiple measurements.
const KeyVector & | nonUniqueKeys () const
| return (for each observation) the (possibly non unique) keys involved in the measurements
const boost::shared_ptr< Cameras > & | cameraRig () const
| return the calibration object
const FastVector< size_t > & | cameraIds () const
| return the calibration object
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
Base::Cameras | cameras (const Values &values) const override
| Collect all cameras involved in this factor.
double | error (const Values &values) const override
| error calculates the error of the factor.
void | computeJacobiansWithTriangulatedPoint (typename Base::FBlocks &Fs, Matrix &E, Vector &b, const Cameras &cameras) const
| Compute jacobian F, E and error vector at a given linearization point.
boost::shared_ptr< RegularHessianFactor< DimPose > > | createHessianFactor (const Values &values, const double &lambda=0.0, bool diagonalDamping=false) const
| linearize and return a Hessianfactor that is an approximation of error(p)
boost::shared_ptr< GaussianFactor > | linearizeDamped (const Values &values, const double &lambda=0.0) const
| Linearize to Gaussian Factor (possibly adding a damping factor Lambda for LM).
boost::shared_ptr< GaussianFactor > | linearize (const Values &values) const override
| linearize
Public Member Functions inherited from gtsam::SmartProjectionFactor< CAMERA >
| SmartProjectionFactor ()
| Default constructor, only for serialization.
| SmartProjectionFactor (const SharedNoiseModel &sharedNoiseModel, const SmartProjectionParams &params=SmartProjectionParams())
| Constructor.
| ~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< CAMERA > > | 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< JacobianFactorQ< Base::Dim, 2 > > | createJacobianQFactor (const Values &values, double lambda) const
| Create JacobianFactorQ factor, takes values.
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< CAMERA > > | 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 > | linearizeDamped (const Values &values, const double lambda=0.0) const
| Linearize to Gaussian Factor.
bool | triangulateAndComputeE (Matrix &E, const Cameras &cameras) const
| Triangulate and compute derivative of error with respect to point.
bool | triangulateAndComputeE (Matrix &E, const Values &values) 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
TriangulationResult | point (const Values &values) const
| COMPUTE 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< CAMERA >
| 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).
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.
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 Attributes

EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef CAMERA | Camera
Public Attributes inherited from gtsam::SmartFactorBase< CAMERA >
GTSAM_MAKE_ALIGNED_OPERATOR_NEW typedef boost::shared_ptr< This > | shared_ptr
| shorthand for a smart pointer to a factor.

## Public Types

typedef CameraSet< CAMERA > | Cameras
typedef boost::shared_ptr< This > | shared_ptr
| shorthand for a smart pointer to a factor
Public Types inherited from gtsam::SmartProjectionFactor< CAMERA >
typedef boost::shared_ptr< This > | shared_ptr
| shorthand for a smart pointer to a factor
typedef CAMERA | Camera
| shorthand for a set of cameras
typedef CameraSet< CAMERA > | Cameras
Public Types inherited from gtsam::SmartFactorBase< CAMERA >
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

KeyVector | nonUniqueKeys_
| vector of keys (one for each observation) with potentially repeated keys
boost::shared_ptr< typename Base::Cameras > | cameraRig_
| cameras in the rig (fixed poses wrt body and intrinsics, for each camera)
FastVector< size_t > | cameraIds_
| vector of camera Ids (one for each observation, in the same order), identifying which camera took the measurement
Protected Attributes inherited from gtsam::SmartProjectionFactor< CAMERA >
SmartProjectionParams | params_
TriangulationResult | result_
| result from triangulateSafe
std::vector< Pose3, Eigen::aligned_allocator< Pose3 > > | cameraPosesTriangulation_
| current triangulation poses
Protected Attributes inherited from gtsam::SmartFactorBase< CAMERA >
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< CAMERA >
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.
Static Public Attributes inherited from gtsam::SmartFactorBase< CAMERA >
static const int | Dim = traits<CAMERA>::dimension
| Camera dimension.
static const int | ZDim = traits<Z>::dimension
| 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

## SmartProjectionRigFactor()

template<class CAMERA>

gtsam::SmartProjectionRigFactor< CAMERA >::SmartProjectionRigFactor | ( | const SharedNoiseModel & | sharedNoiseModel,
|  | const boost::shared_ptr< Cameras > & | cameraRig,
|  | const SmartProjectionParams & | params = SmartProjectionParams() )

inline

Constructor.

Parameters

  sharedNoiseModel | isotropic noise model for the 2D feature measurements
  cameraRig | set of cameras (fixed poses wrt body and intrinsics) in the camera rig
  params | parameters for the smart projection factors

## Member Function Documentation

## add() [1/2]

template<class CAMERA>

void gtsam::SmartProjectionRigFactor< CAMERA >::add | ( | const MEASUREMENT & | measured,
|  | const Key & | poseKey,
|  | const size_t & | cameraId = 0 )

inline

add a new measurement, corresponding to an observation from pose "poseKey" and taken from the camera in the rig identified by "cameraId"

Parameters

  measured | 2-dimensional location of the projection of a single landmark in a single view (the measurement)
  poseKey | key corresponding to the body pose of the camera taking the measurement
  cameraId | ID of the camera in the rig taking the measurement (default 0)

## add() [2/2]

template<class CAMERA>

void gtsam::SmartProjectionRigFactor< CAMERA >::add | ( | const MEASUREMENTS & | measurements,
|  | const KeyVector & | poseKeys,
|  | const FastVector< size_t > & | cameraIds = FastVector<size_t>() )

inline

Variant of the previous "add" function in which we include multiple measurements.

Parameters

  measurements | vector of the 2m dimensional location of the projection of a single landmark in the m views (the measurements)
  poseKeys | keys corresponding to the body poses of the cameras taking the measurements
  cameraIds | IDs of the cameras in the rig taking each measurement (same order as the measurements)

## cameras()

template<class CAMERA>

Base::Cameras gtsam::SmartProjectionRigFactor< CAMERA >::cameras | ( | const Values & | values | ) | const

inline override virtual

Collect all cameras involved in this factor.

Parameters

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

Returns
  vector of cameras

Reimplemented from gtsam::SmartFactorBase< CAMERA >.

## computeJacobiansWithTriangulatedPoint()

template<class CAMERA>

void gtsam::SmartProjectionRigFactor< CAMERA >::computeJacobiansWithTriangulatedPoint | ( | typename Base::FBlocks & | Fs,
|  | Matrix & | E,
|  | Vector & | b,
|  | const Cameras & | cameras ) const

inline

Compute jacobian F, E and error vector at a given linearization point.

Parameters

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

Returns
  Return arguments are the camera jacobians Fs (including the jacobian with respect to both body poses we interpolate from), the point Jacobian E, and the error vector b. Note that the jacobians are computed for a given point.

## equals()

template<class CAMERA>

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

inline override virtual

equals

Reimplemented from gtsam::SmartProjectionFactor< CAMERA >.

## error()

template<class CAMERA>

double gtsam::SmartProjectionRigFactor< CAMERA >::error | ( | const Values & | values | ) | const

inline override virtual

error calculates the error of the factor.

Reimplemented from gtsam::SmartProjectionFactor< CAMERA >.

## linearize()

template<class CAMERA>

boost::shared_ptr< GaussianFactor > gtsam::SmartProjectionRigFactor< CAMERA >::linearize | ( | const Values & | values | ) | const

inline override virtual

linearize

Reimplemented from gtsam::SmartProjectionFactor< CAMERA >.

## linearizeDamped()

template<class CAMERA>

boost::shared_ptr< GaussianFactor > gtsam::SmartProjectionRigFactor< CAMERA >::linearizeDamped | ( | const Values & | values,
|  | const double & | lambda = 0.0 ) const

inline

Linearize to Gaussian Factor (possibly adding a damping factor Lambda for LM).

Parameters

  values | Values structure which must contain camera poses and extrinsic pose for this factor

Returns
  a Gaussian factor

## print()

template<class CAMERA>

void gtsam::SmartProjectionRigFactor< 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::SmartProjectionFactor< CAMERA >.

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

- /build/gtsam-Xg031d/gtsam-4.2.0+dfsg/gtsam/slam/SmartProjectionRigFactor.h