template<class CAMERA>
class gtsam::SmartProjectionRigFactor< CAMERA >
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 assumes that camera calibration is fixed (but each measurement can be taken by a different camera in the rig, hence can have a different extrinsic and intrinsic calibration). The factor only constrains poses (variable dimension is 6 for each pose). This factor requires that values contains the involved poses (Pose3). If all measurements share the same calibration (i.e., are from the same camera), use SmartProjectionPoseFactor instead! If the calibration should be optimized, as well, use SmartProjectionFactor instead!
|
| SmartProjectionRigFactor () |
| Default constructor, only for serialization.
|
|
| SmartProjectionRigFactor (const SharedNoiseModel &sharedNoiseModel, const boost::shared_ptr< Cameras > &cameraRig, const SmartProjectionParams ¶ms=SmartProjectionParams()) |
| Constructor.
|
|
| ~SmartProjectionRigFactor () override=default |
| Virtual destructor.
|
|
void | add (const MEASUREMENT &measured, const Key &poseKey, const size_t &cameraId=0) |
| add a new measurement, corresponding to an observation from pose "poseKey" and taken from the camera in the rig identified by "cameraId"
|
|
void | add (const MEASUREMENTS &measurements, const KeyVector &poseKeys, const FastVector< size_t > &cameraIds=FastVector< size_t >()) |
| Variant of the previous "add" function in which we include multiple measurements.
|
|
const KeyVector & | nonUniqueKeys () const |
| return (for each observation) the (possibly non unique) keys involved in the measurements
|
|
const boost::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 |
| print
|
|
bool | equals (const NonlinearFactor &p, double tol=1e-9) const override |
| equals
|
|
Base::Cameras | cameras (const Values &values) const override |
| Collect all cameras involved in this factor.
|
|
double | error (const Values &values) const override |
| error calculates the error of the factor.
|
|
void | computeJacobiansWithTriangulatedPoint (typename Base::FBlocks &Fs, Matrix &E, Vector &b, const Cameras &cameras) const |
| Compute jacobian F, E and error vector at a given linearization point.
|
|
boost::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)
|
|
boost::shared_ptr< GaussianFactor > | linearizeDamped (const Values &values, const double &lambda=0.0) const |
| Linearize to Gaussian Factor (possibly adding a damping factor Lambda for LM)
|
|
boost::shared_ptr< GaussianFactor > | linearize (const Values &values) const override |
| linearize
|
|
| SmartProjectionFactor () |
| Default constructor, only for serialization.
|
|
| SmartProjectionFactor (const SharedNoiseModel &sharedNoiseModel, const SmartProjectionParams ¶ms=SmartProjectionParams()) |
| Constructor.
|
|
| ~SmartProjectionFactor () override |
| Virtual destructor.
|
|
bool | decideIfTriangulate (const Cameras &cameras) const |
| Check if the new linearization point is the same as the one used for previous triangulation.
|
|
TriangulationResult | triangulateSafe (const Cameras &cameras) const |
| Call gtsam::triangulateSafe iff we need to re-triangulate.
|
|
bool | triangulateForLinearize (const Cameras &cameras) const |
| Possibly re-triangulate before calculating Jacobians.
|
|
boost::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).
|
|
boost::shared_ptr< RegularImplicitSchurFactor< CAMERA > > | createRegularImplicitSchurFactor (const Cameras &cameras, double lambda) const |
|
boost::shared_ptr< JacobianFactorQ< Base::Dim, 2 > > | createJacobianQFactor (const Cameras &cameras, double lambda) const |
| Create JacobianFactorQ factor.
|
|
boost::shared_ptr< JacobianFactorQ< Base::Dim, 2 > > | createJacobianQFactor (const Values &values, double lambda) const |
| Create JacobianFactorQ factor, takes values.
|
|
boost::shared_ptr< JacobianFactor > | createJacobianSVDFactor (const Cameras &cameras, double lambda) const |
| Different (faster) way to compute a JacobianFactorSVD factor.
|
|
virtual boost::shared_ptr< RegularHessianFactor< Base::Dim > > | linearizeToHessian (const Values &values, double lambda=0.0) const |
| Linearize to a Hessianfactor.
|
|
virtual boost::shared_ptr< RegularImplicitSchurFactor< CAMERA > > | linearizeToImplicit (const Values &values, double lambda=0.0) const |
| Linearize to an Implicit Schur factor.
|
|
virtual boost::shared_ptr< JacobianFactorQ< Base::Dim, 2 > > | linearizeToJacobian (const Values &values, double lambda=0.0) const |
| Linearize to a JacobianfactorQ.
|
|
boost::shared_ptr< GaussianFactor > | linearizeDamped (const Cameras &cameras, const double lambda=0.0) const |
| Linearize to Gaussian Factor.
|
|
boost::shared_ptr< GaussianFactor > | linearizeDamped (const Values &values, const double lambda=0.0) const |
| Linearize to Gaussian Factor.
|
|
bool | triangulateAndComputeE (Matrix &E, const Cameras &cameras) const |
| Triangulate and compute derivative of error with respect to point.
|
|
bool | triangulateAndComputeE (Matrix &E, const Values &values) const |
| Triangulate and compute derivative of error with respect to point.
|
|
void | computeJacobiansWithTriangulatedPoint (typename Base::FBlocks &Fs, Matrix &E, Vector &b, const Cameras &cameras) const |
| 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.
|
|
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, boost::optional< Point3 > externalPoint=boost::none) const |
| Calculate the error of the factor.
|
|
TriangulationResult | point () const |
| return the landmark
|
|
TriangulationResult | point (const Values &values) const |
| COMPUTE the landmark.
|
|
bool | isValid () const |
| Is result valid?
|
|
bool | isDegenerate () const |
| return the degenerate state
|
|
bool | isPointBehindCamera () const |
| return the cheirality status flag
|
|
bool | isOutlier () const |
| return the outlier state
|
|
bool | isFarPoint () const |
| return the farPoint state
|
|
| SmartFactorBase () |
| Default Constructor, for serialization.
|
|
| SmartFactorBase (const SharedNoiseModel &sharedNoiseModel, boost::optional< Pose3 > body_P_sensor=boost::none, size_t expectedNumberCameras=10) |
| Construct with given noise model and optional arguments.
|
|
| ~SmartFactorBase () override |
| Virtual destructor, subclasses from NonlinearFactor.
|
|
void | add (const Z &measured, const Key &key) |
| Add a new measurement and pose/camera 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) |
| Add an entire SfM_track (collection of cameras observing a single point).
|
|
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, boost::optional< typename Cameras::FBlocks & > Fs=boost::none, boost::optional< Matrix & > E=boost::none) const |
| Compute reprojection errors [h(x)-z] = [cameras.project(p)-z] and derivatives.
|
|
virtual void | correctForMissingMeasurements (const Cameras &cameras, Vector &ue, boost::optional< typename Cameras::FBlocks & > Fs=boost::none, boost::optional< Matrix & > E=boost::none) const |
| This corrects the Jacobians for the case in which some 2D measurement is missing (nan).
|
|
template<class POINT > |
Vector | whitenedError (const Cameras &cameras, const POINT &point) const |
| Calculate vector of re-projection errors [h(x)-z] = [cameras.project(p) - z], with the noise model applied.
|
|
template<class POINT > |
double | totalReprojectionError (const Cameras &cameras, const POINT &point) const |
| Calculate the error of the factor.
|
|
template<class POINT > |
void | computeJacobians (FBlocks &Fs, Matrix &E, Vector &b, const Cameras &cameras, const POINT &point) const |
| 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.
|
|
template<class POINT > |
void | computeJacobiansSVD (FBlocks &Fs, Matrix &Enull, Vector &b, const Cameras &cameras, const POINT &point) const |
| SVD version that produces smaller Jacobian matrices by doing an SVD decomposition on E, and returning the left nulkl-space of E.
|
|
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.
|
|
void | updateAugmentedHessian (const Cameras &cameras, const Point3 &point, const double lambda, bool diagonalDamping, SymmetricBlockMatrix &augmentedHessian, const KeyVector allKeys) const |
| Add the contribution of the smart factor to a pre-allocated Hessian, using sparse linear algebra.
|
|
void | whitenJacobians (FBlocks &F, Matrix &E, Vector &b) const |
| Whiten the Jacobians computed by computeJacobians using noiseModel_.
|
|
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.
|
|
boost::shared_ptr< JacobianFactorQ< Dim, ZDim > > | createJacobianQFactor (const Cameras &cameras, const Point3 &point, double lambda=0.0, bool diagonalDamping=false) const |
| Return Jacobians as JacobianFactorQ.
|
|
boost::shared_ptr< JacobianFactor > | createJacobianSVDFactor (const Cameras &cameras, const Point3 &point, double lambda=0.0) const |
| Return Jacobians as JacobianFactorSVD.
|
|
Pose3 | body_P_sensor () const |
|
| NonlinearFactor () |
| Default constructor for I/O only.
|
|
template<typename CONTAINER > |
| NonlinearFactor (const CONTAINER &keys) |
| Constructor from a collection of the keys involved in this factor.
|
|
virtual | ~NonlinearFactor () |
| Destructor.
|
|
double | error (const HybridValues &c) const override |
| All factor types need to implement an error function.
|
|
virtual bool | active (const Values &) const |
| Checks whether a factor should be used based on a set of values.
|
|
virtual shared_ptr | clone () const |
| Creates a shared_ptr clone of the factor - needs to be specialized to allow for subclasses.
|
|
virtual shared_ptr | rekey (const std::map< Key, Key > &rekey_mapping) const |
| Creates a shared_ptr clone of the factor with different keys using a map from old->new keys.
|
|
virtual shared_ptr | rekey (const KeyVector &new_keys) const |
| Clones a factor and fully replaces its keys.
|
|
virtual bool | sendable () const |
| 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)
|
|
virtual | ~Factor ()=default |
| Default destructor.
|
|
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 |
| Iterator at beginning of involved variable keys.
|
|
const_iterator | end () const |
| Iterator at end of involved variable keys.
|
|
size_t | size () const |
|
virtual void | printKeys (const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const |
| print only keys
|
|
bool | equals (const This &other, double tol=1e-9) const |
| check equality
|
|
KeyVector & | keys () |
|
iterator | begin () |
| Iterator at beginning of involved variable keys.
|
|
iterator | end () |
| Iterator at end of involved variable keys.
|
|