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 ProjectionFactorRollingShutter.h
14 * @brief Basic projection factor for rolling shutter cameras
15 * @author Yotam Stern
16 */
17
18#pragma once
19
20#include <gtsam/geometry/Cal3_S2.h>
21#include <gtsam/geometry/CalibratedCamera.h>
22#include <gtsam/geometry/PinholeCamera.h>
23#include <gtsam/nonlinear/NonlinearFactor.h>
24#include <gtsam_unstable/dllexport.h>
25
26
27namespace gtsam {
28
29/**
30 * Non-linear factor for 2D projection measurement obtained using a rolling
31 * shutter camera. The calibration is known here. This version takes rolling
32 * shutter information into account as follows: consider two consecutive poses A
33 * and B, and a Point2 measurement taken starting at time A using a rolling
34 * shutter camera. Pose A has timestamp t_A, and Pose B has timestamp t_B. The
35 * Point2 measurement has timestamp t_p (with t_A <= t_p <= t_B) corresponding
36 * to the time of exposure of the row of the image the pixel belongs to. Let us
37 * define the alpha = (t_p - t_A) / (t_B - t_A), we will use the pose
38 * interpolated between A and B by the alpha to project the corresponding
39 * landmark to Point2.
40 * @ingroup slam
41 */
42
43class GTSAM_UNSTABLE_EXPORT ProjectionFactorRollingShutter
44 : public NoiseModelFactorN<Pose3, Pose3, Point3> {
45 protected:
46 // Keep a copy of measurement and calibration for I/O
47 Point2 measured_; ///< 2D measurement
48 double alpha_; ///< interpolation parameter in [0,1] corresponding to the
49 ///< point2 measurement
50 std::shared_ptr<Cal3_S2> K_; ///< shared pointer to calibration object
51 std::optional<Pose3>
52 body_P_sensor_; ///< The pose of the sensor in the body frame
53
54 // verbosity handling for Cheirality Exceptions
55 bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default:
56 ///< false)
57 bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions
58 ///< (default: false)
59
60 public:
61 /// shorthand for base class type
62 typedef NoiseModelFactor3<Pose3, Pose3, Point3> Base;
63
64 // Provide access to the Matrix& version of evaluateError:
65 using Base::evaluateError;
66
67
68 /// shorthand for this class
69 typedef ProjectionFactorRollingShutter This;
70
71 /// shorthand for a smart pointer to a factor
72 typedef std::shared_ptr<This> shared_ptr;
73
74 /// Default constructor
75 ProjectionFactorRollingShutter()
76 : measured_(0, 0),
77 alpha_(0),
78 throwCheirality_(false),
79 verboseCheirality_(false) {}
80
81 /**
82 * Constructor
83 * @param measured is the 2-dimensional pixel location of point in the image
84 * (the measurement)
85 * @param alpha in [0,1] is the rolling shutter parameter for the measurement
86 * @param model is the noise model
87 * @param poseKey_a is the key of the first camera
88 * @param poseKey_b is the key of the second camera
89 * @param pointKey is the key of the landmark
90 * @param K shared pointer to the constant calibration
91 * @param body_P_sensor is the transform from body to sensor frame (default
92 * identity)
93 */
94 ProjectionFactorRollingShutter(
95 const Point2& measured, double alpha, const SharedNoiseModel& model,
96 Key poseKey_a, Key poseKey_b, Key pointKey,
97 const std::shared_ptr<Cal3_S2>& K,
98 std::optional<Pose3> body_P_sensor = {})
99 : Base(model, poseKey_a, poseKey_b, pointKey),
100 measured_(measured),
101 alpha_(alpha),
102 K_(K),
103 body_P_sensor_(body_P_sensor),
104 throwCheirality_(false),
105 verboseCheirality_(false) {}
106
107 /**
108 * Constructor with exception-handling flags
109 * @param measured is the 2-dimensional pixel location of point in the image
110 * (the measurement)
111 * @param alpha in [0,1] is the rolling shutter parameter for the measurement
112 * @param model is the noise model
113 * @param poseKey_a is the key of the first camera
114 * @param poseKey_b is the key of the second camera
115 * @param pointKey is the key of the landmark
116 * @param K shared pointer to the constant calibration
117 * @param throwCheirality determines whether Cheirality exceptions are
118 * rethrown
119 * @param verboseCheirality determines whether exceptions are printed for
120 * Cheirality
121 * @param body_P_sensor is the transform from body to sensor frame (default
122 * identity)
123 */
124 ProjectionFactorRollingShutter(
125 const Point2& measured, double alpha, const SharedNoiseModel& model,
126 Key poseKey_a, Key poseKey_b, Key pointKey,
127 const std::shared_ptr<Cal3_S2>& K, bool throwCheirality,
128 bool verboseCheirality,
129 std::optional<Pose3> body_P_sensor = {})
130 : Base(model, poseKey_a, poseKey_b, pointKey),
131 measured_(measured),
132 alpha_(alpha),
133 K_(K),
134 body_P_sensor_(body_P_sensor),
135 throwCheirality_(throwCheirality),
136 verboseCheirality_(verboseCheirality) {}
137
138 /** Virtual destructor */
139 virtual ~ProjectionFactorRollingShutter() {}
140
141 /// @return a deep copy of this factor
142 gtsam::NonlinearFactor::shared_ptr clone() const override {
143 return std::static_pointer_cast<gtsam::NonlinearFactor>(
144 r: gtsam::NonlinearFactor::shared_ptr(new This(*this)));
145 }
146
147 /**
148 * print
149 * @param s optional string naming the factor
150 * @param keyFormatter optional formatter useful for printing Symbols
151 */
152 void print(
153 const std::string& s = "",
154 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
155 std::cout << s << "ProjectionFactorRollingShutter, z = ";
156 traits<Point2>::Print(m: measured_);
157 std::cout << " rolling shutter interpolation param = " << alpha_;
158 if (this->body_P_sensor_)
159 this->body_P_sensor_->print(s: " sensor pose in body frame: ");
160 Base::print(s: "", keyFormatter);
161 }
162
163 /// equals
164 bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
165 const This* e = dynamic_cast<const This*>(&p);
166 return e && Base::equals(f: p, tol) && (alpha_ == e->alpha()) &&
167 traits<Point2>::Equals(v1: this->measured_, v2: e->measured_, tol) &&
168 this->K_->equals(K: *e->K_, tol) &&
169 (this->throwCheirality_ == e->throwCheirality_) &&
170 (this->verboseCheirality_ == e->verboseCheirality_) &&
171 ((!body_P_sensor_ && !e->body_P_sensor_) ||
172 (body_P_sensor_ && e->body_P_sensor_ &&
173 body_P_sensor_->equals(pose: *e->body_P_sensor_)));
174 }
175
176 /// Evaluate error h(x)-z and optionally derivatives
177 Vector evaluateError(
178 const Pose3& pose_a, const Pose3& pose_b, const Point3& point,
179 OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3) const override;
180
181 /** return the measurement */
182 const Point2& measured() const { return measured_; }
183
184 /** return the calibration object */
185 inline const std::shared_ptr<Cal3_S2> calibration() const { return K_; }
186
187 /** returns the rolling shutter interp param*/
188 inline double alpha() const { return alpha_; }
189
190 /** return verbosity */
191 inline bool verboseCheirality() const { return verboseCheirality_; }
192
193 /** return flag for throwing cheirality exceptions */
194 inline bool throwCheirality() const { return throwCheirality_; }
195
196 private:
197#if GTSAM_ENABLE_BOOST_SERIALIZATION ///
198 /// Serialization function
199 friend class boost::serialization::access;
200 template <class ARCHIVE>
201 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
202 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
203 ar& BOOST_SERIALIZATION_NVP(measured_);
204 ar& BOOST_SERIALIZATION_NVP(alpha_);
205 ar& BOOST_SERIALIZATION_NVP(K_);
206 ar& BOOST_SERIALIZATION_NVP(body_P_sensor_);
207 ar& BOOST_SERIALIZATION_NVP(throwCheirality_);
208 ar& BOOST_SERIALIZATION_NVP(verboseCheirality_);
209 }
210#endif
211
212 public:
213 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
214};
215
216/// traits
217template <>
218struct traits<ProjectionFactorRollingShutter>
219 : public Testable<ProjectionFactorRollingShutter> {};
220
221} // namespace gtsam
222