template<class CALIBRATION>
class gtsam::SmartProjectionPoseFactor< CALIBRATION >
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, and that the calibration is the same for all cameras involved in this factor. The factor only constrains poses (variable dimension is 6). This factor requires that values contains the involved poses (Pose3). If the calibration should be optimized, as well, use SmartProjectionFactor instead!
|
| SmartProjectionPoseFactor () |
| Default constructor, only for serialization.
|
|
| SmartProjectionPoseFactor (const SharedNoiseModel &sharedNoiseModel, const boost::shared_ptr< CALIBRATION > K, const SmartProjectionParams ¶ms=SmartProjectionParams()) |
| Constructor. More...
|
|
| SmartProjectionPoseFactor (const SharedNoiseModel &sharedNoiseModel, const boost::shared_ptr< CALIBRATION > K, const boost::optional< Pose3 > body_P_sensor, const SmartProjectionParams ¶ms=SmartProjectionParams()) |
| Constructor. More...
|
|
| ~SmartProjectionPoseFactor () override |
| Virtual destructor.
|
|
void | print (const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override |
| print More...
|
|
bool | equals (const NonlinearFactor &p, double tol=1e-9) const override |
| equals More...
|
|
double | error (const Values &values) const override |
| error calculates the error of the factor. More...
|
|
const boost::shared_ptr< CALIBRATION > | calibration () const |
| return calibration shared pointers
|
|
Base::Cameras | cameras (const Values &values) const override |
| Collect all cameras involved in this factor. More...
|
|
| SmartProjectionFactor () |
| Default constructor, only for serialization.
|
|
| SmartProjectionFactor (const SharedNoiseModel &sharedNoiseModel, const SmartProjectionParams ¶ms=SmartProjectionParams()) |
| Constructor. More...
|
|
| ~SmartProjectionFactor () override |
| Virtual destructor.
|
|
void | print (const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override |
| print More...
|
|
bool | equals (const NonlinearFactor &p, double tol=1e-9) const override |
| equals More...
|
|
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...
|
|
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< PinholePose< CALIBRATION > > > | 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< PinholePose< CALIBRATION > > > | 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. More...
|
|
boost::shared_ptr< GaussianFactor > | linearizeDamped (const Values &values, const double lambda=0.0) const |
| Linearize to Gaussian Factor. More...
|
|
boost::shared_ptr< GaussianFactor > | linearize (const Values &values) const override |
| linearize More...
|
|
bool | triangulateAndComputeE (Matrix &E, const Cameras &cameras) const |
| Triangulate and compute derivative of error with respect to point. More...
|
|
bool | triangulateAndComputeE (Matrix &E, const Values &values) const |
| Triangulate and compute derivative of error with respect to point. More...
|
|
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. More...
|
|
double | error (const Values &values) const override |
| Calculate total reprojection error. More...
|
|
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. More...
|
|
void | add (const ZVector &measurements, const KeyVector &cameraKeys) |
| Add a bunch of measurements, together with the camera keys.
|
|
void | add (const SFM_TRACK &trackToAdd) |
| Add an entire SfM_track (collection of cameras observing a single point). 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).
|
|
virtual Cameras | cameras (const Values &values) const |
| Collect all cameras: important that in key order. More...
|
|
void | print (const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override |
| print More...
|
|
bool | equals (const NonlinearFactor &p, double tol=1e-9) const override |
| equals More...
|
|
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. More...
|
|
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). More...
|
|
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.
|
|
double | totalReprojectionError (const Cameras &cameras, const POINT &point) const |
| Calculate the error of the factor. More...
|
|
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. More...
|
|
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. More...
|
|
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. More...
|
|
void | whitenJacobians (FBlocks &F, Matrix &E, Vector &b) const |
| Whiten the Jacobians computed by computeJacobians using noiseModel_.
|
|
boost::shared_ptr< RegularImplicitSchurFactor< PinholePose< CALIBRATION > > > | 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. More...
|
|
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. More...
|
|
virtual bool | active (const Values &) const |
| Checks whether a factor should be used based on a set of values. More...
|
|
virtual shared_ptr | clone () const |
| Creates a shared_ptr clone of the factor - needs to be specialized to allow for subclasses. More...
|
|
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. More...
|
|
virtual shared_ptr | rekey (const KeyVector &new_keys) const |
| Clones a factor and fully replaces its keys. More...
|
|
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) More...
|
|
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 More...
|
|
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.
|
|