GTSAM
4.0.2
C++ library for smoothing and mapping (SAM)
|
#include <SmartProjectionPoseFactorRollingShutter.h>
Public Types | |
typedef Eigen::Matrix< double, ZDim, DimBlock > | MatrixZD |
typedef std::vector< MatrixZD, Eigen::aligned_allocator< MatrixZD > > | FBlocks |
typedef CAMERA | Camera |
typedef CameraSet< CAMERA > | Cameras |
typedef std::shared_ptr< This > | shared_ptr |
shorthand for a smart pointer to a factor | |
typedef KeyVector::iterator | iterator |
Iterator over keys. | |
typedef KeyVector::const_iterator | const_iterator |
Const iterator over keys. | |
Public Member Functions | |
SmartProjectionPoseFactorRollingShutter () | |
Default constructor, only for serialization. | |
SmartProjectionPoseFactorRollingShutter (const SharedNoiseModel &sharedNoiseModel, const std::shared_ptr< Cameras > &cameraRig, const SmartProjectionParams ¶ms=SmartProjectionParams()) | |
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< std::pair< Key, Key > > & | world_P_body_key_pairs () const |
const std::vector< double > & | alphas () const |
return the interpolation factors alphas | |
const std::shared_ptr< Cameras > & | cameraRig () const |
return the calibration object | |
const FastVector< size_t > & | cameraIds () const |
return the calibration object | |
void | print (const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override |
bool | equals (const NonlinearFactor &p, double tol=1e-9) const override |
equals | |
Base::Cameras | cameras (const Values &values) const override |
double | error (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) | |
std::shared_ptr< GaussianFactor > | linearizeDamped (const Values &values, const double &lambda=0.0) const |
std::shared_ptr< GaussianFactor > | linearize (const Values &values) const override |
linearize | |
bool | decideIfTriangulate (const Cameras &cameras) const |
Check if the new linearization point is the same as the one used for previous triangulation. More... | |
TriangulationResult | triangulateSafe (const Cameras &cameras) const |
Call gtsam::triangulateSafe iff we need to re-triangulate. More... | |
bool | triangulateForLinearize (const Cameras &cameras) const |
Possibly re-triangulate before calculating Jacobians. More... | |
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). | |
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. | |
std::shared_ptr< RegularImplicitSchurFactor< CAMERA > > | createRegularImplicitSchurFactor (const Cameras &cameras, double lambda) 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. | |
std::shared_ptr< JacobianFactorQ< Base::Dim, 2 > > | createJacobianQFactor (const Cameras &cameras, double lambda) const |
Create JacobianFactorQ factor. | |
std::shared_ptr< JacobianFactorQ< Base::Dim, 2 > > | createJacobianQFactor (const Values &values, double lambda) const |
Create JacobianFactorQ factor, takes values. | |
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. | |
std::shared_ptr< JacobianFactor > | createJacobianSVDFactor (const Cameras &cameras, double lambda) const |
Different (faster) way to compute a JacobianFactorSVD factor. | |
std::shared_ptr< JacobianFactor > | createJacobianSVDFactor (const Cameras &cameras, const Point3 &point, 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. | |
virtual std::shared_ptr< RegularImplicitSchurFactor< CAMERA > > | linearizeToImplicit (const Values &values, double lambda=0.0) const |
Linearize to an Implicit Schur factor. | |
virtual std::shared_ptr< JacobianFactorQ< Base::Dim, 2 > > | linearizeToJacobian (const Values &values, double lambda=0.0) const |
Linearize to a JacobianfactorQ. | |
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 |
bool | triangulateAndComputeE (Matrix &E, const Cameras &cameras) const |
bool | triangulateAndComputeE (Matrix &E, const Values &values) const |
void | computeJacobiansWithTriangulatedPoint (typename Base::FBlocks &Fs, Matrix &E, Vector &b, const Cameras &cameras) const |
bool | triangulateAndComputeJacobians (typename Base::FBlocks &Fs, Matrix &E, Vector &b, const Values &values) const |
Version that takes values, and creates the point. | |
bool | triangulateAndComputeJacobiansSVD (typename Base::FBlocks &Fs, Matrix &Enull, Vector &b, const Values &values) const |
takes values | |
Vector | reprojectionErrorAfterTriangulation (const Values &values) const |
Calculate vector of re-projection errors, before applying noise model. | |
double | totalReprojectionError (const Cameras &cameras, std::optional< Point3 > externalPoint={}) const |
template<class POINT > | |
double | totalReprojectionError (const Cameras &cameras, const POINT &point) const |
TriangulationResult | point () const |
TriangulationResult | point (const Values &values) const |
bool | isValid () const |
Is result valid? | |
bool | isDegenerate () const |
bool | isPointBehindCamera () const |
bool | isOutlier () const |
bool | isFarPoint () const |
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. | |
template<class SFM_TRACK > | |
void | add (const SFM_TRACK &trackToAdd) |
size_t | dim () const override |
Return the dimension (number of rows!) of the factor. | |
const ZVector & | measured () const |
Return the 2D measurements (ZDim, in general). | |
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 |
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 |
template<class POINT > | |
Vector | whitenedError (const Cameras &cameras, const POINT &point) 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 |
void | updateAugmentedHessian (const Cameras &cameras, const Point3 &point, const double lambda, bool diagonalDamping, SymmetricBlockMatrix &augmentedHessian, const KeyVector allKeys) const |
void | whitenJacobians (FBlocks &F, Matrix &E, Vector &b) const |
Whiten the Jacobians computed by computeJacobians using noiseModel_. | |
Pose3 | body_P_sensor () const |
Testable | |
bool | equals (const This &other, double tol=1e-9) const |
check equality | |
virtual void | printKeys (const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const |
print only keys | |
Standard Interface | |
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 |
Standard Interface | |
bool | empty () const |
Whether the factor is empty (involves zero variables). | |
Key | front () const |
First key. | |
Key | back () const |
Last key. | |
const_iterator | find (Key key) const |
find | |
const KeyVector & | keys () const |
Access the factor's involved variable keys. | |
const_iterator | begin () const |
const_iterator | end () const |
size_t | size () const |
Advanced Interface | |
KeyVector & | keys () |
iterator | begin () |
iterator | end () |
Static Public Member Functions | |
static Matrix | PointCov (const Matrix &E) |
Computes Point Covariance P from the "point Jacobian" E. | |
static void | FillDiagonalF (const FBlocks &Fs, Matrix &F) |
Create BIG block-diagonal matrix F from Fblocks. | |
Static Public Attributes | |
static EIGEN_MAKE_ALIGNED_OPERATOR_NEW const int | DimBlock |
static const int | DimPose = 6 |
Pose3 dimension. | |
static const int | ZDim = 2 |
Measurement dimension (Point2) | |
static const int | Dim = traits<CAMERA>::dimension |
Camera dimension. | |
Static Protected Member Functions | |
Standard Constructors | |
template<typename CONTAINER > | |
static Factor | FromKeys (const CONTAINER &keys) |
template<typename ITERATOR > | |
static Factor | FromIterators (ITERATOR first, ITERATOR last) |
Protected Attributes | |
std::vector< std::pair< Key, Key > > | world_P_body_key_pairs_ |
std::vector< double > | alphas_ |
std::shared_ptr< typename Base::Cameras > | cameraRig_ |
FastVector< size_t > | cameraIds_ |
SharedIsotropic | noiseModel_ |
ZVector | measured_ |
std::optional< Pose3 > | body_P_sensor_ |
Pose of the camera in the body frame. | |
FBlocks | Fs |
KeyVector | keys_ |
The keys involved in this factor. | |
Parameters | |
SmartProjectionParams | params_ |
Caching triangulation | |
TriangulationResult | result_ |
result from triangulateSafe | |
std::vector< Pose3, Eigen::aligned_allocator< Pose3 > > | cameraPosesTriangulation_ |
current triangulation poses | |
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.
|
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 |
|
inlinevirtualinherited |
Checks whether a factor should be used based on a set of values. This is primarily used to implement inequality constraints that require a variable active set. For all others, the default implementation returning true solves this problem.
In an inequality/bounding constraint, this active() returns true when the constraint is NOT fulfilled.
Reimplemented in gtsam::BoundingConstraint2< VALUE1, VALUE2 >, gtsam::AntiFactor, and gtsam::BoundingConstraint1< VALUE >.
|
inlineinherited |
Add a new measurement and pose/camera key.
measured | is the 2m dimensional projection of a single landmark |
key | is the index corresponding to the camera observing the landmark |
|
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) |
|
inlineinherited |
Add an entire SfM_track (collection of cameras observing a single point). The noise is assumed to be the same for all measurements.
|
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) |
|
inlineinherited |
Iterator at beginning of involved variable keys
|
inlineinherited |
Iterator at beginning of involved variable keys
|
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 >.
|
inlinevirtualinherited |
Creates a shared_ptr clone of the factor - needs to be specialized to allow for subclasses
By default, throws exception if subclass does not implement the function.
Reimplemented in gtsam::EssentialMatrixFactor4< CALIBRATION >, gtsam::NonlinearEquality2< T >, gtsam::CombinedImuFactor, gtsam::ImuFactor2, gtsam::EssentialMatrixFactor3, gtsam::NonlinearEquality1< VALUE >, gtsam::GeneralSFMFactor2< CALIBRATION >, gtsam::ImuFactor, gtsam::LinearizedHessianFactor, gtsam::PendulumFactorPk1, gtsam::FunctorizedFactor2< R, T1, T2 >, gtsam::MagFactor3, gtsam::Pose3AttitudeFactor, gtsam::SmartRangeFactor, gtsam::NonlinearEquality< VALUE >, gtsam::AHRSFactor, gtsam::EssentialMatrixFactor2, gtsam::ExpressionFactor< T >, gtsam::ExpressionFactor< BearingRange< A1, A2 > >, gtsam::ExpressionFactor< double >, gtsam::MagFactor2, gtsam::PendulumFactorPk, gtsam::GPSFactor2, gtsam::ProjectionFactorRollingShutter, gtsam::RangeFactorWithTransform< A1, A2, T >, gtsam::LinearContainerFactor, gtsam::LinearizedJacobianFactor, gtsam::Rot3AttitudeFactor, gtsam::MagFactor1, gtsam::GenericProjectionFactor< POSE, LANDMARK, CALIBRATION >, gtsam::DiscreteEulerPoincareHelicopter, gtsam::MultiProjectionFactor< POSE, LANDMARK, CALIBRATION >, gtsam::GeneralSFMFactor< CAMERA, LANDMARK >, gtsam::ProjectionFactorPPP< POSE, LANDMARK, CALIBRATION >, gtsam::TransformBtwRobotsUnaryFactorEM< VALUE >, gtsam::GenericStereoFactor< POSE, LANDMARK >, gtsam::TriangulationFactor< CAMERA >, gtsam::RotateDirectionsFactor, gtsam::PendulumFactor2, gtsam::ReferenceFrameFactor< POINT, TRANSFORM >, gtsam::FunctorizedFactor< R, T >, gtsam::PartialPriorFactor< VALUE >, gtsam::FunctorizedFactor< Vector, ParameterMatrix< M > >, gtsam::FunctorizedFactor< double, BASIS::Parameters >, gtsam::FunctorizedFactor< double, Vector >, gtsam::FunctorizedFactor< T, ParameterMatrix< traits< T >::dimension > >, gtsam::FunctorizedFactor< double, ParameterMatrix< P > >, gtsam::PartialPriorFactor< PoseRTV >, gtsam::MagPoseFactor< POSE >, gtsam::ProjectionFactorPPPC< POSE, LANDMARK, CALIBRATION >, gtsam::TransformBtwRobotsUnaryFactor< VALUE >, gtsam::BetweenFactor< VALUE >, gtsam::EssentialMatrixFactor, gtsam::VelocityConstraint, gtsam::PriorFactor< VALUE >, gtsam::GPSFactor, gtsam::PoseToPointFactor< POSE, POINT >, gtsam::BarometricFactor, gtsam::EssentialMatrixConstraint, gtsam::DummyFactor, gtsam::PoseBetweenFactor< POSE >, gtsam::BearingRangeFactor< A1, A2, B, R >, gtsam::PosePriorFactor< POSE >, gtsam::MagFactor, gtsam::PoseTranslationPrior< POSE >, gtsam::PoseRotationPrior< POSE >, gtsam::FullIMUFactor< POSE >, gtsam::AntiFactor, gtsam::RangeFactor< A1, A2, T >, gtsam::IMUFactor< POSE >, gtsam::RelativeElevationFactor, gtsam::PendulumFactor1, gtsam::Reconstruction, gtsam::RotateFactor, and gtsam::VelocityConstraint3.
|
inlineinherited |
Compute F, E, and b (called below in both vanilla and SVD versions), where F is a vector of derivatives wrpt the cameras, and E the stacked derivatives with respect to the point. The value of cameras/point are passed as parameters.
|
inlineinherited |
SVD version that produces smaller Jacobian matrices by doing an SVD decomposition on E, and returning the left nulkl-space of E. See JacobianFactorSVD for more documentation.
|
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 |
|
inlineinherited |
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
|
inlinevirtualinherited |
This corrects the Jacobians for the case in which some 2D measurement is missing (nan). In practice, this does not do anything in the monocular case, but it is implemented in the stereo version.
Reimplemented in gtsam::SmartStereoProjectionFactor.
|
inlineinherited |
An overload of correctForMissingMeasurements. This allows end users to provide optional arguments that are l-value references to the matrices and vectors that will be used to store the results instead of pointers.
|
inlineinherited |
Return Jacobians as JacobianFactorSVD. TODO(dellaert): lambda is currently ignored
|
inlineinherited |
Check if the new linearization point is the same as the one used for previous triangulation.
cameras |
|
inlineinherited |
Iterator at end of involved variable keys
|
inlineinherited |
Iterator at end of involved variable keys
|
overridevirtualinherited |
All factor types need to implement an error function. In factor graphs, this is the negative log-likelihood.
Reimplemented from gtsam::Factor.
|
inlineoverridevirtual |
error calculates the error of the factor.
Reimplemented from gtsam::SmartProjectionFactor< CAMERA >.
|
inlinestaticprotectedinherited |
Construct factor from iterator keys. This is called internally from derived factor static factor methods, as a workaround for not being able to call the protected constructors above.
|
inlinestaticprotectedinherited |
Construct factor from container of keys. This is called internally from derived factor static factor methods, as a workaround for not being able to call the protected constructors above.
|
inlineinherited |
return the degenerate state
|
inlineinherited |
return the farPoint state
|
inlineinherited |
return the outlier state
|
inlineinherited |
return the cheirality status flag
|
inlineinherited |
|
inlineinherited |
|
inlineinherited |
|
inline |
|
inlineinherited |
return the landmark
|
inlineinherited |
COMPUTE the landmark
|
inlineoverridevirtual |
s | optional string naming the factor |
keyFormatter | optional formatter useful for printing Symbols |
Reimplemented from gtsam::SmartProjectionFactor< CAMERA >.
|
virtualinherited |
Creates a shared_ptr clone of the factor with different keys using a map from old->new keys
Reimplemented in gtsam::LinearContainerFactor.
|
virtualinherited |
Clones a factor and fully replaces its keys
new_keys | is the full replacement set of keys |
Reimplemented in gtsam::LinearContainerFactor.
|
inlinevirtualinherited |
Should the factor be evaluated in the same thread as the caller This is to enable factors that has shared states (like the Python GIL lock)
Reimplemented in gtsam::CustomFactor.
|
inlineinherited |
|
inlineinherited |
Calculate the error of the factor. This is the log-likelihood, e.g. \( 0.5(h(x)-z)^2/\sigma^2 \) in case of Gaussian. In this class, we take the raw prediction error \( h(x)-z \), ask the noise model to transform it to \( (h(x)-z)^2/\sigma^2 \), and then multiply by 0.5. Will be used in "error(Values)" function required by NonlinearFactor base class
|
inlineinherited |
Calculate the error of the factor. This is the log-likelihood, e.g. \( 0.5(h(x)-z)^2/\sigma^2 \) in case of Gaussian. In this class, we take the raw prediction error \( h(x)-z \), ask the noise model to transform it to \( (h(x)-z)^2/\sigma^2 \), and then multiply by 0.5.
|
inlineinherited |
Triangulate and compute derivative of error with respect to point
|
inlineinherited |
Triangulate and compute derivative of error with respect to point
|
inlineinherited |
Possibly re-triangulate before calculating Jacobians.
cameras |
|
inlineinherited |
Call gtsam::triangulateSafe iff we need to re-triangulate.
cameras |
|
inlineinherited |
Compute reprojection errors [h(x)-z] = [cameras.project(p)-z] and derivatives. This is the error before the noise model is applied. The templated version described above must finally get resolved to this function.
|
inlineinherited |
An overload of unwhitenedError. This allows end users to provide optional arguments that are l-value references to the matrices and vectors that will be used to store the results instead of pointers.
|
inlineinherited |
Add the contribution of the smart factor to a pre-allocated Hessian, using sparse linear algebra. More efficient than the creation of the Hessian without preallocation of the SymmetricBlockMatrix
|
inlineinherited |
Calculate vector of re-projection errors [h(x)-z] = [cameras.project(p) - z], with the noise model applied.
|
inline |
return (for each observation) the keys of the pair of poses from which we interpolate
|
protected |
interpolation factor (one for each observation) to interpolate between pair of consecutive poses
|
protected |
vector of camera Ids (one for each observation, in the same order), identifying which camera took the measurement
|
protected |
one or more cameras taking observations (fixed poses wrt body + fixed intrinsics)
|
static |
size of the variable stacking 2 poses from which the observation pose is interpolated
|
protectedinherited |
Measurements for each of the m views. We keep a copy of the measurements for I/O and computing the error. The order is kept the same as the keys that we use to create the factor.
|
protectedinherited |
As of Feb 22, 2015, the noise model is the same for all measurements and is isotropic. This allows for moving most calculations of Schur complement etc. to be easily moved to CameraSet, and also agrees pragmatically with what is normally done.
|
protected |
The keys of the pose of the body (with respect to an external world frame): two consecutive poses for each observation