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