#include <SmartProjectionPoseFactorRollingShutter.h>
Public Types | |
typedef CAMERA | Camera |
typedef CameraSet< CAMERA > | Cameras |
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... | |
Public Types inherited from gtsam::SmartProjectionFactor< CAMERA > | |
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 | add (const MEASUREMENT &measured, const Key &world_P_body_key1, const Key &world_P_body_key2, const double &alpha, const size_t &cameraId=0) |
void | add (const MEASUREMENTS &measurements, const std::vector< std::pair< Key, Key >> &world_P_body_key_pairs, const std::vector< double > &alphas, const FastVector< size_t > &cameraIds=FastVector< size_t >()) |
const std::vector< double > & | alphas () const |
return the interpolation factors alphas More... | |
const FastVector< size_t > & | cameraIds () const |
return the calibration object More... | |
const std::shared_ptr< Cameras > & | cameraRig () const |
return the calibration object More... | |
Base::Cameras | cameras (const Values &values) const override |
void | computeJacobiansWithTriangulatedPoint (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 |
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 |
SmartProjectionPoseFactorRollingShutter () | |
Default constructor, only for serialization. More... | |
SmartProjectionPoseFactorRollingShutter (const SharedNoiseModel &sharedNoiseModel, const std::shared_ptr< Cameras > &cameraRig, const SmartProjectionParams ¶ms=SmartProjectionParams()) | |
const std::vector< std::pair< Key, Key > > & | world_P_body_key_pairs () const |
Public Member Functions inherited from gtsam::SmartProjectionFactor< CAMERA > | |
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 | 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 |
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 |
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 > | |
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... | |
template<class SFM_TRACK > | |
void | add (const SFM_TRACK &trackToAdd) |
Pose3 | body_P_sensor () const |
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 |
virtual void | correctForMissingMeasurements (const Cameras &cameras, Vector &ue, typename Cameras::FBlocks *Fs=nullptr, Matrix *E=nullptr) const |
template<class ... OptArgs> | |
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< 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 > | |
Vector | unwhitenedError (const Cameras &cameras, const POINT &point, typename Cameras::FBlocks *Fs=nullptr, Matrix *E=nullptr) 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 |
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 () |
Static Public Attributes | |
static EIGEN_MAKE_ALIGNED_OPERATOR_NEW const int | DimBlock |
static const int | DimPose = 6 |
Pose3 dimension. More... | |
static const int | ZDim = 2 |
Measurement dimension (Point2) 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 Attributes | |
std::vector< double > | alphas_ |
FastVector< size_t > | cameraIds_ |
std::shared_ptr< typename Base::Cameras > | cameraRig_ |
std::vector< std::pair< Key, Key > > | world_P_body_key_pairs_ |
Protected Attributes inherited from gtsam::SmartProjectionFactor< CAMERA > | |
SmartProjectionParams | params_ |
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 SmartProjectionFactor< CAMERA > | Base |
typedef CAMERA::CalibrationType | CALIBRATION |
typedef CAMERA::Measurement | MEASUREMENT |
typedef CAMERA::MeasurementVector | MEASUREMENTS |
typedef SmartProjectionPoseFactorRollingShutter< 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... | |
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 two consecutive poses of the body assuming a rolling shutter model of the camera with given readout time. The factor requires that values contain (for each pixel observation) two consecutive camera poses from which the pixel observation pose can be interpolated.
Definition at line 45 of file SmartProjectionPoseFactorRollingShutter.h.
|
private |
Definition at line 48 of file SmartProjectionPoseFactorRollingShutter.h.
|
private |
Definition at line 50 of file SmartProjectionPoseFactorRollingShutter.h.
typedef CAMERA gtsam::SmartProjectionPoseFactorRollingShutter< CAMERA >::Camera |
Definition at line 84 of file SmartProjectionPoseFactorRollingShutter.h.
typedef CameraSet<CAMERA> gtsam::SmartProjectionPoseFactorRollingShutter< CAMERA >::Cameras |
Definition at line 85 of file SmartProjectionPoseFactorRollingShutter.h.
typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> > gtsam::SmartProjectionPoseFactorRollingShutter< CAMERA >::FBlocks |
Definition at line 82 of file SmartProjectionPoseFactorRollingShutter.h.
typedef Eigen::Matrix<double, ZDim, DimBlock> gtsam::SmartProjectionPoseFactorRollingShutter< CAMERA >::MatrixZD |
Definition at line 80 of file SmartProjectionPoseFactorRollingShutter.h.
|
private |
Definition at line 51 of file SmartProjectionPoseFactorRollingShutter.h.
|
private |
Definition at line 52 of file SmartProjectionPoseFactorRollingShutter.h.
typedef std::shared_ptr<This> gtsam::SmartProjectionPoseFactorRollingShutter< CAMERA >::shared_ptr |
shorthand for a smart pointer to a factor
Definition at line 88 of file SmartProjectionPoseFactorRollingShutter.h.
|
private |
Definition at line 49 of file SmartProjectionPoseFactorRollingShutter.h.
|
inline |
Default constructor, only for serialization.
Definition at line 91 of file SmartProjectionPoseFactorRollingShutter.h.
|
inline |
Constructor
Isotropic | measurement noise |
cameraRig | set of cameras (fixed poses wrt body and intrinsics) taking the measurements |
params | internal parameters of the smart factors |
Definition at line 100 of file SmartProjectionPoseFactorRollingShutter.h.
|
inline |
add a new measurement, with 2 pose keys, interpolation factor, and cameraId
measured | 2-dimensional location of the projection of a single landmark in a single view (the measurement), interpolated from the 2 poses |
world_P_body_key1 | key corresponding to the first body poses (time <= time pixel is acquired) |
world_P_body_key2 | key corresponding to the second body poses (time >= time pixel is acquired) |
alpha | interpolation factor in [0,1], such that if alpha = 0 the interpolated pose is the same as world_P_body_key1 |
cameraId | ID of the camera taking the measurement (default 0) |
Definition at line 128 of file SmartProjectionPoseFactorRollingShutter.h.
|
inline |
Variant of the previous "add" function in which we include multiple measurements
measurements | vector of the 2m dimensional location of the projection of a single landmark in the m views (the measurements) |
world_P_body_key_pairs | vector where the i-th element contains a pair of keys corresponding to the pair of poses from which the observation pose for the i0-th measurement can be interpolated |
alphas | vector of interpolation params (in [0,1]), one for each measurement (in the same order) |
cameraIds | IDs of the cameras taking each measurement (same order as the measurements) |
Definition at line 167 of file SmartProjectionPoseFactorRollingShutter.h.
|
inline |
return the interpolation factors alphas
Definition at line 201 of file SmartProjectionPoseFactorRollingShutter.h.
|
inline |
return the calibration object
Definition at line 207 of file SmartProjectionPoseFactorRollingShutter.h.
|
inline |
return the calibration object
Definition at line 204 of file SmartProjectionPoseFactorRollingShutter.h.
|
inlineoverridevirtual |
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< CAMERA >.
Definition at line 266 of file SmartProjectionPoseFactorRollingShutter.h.
|
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 307 of file SmartProjectionPoseFactorRollingShutter.h.
|
inline |
linearize and return a Hessianfactor that is an approximation of error(p)
Definition at line 358 of file SmartProjectionPoseFactorRollingShutter.h.
|
inlineoverridevirtual |
equals
Reimplemented from gtsam::SmartProjectionFactor< CAMERA >.
Definition at line 232 of file SmartProjectionPoseFactorRollingShutter.h.
|
inlineoverridevirtual |
error calculates the error of the factor.
Reimplemented from gtsam::SmartProjectionFactor< CAMERA >.
Definition at line 290 of file SmartProjectionPoseFactorRollingShutter.h.
|
inlineoverridevirtual |
linearize
Reimplemented from gtsam::SmartProjectionFactor< CAMERA >.
Definition at line 450 of file SmartProjectionPoseFactorRollingShutter.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 435 of file SmartProjectionPoseFactorRollingShutter.h.
|
inlineoverridevirtual |
s | optional string naming the factor |
keyFormatter | optional formatter useful for printing Symbols |
Reimplemented from gtsam::SmartProjectionFactor< CAMERA >.
Definition at line 214 of file SmartProjectionPoseFactorRollingShutter.h.
|
inline |
return (for each observation) the keys of the pair of poses from which we interpolate
Definition at line 196 of file SmartProjectionPoseFactorRollingShutter.h.
|
protected |
interpolation factor (one for each observation) to interpolate between pair of consecutive poses
Definition at line 61 of file SmartProjectionPoseFactorRollingShutter.h.
|
protected |
vector of camera Ids (one for each observation, in the same order), identifying which camera took the measurement
Definition at line 69 of file SmartProjectionPoseFactorRollingShutter.h.
|
protected |
one or more cameras taking observations (fixed poses wrt body + fixed intrinsics)
Definition at line 65 of file SmartProjectionPoseFactorRollingShutter.h.
|
static |
size of the variable stacking 2 poses from which the observation pose is interpolated
Definition at line 74 of file SmartProjectionPoseFactorRollingShutter.h.
|
static |
Pose3 dimension.
Definition at line 77 of file SmartProjectionPoseFactorRollingShutter.h.
|
protected |
The keys of the pose of the body (with respect to an external world frame): two consecutive poses for each observation
Definition at line 57 of file SmartProjectionPoseFactorRollingShutter.h.
|
static |
Measurement dimension (Point2)
Definition at line 78 of file SmartProjectionPoseFactorRollingShutter.h.