#include <SmartProjectionFactor.h>
Public Types | |
typedef CAMERA | Camera |
shorthand for a set of cameras More... | |
typedef CameraSet< CAMERA > | Cameras |
typedef std::shared_ptr< This > | shared_ptr |
shorthand for a smart pointer to a factor More... | |
Public Types inherited from gtsam::SmartFactorBase< CAMERA > | |
typedef CameraSet< CAMERA > | 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 (typename Base::FBlocks &Fs, Matrix &E, Vector &b, const Cameras &cameras) const |
std::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). More... | |
std::shared_ptr< JacobianFactorQ< Base::Dim, 2 > > | createJacobianQFactor (const Cameras &cameras, double lambda) const |
Create JacobianFactorQ factor. More... | |
std::shared_ptr< JacobianFactorQ< Base::Dim, 2 > > | createJacobianQFactor (const Values &values, double lambda) const |
Create JacobianFactorQ factor, takes values. More... | |
std::shared_ptr< JacobianFactor > | createJacobianSVDFactor (const Cameras &cameras, double lambda) const |
Different (faster) way to compute a JacobianFactorSVD factor. More... | |
std::shared_ptr< RegularImplicitSchurFactor< CAMERA > > | createRegularImplicitSchurFactor (const Cameras &cameras, double lambda) const |
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 |
virtual std::shared_ptr< RegularHessianFactor< Base::Dim > > | linearizeToHessian (const Values &values, double lambda=0.0) const |
Linearize to a Hessianfactor. More... | |
virtual std::shared_ptr< RegularImplicitSchurFactor< CAMERA > > | linearizeToImplicit (const Values &values, double lambda=0.0) const |
Linearize to an Implicit Schur factor. More... | |
virtual std::shared_ptr< JacobianFactorQ< Base::Dim, 2 > > | linearizeToJacobian (const Values &values, double lambda=0.0) const |
Linearize to a JacobianfactorQ. More... | |
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... | |
SmartProjectionFactor () | |
SmartProjectionFactor (const SharedNoiseModel &sharedNoiseModel, const SmartProjectionParams ¶ms=SmartProjectionParams()) | |
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 (typename Base::FBlocks &Fs, Matrix &E, Vector &b, const Values &values) const |
Version that takes values, and creates the point. More... | |
bool | triangulateAndComputeJacobiansSVD (typename Base::FBlocks &Fs, Matrix &Enull, Vector &b, const Values &values) const |
takes values More... | |
bool | triangulateForLinearize (const Cameras &cameras) const |
Possibly re-triangulate before calculating Jacobians. More... | |
TriangulationResult | triangulateSafe (const Cameras &cameras) const |
Call gtsam::triangulateSafe iff we need to re-triangulate. More... | |
~SmartProjectionFactor () override | |
Public Member Functions inherited from gtsam::SmartFactorBase< CAMERA > | |
template<class SFM_TRACK > | |
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... | |
template<class POINT > | |
void | computeJacobians (FBlocks &Fs, Matrix &E, Vector &b, const Cameras &cameras, const POINT &point) const |
template<class POINT > | |
void | computeJacobiansSVD (FBlocks &Fs, Matrix &Enull, Vector &b, const Cameras &cameras, const POINT &point) const |
template<class ... OptArgs> | |
void | correctForMissingMeasurements (const Cameras &cameras, Vector &ue, OptArgs &&... optArgs) const |
virtual void | correctForMissingMeasurements (const Cameras &cameras, Vector &ue, typename Cameras::FBlocks *Fs=nullptr, Matrix *E=nullptr) 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< CAMERA > > | 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... | |
const ZVector & | measured () const |
Return the 2D measurements (ZDim, in general). More... | |
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... | |
template<class POINT > | |
double | totalReprojectionError (const Cameras &cameras, const POINT &point) const |
template<class POINT , class ... OptArgs, typename = std::enable_if_t<sizeof...(OptArgs)!=0>> | |
Vector | unwhitenedError (const Cameras &cameras, const POINT &point, OptArgs &&... optArgs) const |
template<class POINT > | |
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 |
template<class POINT > | |
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 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 | |
SmartProjectionParams | params_ |
Caching triangulation | |
TriangulationResult | result_ |
result from triangulateSafe More... | |
std::vector< Pose3, Eigen::aligned_allocator< Pose3 > > | cameraPosesTriangulation_ |
current triangulation poses More... | |
Protected Attributes inherited from gtsam::SmartFactorBase< CAMERA > | |
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< CAMERA > | Base |
typedef SmartProjectionFactor< CAMERA > | SmartProjectionCameraFactor |
typedef SmartProjectionFactor< CAMERA > | This |
Additional Inherited Members | |
Static Public Member Functions inherited from gtsam::SmartFactorBase< CAMERA > | |
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< CAMERA > | |
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< CAMERA > | |
static const int | Dim = traits<CAMERA>::dimension |
Camera dimension. More... | |
static const int | ZDim = traits<Z>::dimension |
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) |
SmartProjectionFactor: triangulates point and keeps an estimate of it around. This factor operates with monocular cameras, where a camera is expected to behave like PinholeCamera or PinholePose. This factor is intended to be used directly with PinholeCamera, which optimizes the camera pose and calibration. This also requires that values contains the involved cameras (instead of poses and calibrations separately). If the calibration is fixed use SmartProjectionPoseFactor instead!
Definition at line 44 of file SmartProjectionFactor.h.
|
private |
Definition at line 49 of file SmartProjectionFactor.h.
typedef CAMERA gtsam::SmartProjectionFactor< CAMERA >::Camera |
shorthand for a set of cameras
Definition at line 73 of file SmartProjectionFactor.h.
typedef CameraSet<CAMERA> gtsam::SmartProjectionFactor< CAMERA >::Cameras |
Definition at line 74 of file SmartProjectionFactor.h.
typedef std::shared_ptr<This> gtsam::SmartProjectionFactor< CAMERA >::shared_ptr |
shorthand for a smart pointer to a factor
Definition at line 70 of file SmartProjectionFactor.h.
|
private |
Definition at line 51 of file SmartProjectionFactor.h.
|
private |
Definition at line 50 of file SmartProjectionFactor.h.
|
inline |
Default constructor, only for serialization
Definition at line 79 of file SmartProjectionFactor.h.
|
inline |
Constructor
sharedNoiseModel | isotropic noise model for the 2D feature measurements |
params | parameters for the smart projection factors |
Definition at line 86 of file SmartProjectionFactor.h.
|
inlineoverride |
Virtual destructor
Definition at line 94 of file SmartProjectionFactor.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 357 of file SmartProjectionFactor.h.
|
inline |
Create a Hessianfactor that is an approximation of error(p).
Definition at line 198 of file SmartProjectionFactor.h.
|
inline |
Create JacobianFactorQ factor.
Definition at line 250 of file SmartProjectionFactor.h.
|
inline |
Create JacobianFactorQ factor, takes values.
Definition at line 260 of file SmartProjectionFactor.h.
|
inline |
Different (faster) way to compute a JacobianFactorSVD factor.
Definition at line 266 of file SmartProjectionFactor.h.
|
inline |
Definition at line 240 of file SmartProjectionFactor.h.
|
inline |
Check if the new linearization point is the same as the one used for previous triangulation.
cameras |
Definition at line 127 of file SmartProjectionFactor.h.
|
inlineoverridevirtual |
equals
Reimplemented from gtsam::SmartFactorBase< CAMERA >.
Reimplemented in gtsam::SmartProjectionPoseFactorRollingShutter< CAMERA >, and gtsam::SmartProjectionRigFactor< CAMERA >.
Definition at line 114 of file SmartProjectionFactor.h.
|
inlineoverridevirtual |
Calculate total reprojection error.
Reimplemented from gtsam::NonlinearFactor.
Reimplemented in gtsam::SmartProjectionPoseFactorRollingShutter< CAMERA >, and gtsam::SmartProjectionRigFactor< CAMERA >.
Definition at line 433 of file SmartProjectionFactor.h.
|
inline |
return the degenerate state
Definition at line 456 of file SmartProjectionFactor.h.
|
inline |
return the farPoint state
Definition at line 465 of file SmartProjectionFactor.h.
|
inline |
return the outlier state
Definition at line 462 of file SmartProjectionFactor.h.
|
inline |
return the cheirality status flag
Definition at line 459 of file SmartProjectionFactor.h.
|
inline |
Is result valid?
Definition at line 453 of file SmartProjectionFactor.h.
|
inlineoverridevirtual |
linearize
Implements gtsam::NonlinearFactor.
Reimplemented in gtsam::SmartProjectionPoseFactorRollingShutter< CAMERA >, and gtsam::SmartProjectionRigFactor< CAMERA >.
Definition at line 328 of file SmartProjectionFactor.h.
|
inline |
Linearize to Gaussian Factor
values | Values structure which must contain camera poses for this factor |
Definition at line 298 of file SmartProjectionFactor.h.
|
inline |
Linearize to Gaussian Factor
values | Values structure which must contain camera poses for this factor |
Definition at line 320 of file SmartProjectionFactor.h.
|
inlinevirtual |
Linearize to a Hessianfactor.
Definition at line 276 of file SmartProjectionFactor.h.
|
inlinevirtual |
Linearize to an Implicit Schur factor.
Definition at line 282 of file SmartProjectionFactor.h.
|
inlinevirtual |
Linearize to a JacobianfactorQ.
Definition at line 288 of file SmartProjectionFactor.h.
|
inline |
return the landmark
Definition at line 442 of file SmartProjectionFactor.h.
|
inline |
COMPUTE the landmark
Definition at line 447 of file SmartProjectionFactor.h.
|
inlineoverridevirtual |
s | optional string naming the factor |
keyFormatter | optional formatter useful for printing Symbols |
Reimplemented from gtsam::SmartFactorBase< CAMERA >.
Reimplemented in gtsam::SmartProjectionPoseFactorRollingShutter< CAMERA >, and gtsam::SmartProjectionRigFactor< CAMERA >.
Definition at line 102 of file SmartProjectionFactor.h.
|
inline |
Calculate vector of re-projection errors, before applying noise model.
Definition at line 396 of file SmartProjectionFactor.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 411 of file SmartProjectionFactor.h.
|
inline |
Triangulate and compute derivative of error with respect to point
Definition at line 337 of file SmartProjectionFactor.h.
|
inline |
Triangulate and compute derivative of error with respect to point
Definition at line 349 of file SmartProjectionFactor.h.
|
inline |
Version that takes values, and creates the point.
Definition at line 374 of file SmartProjectionFactor.h.
|
inline |
takes values
Definition at line 385 of file SmartProjectionFactor.h.
|
inline |
Possibly re-triangulate before calculating Jacobians.
cameras |
Definition at line 192 of file SmartProjectionFactor.h.
|
inline |
Call gtsam::triangulateSafe iff we need to re-triangulate.
cameras |
Definition at line 173 of file SmartProjectionFactor.h.
|
mutableprotected |
current triangulation poses
Definition at line 64 of file SmartProjectionFactor.h.
|
protected |
Definition at line 57 of file SmartProjectionFactor.h.
|
mutableprotected |
result from triangulateSafe
Definition at line 62 of file SmartProjectionFactor.h.