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 ProjectionFactor.h
14 * @brief Basic bearing factor from 2D measurement
15 * @author Chris Beall
16 * @author Richard Roberts
17 * @author Frank Dellaert
18 * @author Alex Cunningham
19 */
20
21#pragma once
22
23#include <gtsam/nonlinear/NonlinearFactor.h>
24#include <gtsam/geometry/PinholeCamera.h>
25#include "gtsam/geometry/Cal3_S2.h"
26
27namespace gtsam {
28
29 /**
30 * Non-linear factor for a constraint derived from a 2D measurement. The calibration is known here.
31 * i.e. the main building block for visual SLAM.
32 * @ingroup slam
33 */
34 template<class POSE, class LANDMARK, class CALIBRATION = Cal3_S2>
35 class MultiProjectionFactor: public NoiseModelFactor {
36 protected:
37
38 // Keep a copy of measurement and calibration for I/O
39 Vector measured_; ///< 2D measurement for each of the n views
40 std::shared_ptr<CALIBRATION> K_; ///< shared pointer to calibration object
41 std::optional<POSE> body_P_sensor_; ///< The pose of the sensor in the body frame
42
43
44 // verbosity handling for Cheirality Exceptions
45 bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false)
46 bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false)
47
48 public:
49
50 /// shorthand for base class type
51 typedef NoiseModelFactor Base;
52
53 /// shorthand for this class
54 typedef MultiProjectionFactor<POSE, LANDMARK, CALIBRATION> This;
55
56 /// shorthand for a smart pointer to a factor
57 typedef std::shared_ptr<This> shared_ptr;
58
59 /// Default constructor
60 MultiProjectionFactor() : throwCheirality_(false), verboseCheirality_(false) {}
61
62 /**
63 * Constructor
64 * TODO: Mark argument order standard (keys, measurement, parameters)
65 * @param measured is the 2n dimensional location of the n points in the n views (the measurements)
66 * @param model is the standard deviation (current version assumes that the uncertainty is the same for all views)
67 * @param poseKeys is the set of indices corresponding to the cameras observing the same landmark
68 * @param pointKey is the index of the landmark
69 * @param K shared pointer to the constant calibration
70 * @param body_P_sensor is the transform from body to sensor frame (default identity)
71 */
72 MultiProjectionFactor(const Vector& measured, const SharedNoiseModel& model,
73 KeySet poseKeys, Key pointKey, const std::shared_ptr<CALIBRATION>& K,
74 std::optional<POSE> body_P_sensor = {}) :
75 Base(model), measured_(measured), K_(K), body_P_sensor_(body_P_sensor),
76 throwCheirality_(false), verboseCheirality_(false) {
77 keys_.assign(poseKeys.begin(), poseKeys.end());
78 keys_.push_back(pointKey);
79 }
80
81 /**
82 * Constructor with exception-handling flags
83 * TODO: Mark argument order standard (keys, measurement, parameters)
84 * @param measured is the 2 dimensional location of point in image (the measurement)
85 * @param model is the standard deviation
86 * @param poseKey is the index of the camera
87 * @param pointKey is the index of the landmark
88 * @param K shared pointer to the constant calibration
89 * @param throwCheirality determines whether Cheirality exceptions are rethrown
90 * @param verboseCheirality determines whether exceptions are printed for Cheirality
91 * @param body_P_sensor is the transform from body to sensor frame (default identity)
92 */
93 MultiProjectionFactor(const Vector& measured, const SharedNoiseModel& model,
94 KeySet poseKeys, Key pointKey, const std::shared_ptr<CALIBRATION>& K,
95 bool throwCheirality, bool verboseCheirality,
96 std::optional<POSE> body_P_sensor = {}) :
97 Base(model), measured_(measured), K_(K), body_P_sensor_(body_P_sensor),
98 throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) {}
99
100 /** Virtual destructor */
101 ~MultiProjectionFactor() override {}
102
103 /// @return a deep copy of this factor
104 NonlinearFactor::shared_ptr clone() const override {
105 return std::static_pointer_cast<NonlinearFactor>(
106 r: NonlinearFactor::shared_ptr(new This(*this))); }
107
108 /**
109 * print
110 * @param s optional string naming the factor
111 * @param keyFormatter optional formatter useful for printing Symbols
112 */
113 void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
114 std::cout << s << "MultiProjectionFactor, z = ";
115 std::cout << measured_ << "measurements, z = ";
116 if(this->body_P_sensor_)
117 this->body_P_sensor_->print(" sensor pose in body frame: ");
118 Base::print(s: "", keyFormatter);
119 }
120
121 /// equals
122 bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
123 const This *e = dynamic_cast<const This*>(&p);
124 return e
125 && Base::equals(f: p, tol)
126 //&& this->measured_.equals(e->measured_, tol)
127 && this->K_->equals(*e->K_, tol)
128 && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
129 }
130
131 /// Evaluate error h(x)-z and optionally derivatives
132 Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const override {
133
134 Vector a;
135 return a;
136
137// Point3 point = x.at<Point3>(*keys_.end());
138//
139// std::vector<KeyType>::iterator vit;
140// for (vit = keys_.begin(); vit != keys_.end()-1; vit++) {
141// Key key = (*vit);
142// Pose3 pose = x.at<Pose3>(key);
143//
144// if(body_P_sensor_) {
145// if(H1) {
146// Matrix H0;
147// PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_, H0), *K_);
148// Point2 reprojectionError(camera.project(point, H1, H2) - measured_);
149// *H1 = *H1 * H0;
150// return reprojectionError;
151// } else {
152// PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_), *K_);
153// Point2 reprojectionError(camera.project(point, H1, H2) - measured_);
154// return reprojectionError;
155// }
156// } else {
157// PinholeCamera<CALIBRATION> camera(pose, *K_);
158// Point2 reprojectionError(camera.project(point, H1, H2) - measured_);
159// return reprojectionError;
160// }
161// }
162
163 }
164
165
166 Vector evaluateError(const Pose3& pose, const Point3& point,
167 OptionalJacobian<2, 6> H1 = {}, OptionalJacobian<2,3> H2 = {}) const {
168 try {
169 if(body_P_sensor_) {
170 if(H1) {
171 Matrix H0;
172 PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_, H0), *K_);
173 Point2 reprojectionError(camera.project(point, H1, H2) - measured_);
174 *H1 = *H1 * H0;
175 return reprojectionError;
176 } else {
177 PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_), *K_);
178 Point2 reprojectionError(camera.project(point, H1, H2) - measured_);
179 return reprojectionError;
180 }
181 } else {
182 PinholeCamera<CALIBRATION> camera(pose, *K_);
183 Point2 reprojectionError(camera.project(point, H1, H2) - measured_);
184 return reprojectionError;
185 }
186 } catch( CheiralityException& e) {
187 if (H1) *H1 = Matrix::Zero(rows: 2,cols: 6);
188 if (H2) *H2 = Matrix::Zero(rows: 2,cols: 3);
189 if (verboseCheirality_)
190 std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->keys_.at(1)) <<
191 " moved behind camera " << DefaultKeyFormatter(this->keys_.at(0)) << std::endl;
192 if (throwCheirality_)
193 throw e;
194 }
195 return Vector::Ones(size: 2) * 2.0 * K_->fx();
196 }
197
198 /** return the measurements */
199 const Vector& measured() const {
200 return measured_;
201 }
202
203 /** return the calibration object */
204 inline const std::shared_ptr<CALIBRATION> calibration() const {
205 return K_;
206 }
207
208 /** return verbosity */
209 inline bool verboseCheirality() const { return verboseCheirality_; }
210
211 /** return flag for throwing cheirality exceptions */
212 inline bool throwCheirality() const { return throwCheirality_; }
213
214 private:
215
216#if GTSAM_ENABLE_BOOST_SERIALIZATION ///
217 /// Serialization function
218 friend class boost::serialization::access;
219 template<class ARCHIVE>
220 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
221 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
222 ar & BOOST_SERIALIZATION_NVP(measured_);
223 ar & BOOST_SERIALIZATION_NVP(K_);
224 ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
225 ar & BOOST_SERIALIZATION_NVP(throwCheirality_);
226 ar & BOOST_SERIALIZATION_NVP(verboseCheirality_);
227 }
228#endif
229 };
230} // \ namespace gtsam
231