Public Types | Public Member Functions | Public Attributes | Static Public Attributes | Protected Attributes | List of all members
gtsam::SmartStereoProjectionFactorPP Class Reference

#include <SmartStereoProjectionFactorPP.h>

Inheritance diagram for gtsam::SmartStereoProjectionFactorPP:
Inheritance graph
[legend]

Public Types

typedef std::vector< MatrixZD, Eigen::aligned_allocator< MatrixZD > > FBlocks
 
typedef Eigen::Matrix< double, ZDim, DimBlockMatrixZD
 
typedef std::shared_ptr< Thisshared_ptr
 shorthand for a smart pointer to a factor More...
 
typedef SmartStereoProjectionFactorPP This
 shorthand for this class More...
 
- Public Types inherited from gtsam::SmartStereoProjectionFactor
typedef CameraSet< StereoCameraCameras
 Vector of cameras. More...
 
typedef PinholeCamera< Cal3_S2MonoCamera
 Vector of monocular cameras (stereo treated as 2 monocular) More...
 
typedef CameraSet< MonoCameraMonoCameras
 
typedef MonoCamera::MeasurementVector MonoMeasurements
 
typedef std::shared_ptr< SmartStereoProjectionFactorshared_ptr
 shorthand for a smart pointer to a factor More...
 
- Public Types inherited from gtsam::SmartFactorBase< StereoCamera >
typedef CameraSet< StereoCameraCameras
 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 StereoPoint2 &measured, const Key &world_P_body_key, const Key &body_P_cam_key, const std::shared_ptr< Cal3_S2Stereo > &K)
 
void add (const std::vector< StereoPoint2 > &measurements, const KeyVector &w_P_body_keys, const KeyVector &body_P_cam_keys, const std::vector< std::shared_ptr< Cal3_S2Stereo >> &Ks)
 
void add (const std::vector< StereoPoint2 > &measurements, const KeyVector &w_P_body_keys, const KeyVector &body_P_cam_keys, const std::shared_ptr< Cal3_S2Stereo > &K)
 
std::vector< std::shared_ptr< Cal3_S2Stereo > > calibration () const
 
Base::Cameras cameras (const Values &values) const override
 
void computeJacobiansAndCorrectForMissingMeasurements (FBlocks &Fs, Matrix &E, Vector &b, const Values &values) const
 
std::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) More...
 
bool equals (const NonlinearFactor &p, double tol=1e-9) const override
 equals More...
 
double error (const Values &values) const override
 
const KeyVectorgetExtrinsicPoseKeys () const
 equals More...
 
std::shared_ptr< GaussianFactorlinearize (const Values &values) const override
 linearize More...
 
std::shared_ptr< GaussianFactorlinearizeDamped (const Values &values, const double lambda=0.0) const
 
void print (const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
 
 SmartStereoProjectionFactorPP (const SharedNoiseModel &sharedNoiseModel, const SmartStereoProjectionParams &params=SmartStereoProjectionParams())
 
- Public Member Functions inherited from gtsam::SmartStereoProjectionFactor
void computeJacobiansWithTriangulatedPoint (FBlocks &Fs, Matrix &E, Vector &b, const Cameras &cameras) const
 
void correctForMissingMeasurements (const Cameras &cameras, Vector &ue, typename Cameras::FBlocks *Fs=nullptr, Matrix *E=nullptr) const override
 
std::shared_ptr< RegularHessianFactor< Base::Dim > > createHessianFactor (const Cameras &cameras, const double lambda=0.0, bool diagonalDamping=false) const
 linearize returns a Hessianfactor that is an approximation of error(p) More...
 
std::shared_ptr< JacobianFactorcreateJacobianSVDFactor (const Cameras &cameras, double lambda) const
 different (faster) way to compute Jacobian factor More...
 
bool decideIfTriangulate (const Cameras &cameras) const
 Check if the new linearization point_ is the same as the one used for previous triangulation. More...
 
bool isDegenerate () const
 
bool isFarPoint () const
 
bool isOutlier () const
 
bool isPointBehindCamera () const
 
bool isValid () const
 Is result valid? More...
 
std::shared_ptr< GaussianFactorlinearizeDamped (const Cameras &cameras, const double lambda=0.0) const
 
std::shared_ptr< GaussianFactorlinearizeDamped (const Values &values, const double lambda=0.0) const
 
TriangulationResult point () const
 
TriangulationResult point (const Values &values) const
 
Vector reprojectionErrorAfterTriangulation (const Values &values) const
 Calculate vector of re-projection errors, before applying noise model. More...
 
 SmartStereoProjectionFactor (const SharedNoiseModel &sharedNoiseModel, const SmartStereoProjectionParams &params=SmartStereoProjectionParams(), const std::optional< Pose3 > body_P_sensor={})
 
double totalReprojectionError (const Cameras &cameras, std::optional< Point3 > externalPoint={}) const
 
bool triangulateAndComputeE (Matrix &E, const Cameras &cameras) const
 
bool triangulateAndComputeE (Matrix &E, const Values &values) const
 
bool triangulateAndComputeJacobians (FBlocks &Fs, Matrix &E, Vector &b, const Values &values) const
 Version that takes values, and creates the point. More...
 
bool triangulateAndComputeJacobiansSVD (FBlocks &Fs, Matrix &Enull, Vector &b, const Values &values) const
 takes values More...
 
bool triangulateForLinearize (const Cameras &cameras) const
 triangulate More...
 
TriangulationResult triangulateSafe (const Cameras &cameras) const
 triangulateSafe More...
 
 ~SmartStereoProjectionFactor () override
 
- Public Member Functions inherited from gtsam::SmartFactorBase< StereoCamera >
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...
 
void add (const SFM_TRACK &trackToAdd)
 
Pose3 body_P_sensor () const
 
void computeJacobians (FBlocks &Fs, Matrix &E, Vector &b, const Cameras &cameras, const POINT &point) const
 
void computeJacobiansSVD (FBlocks &Fs, Matrix &Enull, Vector &b, const Cameras &cameras, const POINT &point) const
 
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< StereoCamera > > 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...
 
double totalReprojectionError (const Cameras &cameras, const POINT &point) const
 
Vector unwhitenedError (const Cameras &cameras, const POINT &point, typename Cameras::FBlocks *Fs=nullptr, Matrix *E=nullptr) const
 
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
 
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)
 
double error (const HybridValues &c) const override
 
virtual bool active (const Values &) const
 
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 ()
 

Public Attributes

EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef SmartStereoProjectionFactor Base
 shorthand for base class type More...
 
- Public Attributes inherited from gtsam::SmartFactorBase< StereoCamera >
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 DimBlock = 12
 Camera dimension: 6 for body pose, 6 for extrinsic pose. More...
 
static const int DimPose = 6
 Pose3 dimension. More...
 
static const int ZDim = 3
 Measurement dimension (for a StereoPoint2 measurement) More...
 
- Static Public Attributes inherited from gtsam::SmartFactorBase< StereoCamera >
static const int Dim
 Camera dimension. More...
 
static const int ZDim
 Measurement dimension. More...
 

Protected Attributes

KeyVector body_P_cam_keys_
 The keys corresponding to the extrinsic pose calibration for each view (pose that transform from camera to body) More...
 
std::vector< std::shared_ptr< Cal3_S2Stereo > > K_all_
 shared pointer to calibration object (one for each camera) More...
 
KeyVector world_P_body_keys_
 The keys corresponding to the pose of the body (with respect to an external world frame) for each view. More...
 
- Protected Attributes inherited from gtsam::SmartStereoProjectionFactor
const SmartStereoProjectionParams params_
 
TriangulationResult result_
 result from triangulateSafe More...
 
std::vector< Pose3cameraPosesTriangulation_
 current triangulation poses More...
 
- Protected Attributes inherited from gtsam::SmartFactorBase< StereoCamera >
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...
 

Additional Inherited Members

- Static Public Member Functions inherited from gtsam::SmartFactorBase< StereoCamera >
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...
 
- 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

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 optimizes the pose of the body as well as the extrinsic camera calibration (pose of camera wrt body). Each camera may have its own extrinsic calibration or the same calibration can be shared by multiple cameras. This factor requires that values contain the involved poses and extrinsics (both are Pose3 variables).

Definition at line 43 of file SmartStereoProjectionFactorPP.h.

Member Typedef Documentation

◆ FBlocks

Definition at line 71 of file SmartStereoProjectionFactorPP.h.

◆ MatrixZD

Definition at line 70 of file SmartStereoProjectionFactorPP.h.

◆ shared_ptr

shorthand for a smart pointer to a factor

Definition at line 65 of file SmartStereoProjectionFactorPP.h.

◆ This

shorthand for this class

Definition at line 62 of file SmartStereoProjectionFactorPP.h.

Constructor & Destructor Documentation

◆ SmartStereoProjectionFactorPP()

gtsam::SmartStereoProjectionFactorPP::SmartStereoProjectionFactorPP ( const SharedNoiseModel sharedNoiseModel,
const SmartStereoProjectionParams params = SmartStereoProjectionParams() 
)

Constructor

Parameters
Isotropicmeasurement noise
paramsinternal parameters of the smart factors

Definition at line 23 of file SmartStereoProjectionFactorPP.cpp.

Member Function Documentation

◆ add() [1/3]

void gtsam::SmartStereoProjectionFactorPP::add ( const StereoPoint2 measured,
const Key world_P_body_key,
const Key body_P_cam_key,
const std::shared_ptr< Cal3_S2Stereo > &  K 
)

add a new measurement, with a pose key, and an extrinsic pose key

Parameters
measuredis the 3-dimensional location of the projection of a single landmark in the a single (stereo) view (the measurement)
world_P_body_keyis the key corresponding to the body poses observing the same landmark
body_P_cam_keyis the key corresponding to the extrinsic camera-to-body pose calibration
Kis the (fixed) camera intrinsic calibration

Definition at line 28 of file SmartStereoProjectionFactorPP.cpp.

◆ add() [2/3]

void gtsam::SmartStereoProjectionFactorPP::add ( const std::vector< StereoPoint2 > &  measurements,
const KeyVector w_P_body_keys,
const KeyVector body_P_cam_keys,
const std::vector< std::shared_ptr< Cal3_S2Stereo >> &  Ks 
)

Variant of the previous one in which we include a set of measurements

Parameters
measurementsvector of the 3m dimensional location of the projection of a single landmark in the m (stereo) view (the measurements)
w_P_body_keysare the ordered keys corresponding to the body poses observing the same landmark
body_P_cam_keysare the ordered keys corresponding to the extrinsic camera-to-body poses calibration (note: elements of this vector do not need to be unique: 2 camera views can share the same calibration)
Ksvector of intrinsic calibration objects

Definition at line 45 of file SmartStereoProjectionFactorPP.cpp.

◆ add() [3/3]

void gtsam::SmartStereoProjectionFactorPP::add ( const std::vector< StereoPoint2 > &  measurements,
const KeyVector w_P_body_keys,
const KeyVector body_P_cam_keys,
const std::shared_ptr< Cal3_S2Stereo > &  K 
)

Variant of the previous one in which we include a set of measurements with the same noise and calibration

Parameters
measurementsvector of the 3m dimensional location of the projection of a single landmark in the m (stereo) view (the measurements)
w_P_body_keysare the ordered keys corresponding to the body poses observing the same landmark
body_P_cam_keysare the ordered keys corresponding to the extrinsic camera-to-body poses calibration (note: elements of this vector do not need to be unique: 2 camera views can share the same calibration)
Kthe (known) camera calibration (same for all measurements)

Definition at line 65 of file SmartStereoProjectionFactorPP.cpp.

◆ calibration()

std::vector<std::shared_ptr<Cal3_S2Stereo> > gtsam::SmartStereoProjectionFactorPP::calibration ( ) const
inline

return the calibration object

Definition at line 143 of file SmartStereoProjectionFactorPP.h.

◆ cameras()

SmartStereoProjectionFactorPP::Base::Cameras gtsam::SmartStereoProjectionFactorPP::cameras ( const Values values) const
overridevirtual

Collect all cameras involved in this factor

Parameters
valuesValues structure which must contain camera poses corresponding to keys involved in this factor
Returns
vector of Values

Reimplemented from gtsam::SmartFactorBase< StereoCamera >.

Definition at line 112 of file SmartStereoProjectionFactorPP.cpp.

◆ computeJacobiansAndCorrectForMissingMeasurements()

void gtsam::SmartStereoProjectionFactorPP::computeJacobiansAndCorrectForMissingMeasurements ( FBlocks Fs,
Matrix E,
Vector b,
const Values values 
) const
inline

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

Parameters
valuesValues 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 the body pose and extrinsic pose), the point Jacobian E, and the error vector b. Note that the jacobians are computed for a given point.

Definition at line 164 of file SmartStereoProjectionFactorPP.h.

◆ createHessianFactor()

std::shared_ptr<RegularHessianFactor<DimPose> > gtsam::SmartStereoProjectionFactorPP::createHessianFactor ( const Values values,
const double  lambda = 0.0,
bool  diagonalDamping = false 
) const
inline

linearize and return a Hessianfactor that is an approximation of error(p)

Definition at line 204 of file SmartStereoProjectionFactorPP.h.

◆ equals()

bool gtsam::SmartStereoProjectionFactorPP::equals ( const NonlinearFactor p,
double  tol = 1e-9 
) const
overridevirtual

equals

Reimplemented from gtsam::SmartStereoProjectionFactor.

Definition at line 94 of file SmartStereoProjectionFactorPP.cpp.

◆ error()

double gtsam::SmartStereoProjectionFactorPP::error ( const Values values) const
overridevirtual

error calculates the error of the factor.

Reimplemented from gtsam::SmartStereoProjectionFactor.

Definition at line 103 of file SmartStereoProjectionFactorPP.cpp.

◆ getExtrinsicPoseKeys()

const KeyVector& gtsam::SmartStereoProjectionFactorPP::getExtrinsicPoseKeys ( ) const
inline

equals

Definition at line 133 of file SmartStereoProjectionFactorPP.h.

◆ linearize()

std::shared_ptr<GaussianFactor> gtsam::SmartStereoProjectionFactorPP::linearize ( const Values values) const
inlineoverridevirtual

linearize

Reimplemented from gtsam::SmartStereoProjectionFactor.

Definition at line 282 of file SmartStereoProjectionFactorPP.h.

◆ linearizeDamped()

std::shared_ptr<GaussianFactor> gtsam::SmartStereoProjectionFactorPP::linearizeDamped ( const Values values,
const double  lambda = 0.0 
) const
inline

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

Parameters
valuesValues structure which must contain camera poses and extrinsic pose for this factor
Returns
a Gaussian factor

Definition at line 269 of file SmartStereoProjectionFactorPP.h.

◆ print()

void gtsam::SmartStereoProjectionFactorPP::print ( const std::string &  s = "",
const KeyFormatter keyFormatter = DefaultKeyFormatter 
) const
overridevirtual

print

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

Reimplemented from gtsam::SmartStereoProjectionFactor.

Definition at line 84 of file SmartStereoProjectionFactorPP.cpp.

Member Data Documentation

◆ Base

shorthand for base class type

Definition at line 59 of file SmartStereoProjectionFactorPP.h.

◆ body_P_cam_keys_

KeyVector gtsam::SmartStereoProjectionFactorPP::body_P_cam_keys_
protected

The keys corresponding to the extrinsic pose calibration for each view (pose that transform from camera to body)

Definition at line 53 of file SmartStereoProjectionFactorPP.h.

◆ DimBlock

const int gtsam::SmartStereoProjectionFactorPP::DimBlock = 12
static

Camera dimension: 6 for body pose, 6 for extrinsic pose.

Definition at line 67 of file SmartStereoProjectionFactorPP.h.

◆ DimPose

const int gtsam::SmartStereoProjectionFactorPP::DimPose = 6
static

Pose3 dimension.

Definition at line 68 of file SmartStereoProjectionFactorPP.h.

◆ K_all_

std::vector<std::shared_ptr<Cal3_S2Stereo> > gtsam::SmartStereoProjectionFactorPP::K_all_
protected

shared pointer to calibration object (one for each camera)

Definition at line 47 of file SmartStereoProjectionFactorPP.h.

◆ world_P_body_keys_

KeyVector gtsam::SmartStereoProjectionFactorPP::world_P_body_keys_
protected

The keys corresponding to the pose of the body (with respect to an external world frame) for each view.

Definition at line 50 of file SmartStereoProjectionFactorPP.h.

◆ ZDim

const int gtsam::SmartStereoProjectionFactorPP::ZDim = 3
static

Measurement dimension (for a StereoPoint2 measurement)

Definition at line 69 of file SmartStereoProjectionFactorPP.h.


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


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