#include <SmartProjectionFactor.h>
Public Types | |
typedef CameraSet< CAMERA > | Cameras |
shorthand for a set of cameras More... | |
typedef boost::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 |
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, Dim > | MatrixZD |
Public Types inherited from gtsam::NonlinearFactor | |
typedef boost::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 (std::vector< typename Base::MatrixZD, Eigen::aligned_allocator< typename Base::MatrixZD > > &Fblocks, Matrix &E, Vector &b, const Cameras &cameras) const |
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< JacobianFactorQ< Base::Dim, 2 > > | createJacobianQFactor (const Cameras &cameras, double lambda) const |
create factor More... | |
boost::shared_ptr< JacobianFactorQ< Base::Dim, 2 > > | createJacobianQFactor (const Values &values, double lambda) const |
Create a factor, takes values. More... | |
boost::shared_ptr< JacobianFactor > | createJacobianSVDFactor (const Cameras &cameras, double lambda) const |
different (faster) way to compute Jacobian factor More... | |
boost::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... | |
boost::shared_ptr< GaussianFactor > | linearize (const Values &values) const override |
linearize More... | |
boost::shared_ptr< GaussianFactor > | linearizeDamped (const Cameras &cameras, const double lambda=0.0) const |
boost::shared_ptr< GaussianFactor > | linearizeDamped (const Values &values, const double lambda=0.0) const |
virtual boost::shared_ptr< RegularHessianFactor< Base::Dim > > | linearizeToHessian (const Values &values, double lambda=0.0) const |
linearize to a Hessianfactor More... | |
virtual boost::shared_ptr< RegularImplicitSchurFactor< CAMERA > > | linearizeToImplicit (const Values &values, double lambda=0.0) const |
linearize to an Implicit Schur factor More... | |
virtual boost::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, 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 (std::vector< typename Base::MatrixZD, Eigen::aligned_allocator< typename Base::MatrixZD > > &Fblocks, Matrix &E, Vector &b, const Values &values) const |
Version that takes values, and creates the point. More... | |
bool | triangulateAndComputeJacobiansSVD (std::vector< typename Base::MatrixZD, Eigen::aligned_allocator< typename Base::MatrixZD > > &Fblocks, 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... | |
~SmartProjectionFactor () override | |
Public Member Functions inherited from gtsam::SmartFactorBase< CAMERA > | |
void | add (const Z &measured, const Key &key) |
void | add (const ZVector &measurements, const KeyVector &cameraKeys) |
template<class SFM_TRACK > | |
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... | |
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 |
SVD version. More... | |
virtual void | correctForMissingMeasurements (const Cameras &cameras, Vector &ue, boost::optional< typename Cameras::FBlocks & > Fs=boost::none, boost::optional< Matrix & > E=boost::none) const |
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< JacobianFactor > | createJacobianSVDFactor (const Cameras &cameras, const Point3 &point, double lambda=0.0) const |
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. More... | |
size_t | dim () const override |
get the dimension (number of rows!) of the factor More... | |
const ZVector & | measured () const |
SmartFactorBase () | |
Default Constructor, for serialization. More... | |
SmartFactorBase (const SharedNoiseModel &sharedNoiseModel, boost::optional< Pose3 > body_P_sensor=boost::none, size_t expectedNumberCameras=10) | |
Constructor. More... | |
template<class POINT > | |
double | totalReprojectionError (const Cameras &cameras, const POINT &point) const |
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 |
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) | |
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 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... | |
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_ |
Protected Attributes inherited from gtsam::SmartFactorBase< CAMERA > | |
boost::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 |
Private Member Functions | |
template<class ARCHIVE > | |
void | serialize (ARCHIVE &ar, const unsigned int version) |
Friends | |
class | boost::serialization::access |
Serialization function. More... | |
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 (Matrix &E) |
Computes Point Covariance P from E. More... | |
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 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) | |
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) |
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 45 of file SmartProjectionFactor.h.
|
private |
Definition at line 50 of file SmartProjectionFactor.h.
typedef CameraSet<CAMERA> gtsam::SmartProjectionFactor< CAMERA >::Cameras |
shorthand for a set of cameras
Definition at line 73 of file SmartProjectionFactor.h.
typedef boost::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 52 of file SmartProjectionFactor.h.
|
private |
Definition at line 51 of file SmartProjectionFactor.h.
|
inline |
Default constructor, only for serialization
Definition at line 78 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 85 of file SmartProjectionFactor.h.
|
inlineoverride |
Virtual destructor
Definition at line 93 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 336 of file SmartProjectionFactor.h.
|
inline |
linearize returns a Hessianfactor that is an approximation of error(p)
Definition at line 176 of file SmartProjectionFactor.h.
|
inline |
create factor
Definition at line 230 of file SmartProjectionFactor.h.
|
inline |
Create a factor, takes values.
Definition at line 240 of file SmartProjectionFactor.h.
|
inline |
different (faster) way to compute Jacobian factor
Definition at line 246 of file SmartProjectionFactor.h.
|
inline |
Definition at line 220 of file SmartProjectionFactor.h.
|
inline |
Check if the new linearization point is the same as the one used for previous triangulation.
Definition at line 120 of file SmartProjectionFactor.h.
|
inlineoverridevirtual |
equals
Reimplemented from gtsam::SmartFactorBase< CAMERA >.
Definition at line 113 of file SmartProjectionFactor.h.
|
inlineoverridevirtual |
Calculate total reprojection error.
Implements gtsam::NonlinearFactor.
Definition at line 412 of file SmartProjectionFactor.h.
|
inline |
return the degenerate state
Definition at line 435 of file SmartProjectionFactor.h.
|
inline |
return the farPoint state
Definition at line 444 of file SmartProjectionFactor.h.
|
inline |
return the outlier state
Definition at line 441 of file SmartProjectionFactor.h.
|
inline |
return the cheirality status flag
Definition at line 438 of file SmartProjectionFactor.h.
|
inline |
Is result valid?
Definition at line 432 of file SmartProjectionFactor.h.
|
inlineoverridevirtual |
linearize
Implements gtsam::NonlinearFactor.
Definition at line 308 of file SmartProjectionFactor.h.
|
inline |
Linearize to Gaussian Factor
values | Values structure which must contain camera poses for this factor |
Definition at line 278 of file SmartProjectionFactor.h.
|
inline |
Linearize to Gaussian Factor
values | Values structure which must contain camera poses for this factor |
Definition at line 300 of file SmartProjectionFactor.h.
|
inlinevirtual |
linearize to a Hessianfactor
Definition at line 256 of file SmartProjectionFactor.h.
|
inlinevirtual |
linearize to an Implicit Schur factor
Definition at line 262 of file SmartProjectionFactor.h.
|
inlinevirtual |
linearize to a JacobianfactorQ
Definition at line 268 of file SmartProjectionFactor.h.
|
inline |
return the landmark
Definition at line 421 of file SmartProjectionFactor.h.
|
inline |
COMPUTE the landmark
Definition at line 426 of file SmartProjectionFactor.h.
|
inlineoverridevirtual |
s | optional string naming the factor |
keyFormatter | optional formatter useful for printing Symbols |
Reimplemented from gtsam::SmartFactorBase< CAMERA >.
Definition at line 101 of file SmartProjectionFactor.h.
|
inline |
Calculate vector of re-projection errors, before applying noise model.
Definition at line 375 of file SmartProjectionFactor.h.
|
inlineprivate |
Definition at line 451 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 390 of file SmartProjectionFactor.h.
|
inline |
Triangulate and compute derivative of error with respect to point
Definition at line 317 of file SmartProjectionFactor.h.
|
inline |
Triangulate and compute derivative of error with respect to point
Definition at line 328 of file SmartProjectionFactor.h.
|
inline |
Version that takes values, and creates the point.
Definition at line 353 of file SmartProjectionFactor.h.
|
inline |
takes values
Definition at line 364 of file SmartProjectionFactor.h.
|
inline |
triangulate
Definition at line 170 of file SmartProjectionFactor.h.
|
inline |
triangulateSafe
Definition at line 156 of file SmartProjectionFactor.h.
|
friend |
Serialization function.
Definition at line 449 of file SmartProjectionFactor.h.
|
mutableprotected |
current triangulation poses
Definition at line 64 of file SmartProjectionFactor.h.
|
protected |
Definition at line 58 of file SmartProjectionFactor.h.
|
mutableprotected |
result from triangulateSafe
Definition at line 63 of file SmartProjectionFactor.h.