1
2/**
3 * @file InvDepthFactorVariant2.h
4 * @brief Inverse Depth Factor based on Civera09tro, Montiel06rss.
5 * Landmarks are parameterized as (theta,phi,rho) with the reference point
6 * created at landmark construction and then never updated (i.e. the point
7 * [x,y,z] is treated as fixed and not part of the optimization). The factor
8 * involves a single pose and a landmark.
9 * @author Chris Beall, Stephen Williams
10 */
11
12#pragma once
13
14#include <gtsam/nonlinear/NonlinearFactor.h>
15#include <gtsam/geometry/PinholeCamera.h>
16#include <gtsam/geometry/Cal3_S2.h>
17#include <gtsam/geometry/Pose3.h>
18#include <gtsam/geometry/Point2.h>
19#include <gtsam/base/numericalDerivative.h>
20
21namespace gtsam {
22
23/**
24 * Binary factor representing a visual measurement using an inverse-depth parameterization
25 */
26class InvDepthFactorVariant2: public NoiseModelFactorN<Pose3, Vector3> {
27protected:
28
29 // Keep a copy of measurement and calibration for I/O
30 Point2 measured_; ///< 2D measurement
31 Cal3_S2::shared_ptr K_; ///< shared pointer to calibration object
32 Point3 referencePoint_; ///< the reference point/origin for this landmark
33
34public:
35
36 /// shorthand for base class type
37 typedef NoiseModelFactor2<Pose3, Vector3> Base;
38
39 // Provide access to the Matrix& version of evaluateError:
40 using Base::evaluateError;
41
42 /// shorthand for this class
43 typedef InvDepthFactorVariant2 This;
44
45 /// shorthand for a smart pointer to a factor
46 typedef std::shared_ptr<This> shared_ptr;
47
48 /// Default constructor
49 InvDepthFactorVariant2() :
50 measured_(0.0, 0.0), K_(new Cal3_S2(444, 555, 666, 777, 888)) {
51 }
52
53 /**
54 * Constructor
55 * @param poseKey is the index of the camera pose
56 * @param pointKey is the index of the landmark
57 * @param measured is the 2 dimensional location of point in image (the measurement)
58 * @param K shared pointer to the constant calibration
59 * @param model is the standard deviation
60 */
61 InvDepthFactorVariant2(const Key poseKey, const Key landmarkKey,
62 const Point2& measured, const Cal3_S2::shared_ptr& K, const Point3 referencePoint,
63 const SharedNoiseModel& model) :
64 Base(model, poseKey, landmarkKey), measured_(measured), K_(K), referencePoint_(referencePoint) {}
65
66 /** Virtual destructor */
67 ~InvDepthFactorVariant2() override {}
68
69 /**
70 * print
71 * @param s optional string naming the factor
72 * @param keyFormatter optional formatter useful for printing Symbols
73 */
74 void print(const std::string& s = "InvDepthFactorVariant2",
75 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
76 Base::print(s, keyFormatter);
77 traits<Point2>::Print(m: measured_, str: s + ".z");
78 }
79
80 /// equals
81 bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
82 const This *e = dynamic_cast<const This*>(&p);
83 return e
84 && Base::equals(f: p, tol)
85 && traits<Point2>::Equals(v1: this->measured_, v2: e->measured_, tol)
86 && this->K_->equals(K: *e->K_, tol)
87 && traits<Point3>::Equals(v1: this->referencePoint_, v2: e->referencePoint_, tol);
88 }
89
90 Vector inverseDepthError(const Pose3& pose, const Vector3& landmark) const {
91 try {
92 // Calculate the 3D coordinates of the landmark in the world frame
93 double theta = landmark(0), phi = landmark(1), rho = landmark(2);
94 Point3 world_P_landmark = referencePoint_ + Point3(cos(x: theta)*cos(x: phi)/rho, sin(x: theta)*cos(x: phi)/rho, sin(x: phi)/rho);
95 // Project landmark into Pose2
96 PinholeCamera<Cal3_S2> camera(pose, *K_);
97 return camera.project(pw: world_P_landmark) - measured_;
98 } catch( CheiralityException& e) {
99 std::cout << e.what()
100 << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key<2>()) << "]"
101 << " moved behind camera [" << DefaultKeyFormatter(this->key<1>()) <<"]"
102 << std::endl;
103 return Vector::Ones(newSize: 2) * 2.0 * K_->fx();
104 }
105 return (Vector(1) << 0.0).finished();
106 }
107
108 /// Evaluate error h(x)-z and optionally derivatives
109 Vector evaluateError(const Pose3& pose, const Vector3& landmark,
110 OptionalMatrixType H1, OptionalMatrixType H2) const override {
111
112 if (H1) {
113 (*H1) = numericalDerivative11<Vector, Pose3>(
114 h: std::bind(f: &InvDepthFactorVariant2::inverseDepthError, args: this,
115 args: std::placeholders::_1, args: landmark), x: pose);
116 }
117 if (H2) {
118 (*H2) = numericalDerivative11<Vector, Vector3>(
119 h: std::bind(f: &InvDepthFactorVariant2::inverseDepthError, args: this, args: pose,
120 args: std::placeholders::_1), x: landmark);
121 }
122
123 return inverseDepthError(pose, landmark);
124 }
125
126 /** return the measurement */
127 const Point2& imagePoint() const {
128 return measured_;
129 }
130
131 /** return the calibration object */
132 const Cal3_S2::shared_ptr calibration() const {
133 return K_;
134 }
135
136 /** return the calibration object */
137 const Point3& referencePoint() const {
138 return referencePoint_;
139 }
140
141private:
142
143#if GTSAM_ENABLE_BOOST_SERIALIZATION ///
144 /// Serialization function
145 friend class boost::serialization::access;
146 template<class ARCHIVE>
147 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
148 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
149 ar & BOOST_SERIALIZATION_NVP(measured_);
150 ar & BOOST_SERIALIZATION_NVP(K_);
151 ar & BOOST_SERIALIZATION_NVP(referencePoint_);
152 }
153#endif
154};
155
156} // \ namespace gtsam
157