Public Types | Public Member Functions | Private Types | Private Member Functions | Friends | List of all members
gtsam::SmartStereoProjectionFactor Class Reference

#include <SmartStereoProjectionFactor.h>

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

Public Types

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 boost::shared_ptr< SmartStereoProjectionFactorshared_ptr
 shorthand for a smart pointer to a factor More...
 
- Public Types inherited from gtsam::SmartFactorBase< StereoCamera >
typedef CameraSet< StereoCameraCameras
 We use the new CameraSte data structure 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 boost::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 computeJacobiansWithTriangulatedPoint (FBlocks &Fs, Matrix &E, Vector &b, const Cameras &cameras) const
 
void correctForMissingMeasurements (const Cameras &cameras, Vector &ue, boost::optional< typename Cameras::FBlocks & > Fs=boost::none, boost::optional< Matrix & > E=boost::none) const override
 
boost::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...
 
boost::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 equals (const NonlinearFactor &p, double tol=1e-9) const override
 equals More...
 
double error (const Values &values) const override
 Calculate total reprojection error. More...
 
bool isDegenerate () const
 
bool isFarPoint () const
 
bool isOutlier () const
 
bool isPointBehindCamera () const
 
bool isValid () const
 Is result valid? More...
 
boost::shared_ptr< GaussianFactorlinearize (const Values &values) const override
 linearize More...
 
boost::shared_ptr< GaussianFactorlinearizeDamped (const Cameras &cameras, const double lambda=0.0) const
 
boost::shared_ptr< GaussianFactorlinearizeDamped (const Values &values, const double lambda=0.0) const
 
TriangulationResult point () const
 
TriangulationResult point (const Values &values) const
 
void print (const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
 
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 boost::optional< Pose3 > body_P_sensor=boost::none)
 
double totalReprojectionError (const Cameras &cameras, boost::optional< Point3 > externalPoint=boost::none) 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)
 
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...
 
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
 SVD version. More...
 
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. More...
 
boost::shared_ptr< JacobianFactorQ< Dim, ZDim > > createJacobianQFactor (const Cameras &cameras, const Point3 &point, double lambda=0.0, bool diagonalDamping=false) const
 
boost::shared_ptr< JacobianFactorcreateJacobianSVDFactor (const Cameras &cameras, const Point3 &point, double lambda=0.0) const
 
boost::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
 get 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
 
void print (const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
 
 SmartFactorBase ()
 Default Constructor, for serialization. More...
 
 SmartFactorBase (const SharedNoiseModel &sharedNoiseModel, boost::optional< Pose3 > body_P_sensor=boost::none, size_t expectedNumberCameras=10)
 Constructor. More...
 
double totalReprojectionError (const Cameras &cameras, const POINT &point) const
 
Vector unwhitenedError (const Cameras &cameras, const POINT &point, boost::optional< typename Cameras::FBlocks & > Fs=boost::none, boost::optional< Matrix & > E=boost::none) 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)
 
virtual ~NonlinearFactor ()
 
virtual bool active (const Values &) const
 
virtual shared_ptr clone () const
 
shared_ptr rekey (const std::map< Key, Key > &rekey_mapping) const
 
shared_ptr rekey (const KeyVector &new_keys) const
 
- Public Member Functions inherited from gtsam::Factor
virtual ~Factor ()=default
 Default destructor. 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...
 
KeyVectorkeys ()
 
iterator begin ()
 
iterator end ()
 

Protected Attributes

Parameters
const SmartStereoProjectionParams params_
 
Caching triangulation
TriangulationResult result_
 result from triangulateSafe More...
 
std::vector< Pose3cameraPosesTriangulation_
 
- Protected Attributes inherited from gtsam::SmartFactorBase< StereoCamera >
boost::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 SmartFactorBase< StereoCameraBase
 

Private Member Functions

template<class ARCHIVE >
void serialize (ARCHIVE &ar, const unsigned int)
 

Friends

class boost::serialization::access
 Serialization function. 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 (Matrix &E)
 Computes Point Covariance P from E. More...
 
- Public Attributes inherited from gtsam::SmartFactorBase< StereoCamera >
GTSAM_MAKE_ALIGNED_OPERATOR_NEW typedef boost::shared_ptr< Thisshared_ptr
 shorthand for a smart pointer to a factor More...
 
- Static Public Attributes inherited from gtsam::SmartFactorBase< StereoCamera >
static const int Dim
 Camera dimension. More...
 
static const int ZDim
 Measurement dimension. 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)
 
bool equals (const This &other, double tol=1e-9) const
 check equality More...
 
- 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

SmartStereoProjectionFactor: triangulates point and keeps an estimate of it around. This factor operates with StereoCamera. This factor requires that values contains the involved StereoCameras. Calibration is assumed to be fixed, as this is also assumed in StereoCamera. If you'd like to store poses in values instead of cameras, use SmartStereoProjectionPoseFactor instead

Definition at line 52 of file SmartStereoProjectionFactor.h.

Member Typedef Documentation

Definition at line 55 of file SmartStereoProjectionFactor.h.

Vector of cameras.

Definition at line 76 of file SmartStereoProjectionFactor.h.

Vector of monocular cameras (stereo treated as 2 monocular)

Definition at line 79 of file SmartStereoProjectionFactor.h.

Definition at line 80 of file SmartStereoProjectionFactor.h.

Definition at line 81 of file SmartStereoProjectionFactor.h.

shorthand for a smart pointer to a factor

Definition at line 73 of file SmartStereoProjectionFactor.h.

Constructor & Destructor Documentation

gtsam::SmartStereoProjectionFactor::SmartStereoProjectionFactor ( const SharedNoiseModel sharedNoiseModel,
const SmartStereoProjectionParams params = SmartStereoProjectionParams(),
const boost::optional< Pose3 body_P_sensor = boost::none 
)
inline

Constructor

Parameters
paramsinternal parameters of the smart factors

Definition at line 87 of file SmartStereoProjectionFactor.h.

gtsam::SmartStereoProjectionFactor::~SmartStereoProjectionFactor ( )
inlineoverride

Virtual destructor

Definition at line 96 of file SmartStereoProjectionFactor.h.

Member Function Documentation

void gtsam::SmartStereoProjectionFactor::computeJacobiansWithTriangulatedPoint ( FBlocks Fs,
Matrix E,
Vector b,
const Cameras cameras 
) const
inline

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

Definition at line 359 of file SmartStereoProjectionFactor.h.

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

This corrects the Jacobians and error vector for the case in which the right pixel in the monocular camera is missing (nan)

Reimplemented from gtsam::SmartFactorBase< StereoCamera >.

Definition at line 452 of file SmartStereoProjectionFactor.h.

boost::shared_ptr<RegularHessianFactor<Base::Dim> > gtsam::SmartStereoProjectionFactor::createHessianFactor ( const Cameras cameras,
const double  lambda = 0.0,
bool  diagonalDamping = false 
) const
inline

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

Definition at line 199 of file SmartStereoProjectionFactor.h.

boost::shared_ptr<JacobianFactor> gtsam::SmartStereoProjectionFactor::createJacobianSVDFactor ( const Cameras cameras,
double  lambda 
) const
inline

different (faster) way to compute Jacobian factor

Definition at line 269 of file SmartStereoProjectionFactor.h.

bool gtsam::SmartStereoProjectionFactor::decideIfTriangulate ( const Cameras cameras) const
inline

Check if the new linearization point_ is the same as the one used for previous triangulation.

Definition at line 122 of file SmartStereoProjectionFactor.h.

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

equals

Reimplemented from gtsam::NonlinearFactor.

Reimplemented in gtsam::SmartStereoProjectionPoseFactor.

Definition at line 114 of file SmartStereoProjectionFactor.h.

double gtsam::SmartStereoProjectionFactor::error ( const Values values) const
inlineoverridevirtual

Calculate total reprojection error.

Implements gtsam::NonlinearFactor.

Reimplemented in gtsam::SmartStereoProjectionPoseFactor.

Definition at line 441 of file SmartStereoProjectionFactor.h.

bool gtsam::SmartStereoProjectionFactor::isDegenerate ( ) const
inline

return the degenerate state

Definition at line 490 of file SmartStereoProjectionFactor.h.

bool gtsam::SmartStereoProjectionFactor::isFarPoint ( ) const
inline

return the farPoint state

Definition at line 499 of file SmartStereoProjectionFactor.h.

bool gtsam::SmartStereoProjectionFactor::isOutlier ( ) const
inline

return the outlier state

Definition at line 496 of file SmartStereoProjectionFactor.h.

bool gtsam::SmartStereoProjectionFactor::isPointBehindCamera ( ) const
inline

return the cheirality status flag

Definition at line 493 of file SmartStereoProjectionFactor.h.

bool gtsam::SmartStereoProjectionFactor::isValid ( ) const
inline

Is result valid?

Definition at line 487 of file SmartStereoProjectionFactor.h.

boost::shared_ptr<GaussianFactor> gtsam::SmartStereoProjectionFactor::linearize ( const Values values) const
inlineoverridevirtual

linearize

Implements gtsam::NonlinearFactor.

Definition at line 330 of file SmartStereoProjectionFactor.h.

boost::shared_ptr<GaussianFactor> gtsam::SmartStereoProjectionFactor::linearizeDamped ( const Cameras cameras,
const double  lambda = 0.0 
) const
inline

Linearize to Gaussian Factor

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

Definition at line 300 of file SmartStereoProjectionFactor.h.

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

Linearize to Gaussian Factor

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

Definition at line 322 of file SmartStereoProjectionFactor.h.

TriangulationResult gtsam::SmartStereoProjectionFactor::point ( ) const
inline

return the landmark

Definition at line 476 of file SmartStereoProjectionFactor.h.

TriangulationResult gtsam::SmartStereoProjectionFactor::point ( const Values values) const
inline

COMPUTE the landmark

Definition at line 481 of file SmartStereoProjectionFactor.h.

void gtsam::SmartStereoProjectionFactor::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::SmartStereoProjectionPoseFactor.

Definition at line 104 of file SmartStereoProjectionFactor.h.

Vector gtsam::SmartStereoProjectionFactor::reprojectionErrorAfterTriangulation ( const Values values) const
inline

Calculate vector of re-projection errors, before applying noise model.

Definition at line 401 of file SmartStereoProjectionFactor.h.

template<class ARCHIVE >
void gtsam::SmartStereoProjectionFactor::serialize ( ARCHIVE &  ar,
const unsigned  int 
)
inlineprivate

Definition at line 506 of file SmartStereoProjectionFactor.h.

double gtsam::SmartStereoProjectionFactor::totalReprojectionError ( const Cameras cameras,
boost::optional< Point3 externalPoint = boost::none 
) 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.

Definition at line 416 of file SmartStereoProjectionFactor.h.

bool gtsam::SmartStereoProjectionFactor::triangulateAndComputeE ( Matrix E,
const Cameras cameras 
) const
inline

Triangulate and compute derivative of error with respect to point

Returns
whether triangulation worked

Definition at line 339 of file SmartStereoProjectionFactor.h.

bool gtsam::SmartStereoProjectionFactor::triangulateAndComputeE ( Matrix E,
const Values values 
) const
inline

Triangulate and compute derivative of error with respect to point

Returns
whether triangulation worked

Definition at line 350 of file SmartStereoProjectionFactor.h.

bool gtsam::SmartStereoProjectionFactor::triangulateAndComputeJacobians ( FBlocks Fs,
Matrix E,
Vector b,
const Values values 
) const
inline

Version that takes values, and creates the point.

Definition at line 379 of file SmartStereoProjectionFactor.h.

bool gtsam::SmartStereoProjectionFactor::triangulateAndComputeJacobiansSVD ( FBlocks Fs,
Matrix Enull,
Vector b,
const Values values 
) const
inline

takes values

Definition at line 390 of file SmartStereoProjectionFactor.h.

bool gtsam::SmartStereoProjectionFactor::triangulateForLinearize ( const Cameras cameras) const
inline

triangulate

Definition at line 193 of file SmartStereoProjectionFactor.h.

TriangulationResult gtsam::SmartStereoProjectionFactor::triangulateSafe ( const Cameras cameras) const
inline

triangulateSafe

Definition at line 163 of file SmartStereoProjectionFactor.h.

Friends And Related Function Documentation

friend class boost::serialization::access
friend

Serialization function.

Definition at line 504 of file SmartStereoProjectionFactor.h.

Member Data Documentation

std::vector<Pose3> gtsam::SmartStereoProjectionFactor::cameraPosesTriangulation_
mutableprotected

current triangulation poses

Definition at line 67 of file SmartStereoProjectionFactor.h.

const SmartStereoProjectionParams gtsam::SmartStereoProjectionFactor::params_
protected

Definition at line 61 of file SmartStereoProjectionFactor.h.

TriangulationResult gtsam::SmartStereoProjectionFactor::result_
mutableprotected

result from triangulateSafe

Definition at line 66 of file SmartStereoProjectionFactor.h.


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


gtsam
Author(s):
autogenerated on Sat May 8 2021 02:58:29