1
2/**
3 * @file InvDepthFactor3.h
4 * @brief Inverse Depth Factor based on Civera09tro, Montiel06rss.
5 * Landmarks are initialized from the first camera observation with
6 * (x,y,z,theta,phi,inv_depth), where x,y,z are the coordinates of
7 * the camera. InvDepthCamera provides methods to initialize inverse
8 * depth landmarks (backproject), and to convert inverse depth
9 * landmarks to cartesian coordinates (Point3) for visualization, etc.
10 * The inverse depth parameterization is split into (x,y,z,theta,phi),
11 * (inv_depth) to make it easy to add a prior on inverse depth alone
12 * @author Chris Beall
13 */
14
15#pragma once
16
17#include <gtsam/geometry/Cal3_S2.h>
18#include <gtsam/nonlinear/NonlinearFactor.h>
19#include <gtsam_unstable/geometry/InvDepthCamera3.h>
20
21namespace gtsam {
22
23/**
24 * Ternary factor representing a visual measurement that includes inverse depth
25 */
26template<class POSE, class LANDMARK, class INVDEPTH>
27class InvDepthFactor3: public NoiseModelFactorN<POSE, LANDMARK, INVDEPTH> {
28protected:
29
30 // Keep a copy of measurement and calibration for I/O
31 Point2 measured_; ///< 2D measurement
32 std::shared_ptr<Cal3_S2> K_; ///< shared pointer to calibration object
33
34public:
35
36 /// shorthand for base class type
37 typedef NoiseModelFactor3<POSE, LANDMARK, INVDEPTH> Base;
38
39 // Provide access to the Matrix& version of evaluateError:
40 using Base::evaluateError;
41
42 /// shorthand for this class
43 typedef InvDepthFactor3<POSE, LANDMARK, INVDEPTH> This;
44
45 /// shorthand for a smart pointer to a factor
46 typedef std::shared_ptr<This> shared_ptr;
47
48 /// Default constructor
49 InvDepthFactor3() :
50 measured_(0.0, 0.0), K_(new Cal3_S2(444, 555, 666, 777, 888)) {
51 }
52
53 /**
54 * Constructor
55 * TODO: Mark argument order standard (keys, measurement, parameters)
56 * @param measured is the 2 dimensional location of point in image (the measurement)
57 * @param model is the standard deviation
58 * @param poseKey is the index of the camera pose
59 * @param pointKey is the index of the landmark
60 * @param invDepthKey is the index of inverse depth
61 * @param K shared pointer to the constant calibration
62 */
63 InvDepthFactor3(const Point2& measured, const SharedNoiseModel& model,
64 const Key poseKey, Key pointKey, Key invDepthKey, const Cal3_S2::shared_ptr& K) :
65 Base(model, poseKey, pointKey, invDepthKey), measured_(measured), K_(K) {}
66
67 /** Virtual destructor */
68 ~InvDepthFactor3() override {}
69
70 /**
71 * print
72 * @param s optional string naming the factor
73 * @param keyFormatter optional formatter useful for printing Symbols
74 */
75 void print(const std::string& s = "InvDepthFactor3",
76 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
77 Base::print(s, keyFormatter);
78 traits<Point2>::Print(m: measured_, str: s + ".z");
79 }
80
81 /// equals
82 bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
83 const This *e = dynamic_cast<const This*>(&p);
84 return e && Base::equals(p, tol) && traits<Point2>::Equals(v1: this->measured_, v2: e->measured_, tol) && this->K_->equals(*e->K_, tol);
85 }
86
87 /// Evaluate error h(x)-z and optionally derivatives
88 Vector evaluateError(const POSE& pose, const Vector5& point, const INVDEPTH& invDepth,
89 OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3) const override {
90 try {
91 InvDepthCamera3<Cal3_S2> camera(pose, K_);
92 return camera.project(pw: point, rho: invDepth, H1, H2, H3) - measured_;
93 } catch( CheiralityException& e) {
94 if (H1) *H1 = Matrix::Zero(rows: 2,cols: 6);
95 if (H2) *H2 = Matrix::Zero(rows: 2,cols: 5);
96 if (H3) *H3 = Matrix::Zero(rows: 2,cols: 1);
97 std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
98 " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
99 return Vector::Ones(newSize: 2) * 2.0 * K_->fx();
100 }
101 return (Vector(1) << 0.0).finished();
102 }
103
104 /** return the measurement */
105 const Point2& imagePoint() const {
106 return measured_;
107 }
108
109 /** return the calibration object */
110 inline const Cal3_S2::shared_ptr calibration() const {
111 return K_;
112 }
113
114private:
115
116#if GTSAM_ENABLE_BOOST_SERIALIZATION ///
117 /// Serialization function
118 friend class boost::serialization::access;
119 template<class ARCHIVE>
120 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
121 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
122 ar & BOOST_SERIALIZATION_NVP(measured_);
123 ar & BOOST_SERIALIZATION_NVP(K_);
124 }
125#endif
126};
127} // \ namespace gtsam
128