1/* ----------------------------------------------------------------------------
2
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4 * Atlanta, Georgia 30332-0415
5 * All Rights Reserved
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
8 * See LICENSE for the license information
9
10 * -------------------------------------------------------------------------- */
11
12/**
13 * @file ProjectionFactorPPP.h
14 * @brief Derived from ProjectionFactor, but estimates body-camera transform
15 * in addition to body pose and 3D landmark
16 * @author Chris Beall
17 */
18
19#pragma once
20
21#include <gtsam/nonlinear/NonlinearFactor.h>
22#include <gtsam/geometry/PinholeCamera.h>
23#include <gtsam/geometry/Cal3_S2.h>
24
25namespace gtsam {
26
27 /**
28 * Non-linear factor for a constraint derived from a 2D measurement. The calibration is known here.
29 * i.e. the main building block for visual SLAM.
30 * @ingroup slam
31 */
32 template<class POSE, class LANDMARK, class CALIBRATION = Cal3_S2>
33 class ProjectionFactorPPP: public NoiseModelFactorN<POSE, POSE, LANDMARK> {
34 protected:
35
36 // Keep a copy of measurement and calibration for I/O
37 Point2 measured_; ///< 2D measurement
38 std::shared_ptr<CALIBRATION> K_; ///< shared pointer to calibration object
39
40 // verbosity handling for Cheirality Exceptions
41 bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false)
42 bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false)
43
44 public:
45
46 /// shorthand for base class type
47 typedef NoiseModelFactor3<POSE, POSE, LANDMARK> Base;
48
49 // Provide access to the Matrix& version of evaluateError:
50 using Base::evaluateError;
51
52 /// shorthand for this class
53 typedef ProjectionFactorPPP<POSE, LANDMARK, CALIBRATION> This;
54
55 /// shorthand for a smart pointer to a factor
56 typedef std::shared_ptr<This> shared_ptr;
57
58 /// Default constructor
59 ProjectionFactorPPP() :
60 measured_(0.0, 0.0), throwCheirality_(false), verboseCheirality_(false) {
61 }
62
63 /**
64 * Constructor
65 * TODO: Mark argument order standard (keys, measurement, parameters)
66 * @param measured is the 2 dimensional location of point in image (the measurement)
67 * @param model is the standard deviation
68 * @param poseKey is the index of the camera
69 * @param transformKey is the index of the body-camera transform
70 * @param pointKey is the index of the landmark
71 * @param K shared pointer to the constant calibration
72 */
73 ProjectionFactorPPP(const Point2& measured, const SharedNoiseModel& model,
74 Key poseKey, Key transformKey, Key pointKey,
75 const std::shared_ptr<CALIBRATION>& K) :
76 Base(model, poseKey, transformKey, pointKey), measured_(measured), K_(K),
77 throwCheirality_(false), verboseCheirality_(false) {}
78
79 /**
80 * Constructor with exception-handling flags
81 * TODO: Mark argument order standard (keys, measurement, parameters)
82 * @param measured is the 2 dimensional location of point in image (the measurement)
83 * @param model is the standard deviation
84 * @param poseKey is the index of the camera
85 * @param pointKey is the index of the landmark
86 * @param K shared pointer to the constant calibration
87 * @param throwCheirality determines whether Cheirality exceptions are rethrown
88 * @param verboseCheirality determines whether exceptions are printed for Cheirality
89 */
90 ProjectionFactorPPP(const Point2& measured, const SharedNoiseModel& model,
91 Key poseKey, Key transformKey, Key pointKey,
92 const std::shared_ptr<CALIBRATION>& K,
93 bool throwCheirality, bool verboseCheirality) :
94 Base(model, poseKey, transformKey, pointKey), measured_(measured), K_(K),
95 throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) {}
96
97 /** Virtual destructor */
98 ~ProjectionFactorPPP() override {}
99
100 /// @return a deep copy of this factor
101 NonlinearFactor::shared_ptr clone() const override {
102 return std::static_pointer_cast<NonlinearFactor>(
103 r: NonlinearFactor::shared_ptr(new This(*this))); }
104
105 /**
106 * print
107 * @param s optional string naming the factor
108 * @param keyFormatter optional formatter useful for printing Symbols
109 */
110 void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
111 std::cout << s << "ProjectionFactorPPP, z = ";
112 traits<Point2>::Print(m: measured_);
113 Base::print("", keyFormatter);
114 }
115
116 /// equals
117 bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
118 const This *e = dynamic_cast<const This*>(&p);
119 return e
120 && Base::equals(p, tol)
121 && traits<Point2>::Equals(v1: this->measured_, v2: e->measured_, tol)
122 && this->K_->equals(*e->K_, tol);
123 }
124
125 /// Evaluate error h(x)-z and optionally derivatives
126 Vector evaluateError(const Pose3& pose, const Pose3& transform, const Point3& point,
127 OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3) const override {
128 try {
129 if(H1 || H2 || H3) {
130 Matrix H0, H02;
131 PinholeCamera<CALIBRATION> camera(pose.compose(g: transform, H1: H0, H2: H02), *K_);
132 Point2 reprojectionError(camera.project(point, H1, H3, {}) - measured_);
133 *H2 = *H1 * H02;
134 *H1 = *H1 * H0;
135 return reprojectionError;
136 } else {
137 PinholeCamera<CALIBRATION> camera(pose.compose(g: transform), *K_);
138 return camera.project(point, H1, H3, {}) - measured_;
139 }
140 } catch( CheiralityException& e) {
141 if (H1) *H1 = Matrix::Zero(rows: 2,cols: 6);
142 if (H2) *H2 = Matrix::Zero(rows: 2,cols: 6);
143 if (H3) *H3 = Matrix::Zero(rows: 2,cols: 3);
144 if (verboseCheirality_)
145 std::cout << e.what() << ": Landmark "
146 << DefaultKeyFormatter(this->key2())
147 << " moved behind camera "
148 << DefaultKeyFormatter(this->key1())
149 << std::endl;
150 if (throwCheirality_)
151 throw e;
152 }
153 return Vector::Ones(newSize: 2) * 2.0 * K_->fx();
154 }
155
156 /** return the measurement */
157 const Point2& measured() const {
158 return measured_;
159 }
160
161 /** return the calibration object */
162 inline const std::shared_ptr<CALIBRATION> calibration() const {
163 return K_;
164 }
165
166 /** return verbosity */
167 inline bool verboseCheirality() const { return verboseCheirality_; }
168
169 /** return flag for throwing cheirality exceptions */
170 inline bool throwCheirality() const { return throwCheirality_; }
171
172 private:
173
174#if GTSAM_ENABLE_BOOST_SERIALIZATION ///
175 /// Serialization function
176 friend class boost::serialization::access;
177 template<class ARCHIVE>
178 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
179 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
180 ar & BOOST_SERIALIZATION_NVP(measured_);
181 ar & BOOST_SERIALIZATION_NVP(K_);
182 ar & BOOST_SERIALIZATION_NVP(throwCheirality_);
183 ar & BOOST_SERIALIZATION_NVP(verboseCheirality_);
184 }
185#endif
186 };
187
188 /// traits
189 template<class POSE, class LANDMARK, class CALIBRATION>
190 struct traits<ProjectionFactorPPP<POSE, LANDMARK, CALIBRATION> > :
191 public Testable<ProjectionFactorPPP<POSE, LANDMARK, CALIBRATION> > {
192 };
193
194} // \ namespace gtsam
195