#include <SmartStereoProjectionFactor.h>
Public Types | |
typedef CameraSet< StereoCamera > | Cameras |
Vector of cameras. More... | |
typedef PinholeCamera< Cal3_S2 > | MonoCamera |
Vector of monocular cameras (stereo treated as 2 monocular) More... | |
typedef CameraSet< MonoCamera > | MonoCameras |
typedef MonoCamera::MeasurementVector | MonoMeasurements |
typedef std::shared_ptr< SmartStereoProjectionFactor > | shared_ptr |
shorthand for a smart pointer to a factor More... | |
Public Types inherited from gtsam::SmartFactorBase< StereoCamera > | |
typedef CameraSet< StereoCamera > | 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, Dim > | MatrixZD |
Public Types inherited from gtsam::NonlinearFactor | |
typedef std::shared_ptr< This > | shared_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, 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< JacobianFactor > | createJacobianSVDFactor (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... | |
std::shared_ptr< GaussianFactor > | linearize (const Values &values) const override |
linearize More... | |
std::shared_ptr< GaussianFactor > | linearizeDamped (const Cameras &cameras, const double lambda=0.0) const |
std::shared_ptr< GaussianFactor > | linearizeDamped (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 ¶ms=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 SFM_TRACK &trackToAdd) |
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... | |
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 |
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< JacobianFactor > | createJacobianSVDFactor (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 ZVector & | measured () 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, OptArgs &&... optArgs) const |
Vector | unwhitenedError (const Cameras &cameras, const POINT &point, typename Cameras::FBlocks *Fs=nullptr, Matrix *E=nullptr) 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 size_t | dim () const =0 |
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 KeyVector & | keys () 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... | |
KeyVector & | keys () |
iterator | begin () |
iterator | end () |
Protected Attributes | |
Parameters | |
const SmartStereoProjectionParams | params_ |
Caching triangulation | |
TriangulationResult | result_ |
result from triangulateSafe More... | |
std::vector< Pose3 > | cameraPosesTriangulation_ |
current triangulation poses More... | |
Protected Attributes inherited from gtsam::SmartFactorBase< StereoCamera > | |
std::optional< Pose3 > | body_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< StereoCamera > | Base |
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... | |
Public Attributes inherited from gtsam::SmartFactorBase< StereoCamera > | |
GTSAM_MAKE_ALIGNED_OPERATOR_NEW typedef std::shared_ptr< This > | shared_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) | |
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) |
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 55 of file SmartStereoProjectionFactor.h.
|
private |
Definition at line 59 of file SmartStereoProjectionFactor.h.
Vector of cameras.
Definition at line 80 of file SmartStereoProjectionFactor.h.
Vector of monocular cameras (stereo treated as 2 monocular)
Definition at line 83 of file SmartStereoProjectionFactor.h.
Definition at line 84 of file SmartStereoProjectionFactor.h.
Definition at line 85 of file SmartStereoProjectionFactor.h.
typedef std::shared_ptr<SmartStereoProjectionFactor> gtsam::SmartStereoProjectionFactor::shared_ptr |
shorthand for a smart pointer to a factor
Definition at line 77 of file SmartStereoProjectionFactor.h.
|
inline |
Constructor
params | internal parameters of the smart factors |
Definition at line 91 of file SmartStereoProjectionFactor.h.
|
inlineoverride |
Virtual destructor
Definition at line 100 of file SmartStereoProjectionFactor.h.
|
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 363 of file SmartStereoProjectionFactor.h.
|
inlineoverridevirtual |
This corrects the Jacobians and error vector for the case in which the right 2D measurement in the monocular camera is missing (nan).
Reimplemented from gtsam::SmartFactorBase< StereoCamera >.
Definition at line 457 of file SmartStereoProjectionFactor.h.
|
inline |
linearize returns a Hessianfactor that is an approximation of error(p)
Definition at line 203 of file SmartStereoProjectionFactor.h.
|
inline |
different (faster) way to compute Jacobian factor
Definition at line 273 of file SmartStereoProjectionFactor.h.
|
inline |
Check if the new linearization point_ is the same as the one used for previous triangulation.
Definition at line 126 of file SmartStereoProjectionFactor.h.
|
inlineoverridevirtual |
equals
Reimplemented from gtsam::NonlinearFactor.
Reimplemented in gtsam::SmartStereoProjectionFactorPP, and gtsam::SmartStereoProjectionPoseFactor.
Definition at line 118 of file SmartStereoProjectionFactor.h.
|
inlineoverridevirtual |
Calculate total reprojection error.
Reimplemented from gtsam::NonlinearFactor.
Reimplemented in gtsam::SmartStereoProjectionFactorPP, and gtsam::SmartStereoProjectionPoseFactor.
Definition at line 445 of file SmartStereoProjectionFactor.h.
|
inline |
return the degenerate state
Definition at line 494 of file SmartStereoProjectionFactor.h.
|
inline |
return the farPoint state
Definition at line 503 of file SmartStereoProjectionFactor.h.
|
inline |
return the outlier state
Definition at line 500 of file SmartStereoProjectionFactor.h.
|
inline |
return the cheirality status flag
Definition at line 497 of file SmartStereoProjectionFactor.h.
|
inline |
Is result valid?
Definition at line 491 of file SmartStereoProjectionFactor.h.
|
inlineoverridevirtual |
linearize
Implements gtsam::NonlinearFactor.
Reimplemented in gtsam::SmartStereoProjectionFactorPP.
Definition at line 334 of file SmartStereoProjectionFactor.h.
|
inline |
Linearize to Gaussian Factor
values | Values structure which must contain camera poses for this factor |
Definition at line 304 of file SmartStereoProjectionFactor.h.
|
inline |
Linearize to Gaussian Factor
values | Values structure which must contain camera poses for this factor |
Definition at line 326 of file SmartStereoProjectionFactor.h.
|
inline |
return the landmark
Definition at line 480 of file SmartStereoProjectionFactor.h.
|
inline |
COMPUTE the landmark
Definition at line 485 of file SmartStereoProjectionFactor.h.
|
inlineoverridevirtual |
s | optional string naming the factor |
keyFormatter | optional formatter useful for printing Symbols |
Reimplemented from gtsam::NonlinearFactor.
Reimplemented in gtsam::SmartStereoProjectionFactorPP, and gtsam::SmartStereoProjectionPoseFactor.
Definition at line 108 of file SmartStereoProjectionFactor.h.
|
inline |
Calculate vector of re-projection errors, before applying noise model.
Definition at line 405 of file SmartStereoProjectionFactor.h.
|
inline |
Calculate the error of the factor. This is the log-likelihood, e.g. in case of Gaussian. In this class, we take the raw prediction error , ask the noise model to transform it to , and then multiply by 0.5.
Definition at line 420 of file SmartStereoProjectionFactor.h.
|
inline |
Triangulate and compute derivative of error with respect to point
Definition at line 343 of file SmartStereoProjectionFactor.h.
|
inline |
Triangulate and compute derivative of error with respect to point
Definition at line 354 of file SmartStereoProjectionFactor.h.
|
inline |
Version that takes values, and creates the point.
Definition at line 383 of file SmartStereoProjectionFactor.h.
|
inline |
takes values
Definition at line 394 of file SmartStereoProjectionFactor.h.
|
inline |
triangulate
Definition at line 197 of file SmartStereoProjectionFactor.h.
|
inline |
triangulateSafe
Definition at line 167 of file SmartStereoProjectionFactor.h.
|
mutableprotected |
current triangulation poses
Definition at line 71 of file SmartStereoProjectionFactor.h.
|
protected |
Definition at line 65 of file SmartStereoProjectionFactor.h.
|
mutableprotected |
result from triangulateSafe
Definition at line 70 of file SmartStereoProjectionFactor.h.