#include <SmartStereoProjectionFactorPP.h>
Public Types | |
typedef std::vector< MatrixZD, Eigen::aligned_allocator< MatrixZD > > | FBlocks |
typedef Eigen::Matrix< double, ZDim, DimBlock > | MatrixZD |
typedef std::shared_ptr< This > | shared_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< 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 | 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) |
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 StereoPoint2 &measured, const Key &world_P_body_key, const Key &body_P_cam_key, 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 KeyVector & | getExtrinsicPoseKeys () const |
equals More... | |
std::shared_ptr< GaussianFactor > | linearize (const Values &values) const override |
linearize More... | |
std::shared_ptr< GaussianFactor > | linearizeDamped (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 ¶ms=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< 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 | isDegenerate () const |
bool | isFarPoint () const |
bool | isOutlier () const |
bool | isPointBehindCamera () const |
bool | isValid () const |
Is result valid? 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 |
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 |
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 () |
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< This > | shared_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< 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... | |
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) |
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.
typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> > gtsam::SmartStereoProjectionFactorPP::FBlocks |
Definition at line 71 of file SmartStereoProjectionFactorPP.h.
typedef Eigen::Matrix<double, ZDim, DimBlock> gtsam::SmartStereoProjectionFactorPP::MatrixZD |
Definition at line 70 of file SmartStereoProjectionFactorPP.h.
typedef std::shared_ptr<This> gtsam::SmartStereoProjectionFactorPP::shared_ptr |
shorthand for a smart pointer to a factor
Definition at line 65 of file SmartStereoProjectionFactorPP.h.
shorthand for this class
Definition at line 62 of file SmartStereoProjectionFactorPP.h.
gtsam::SmartStereoProjectionFactorPP::SmartStereoProjectionFactorPP | ( | const SharedNoiseModel & | sharedNoiseModel, |
const SmartStereoProjectionParams & | params = SmartStereoProjectionParams() |
||
) |
Constructor
Isotropic | measurement noise |
params | internal parameters of the smart factors |
Definition at line 25 of file SmartStereoProjectionFactorPP.cpp.
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
measurements | vector of the 3m dimensional location of the projection of a single landmark in the m (stereo) view (the measurements) |
w_P_body_keys | are the ordered keys corresponding to the body poses observing the same landmark |
body_P_cam_keys | are 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) |
K | the (known) camera calibration (same for all measurements) |
Definition at line 67 of file SmartStereoProjectionFactorPP.cpp.
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
measurements | vector of the 3m dimensional location of the projection of a single landmark in the m (stereo) view (the measurements) |
w_P_body_keys | are the ordered keys corresponding to the body poses observing the same landmark |
body_P_cam_keys | are 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) |
Ks | vector of intrinsic calibration objects |
Definition at line 47 of file SmartStereoProjectionFactorPP.cpp.
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
measured | is the 3-dimensional location of the projection of a single landmark in the a single (stereo) view (the measurement) |
world_P_body_key | is the key corresponding to the body poses observing the same landmark |
body_P_cam_key | is the key corresponding to the extrinsic camera-to-body pose calibration |
K | is the (fixed) camera intrinsic calibration |
Definition at line 30 of file SmartStereoProjectionFactorPP.cpp.
|
inline |
return the calibration object
Definition at line 143 of file SmartStereoProjectionFactorPP.h.
|
overridevirtual |
Collect all cameras involved in this factor
values | Values structure which must contain camera poses corresponding to keys involved in this factor |
Reimplemented from gtsam::SmartFactorBase< StereoCamera >.
Definition at line 114 of file SmartStereoProjectionFactorPP.cpp.
|
inline |
Compute jacobian F, E and error vector at a given linearization point
values | Values structure which must contain camera poses corresponding to keys involved in this factor |
Definition at line 164 of file SmartStereoProjectionFactorPP.h.
|
inline |
linearize and return a Hessianfactor that is an approximation of error(p)
Definition at line 204 of file SmartStereoProjectionFactorPP.h.
|
overridevirtual |
equals
Reimplemented from gtsam::SmartStereoProjectionFactor.
Definition at line 96 of file SmartStereoProjectionFactorPP.cpp.
|
overridevirtual |
error calculates the error of the factor.
Reimplemented from gtsam::SmartStereoProjectionFactor.
Definition at line 105 of file SmartStereoProjectionFactorPP.cpp.
|
inline |
equals
Definition at line 133 of file SmartStereoProjectionFactorPP.h.
|
inlineoverridevirtual |
linearize
Reimplemented from gtsam::SmartStereoProjectionFactor.
Definition at line 282 of file SmartStereoProjectionFactorPP.h.
|
inline |
Linearize to Gaussian Factor (possibly adding a damping factor Lambda for LM)
values | Values structure which must contain camera poses and extrinsic pose for this factor |
Definition at line 269 of file SmartStereoProjectionFactorPP.h.
|
overridevirtual |
s | optional string naming the factor |
keyFormatter | optional formatter useful for printing Symbols |
Reimplemented from gtsam::SmartStereoProjectionFactor.
Definition at line 86 of file SmartStereoProjectionFactorPP.cpp.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef SmartStereoProjectionFactor gtsam::SmartStereoProjectionFactorPP::Base |
shorthand for base class type
Definition at line 59 of file SmartStereoProjectionFactorPP.h.
|
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.
|
static |
Camera dimension: 6 for body pose, 6 for extrinsic pose.
Definition at line 67 of file SmartStereoProjectionFactorPP.h.
|
static |
Pose3 dimension.
Definition at line 68 of file SmartStereoProjectionFactorPP.h.
|
protected |
shared pointer to calibration object (one for each camera)
Definition at line 47 of file SmartStereoProjectionFactorPP.h.
|
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.
|
static |
Measurement dimension (for a StereoPoint2 measurement)
Definition at line 69 of file SmartStereoProjectionFactorPP.h.