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 ProjectionFactorRollingShutterRollingShutter.cpp
14 * @brief Unit tests for ProjectionFactorRollingShutter Class
15 * @author Luca Carlone
16 * @date July 2021
17 */
18
19#include <CppUnitLite/TestHarness.h>
20#include <gtsam/base/TestableAssertions.h>
21#include <gtsam/base/numericalDerivative.h>
22#include <gtsam/geometry/Cal3DS2.h>
23#include <gtsam/geometry/Cal3_S2.h>
24#include <gtsam/geometry/Point2.h>
25#include <gtsam/geometry/Point3.h>
26#include <gtsam/geometry/Pose3.h>
27#include <gtsam/inference/Symbol.h>
28#include <gtsam_unstable/slam/ProjectionFactorRollingShutter.h>
29
30using namespace std::placeholders;
31using namespace std;
32using namespace gtsam;
33
34// make a realistic calibration matrix
35static double fov = 60; // degrees
36static size_t w = 640, h = 480;
37static Cal3_S2::shared_ptr K(new Cal3_S2(fov, w, h));
38
39// Create a noise model for the pixel error
40static SharedNoiseModel model(noiseModel::Unit::Create(dim: 2));
41
42// Convenience for named keys
43using symbol_shorthand::L;
44using symbol_shorthand::T;
45using symbol_shorthand::X;
46
47// Convenience to define common variables across many tests
48static Key poseKey1(X(j: 1));
49static Key poseKey2(X(j: 2));
50static Key pointKey(L(j: 1));
51static double interp_params = 0.5;
52static Point2 measurement(323.0, 240.0);
53static Pose3 body_P_sensor(Rot3::RzRyRx(x: -M_PI_2, y: 0.0, z: -M_PI_2),
54 Point3(0.25, -0.10, 1.0));
55
56/* ************************************************************************* */
57TEST(ProjectionFactorRollingShutter, Constructor) {
58 ProjectionFactorRollingShutter factor(measurement, interp_params, model,
59 poseKey1, poseKey2, pointKey, K);
60}
61
62/* ************************************************************************* */
63TEST(ProjectionFactorRollingShutter, ConstructorWithTransform) {
64 ProjectionFactorRollingShutter factor(measurement, interp_params, model,
65 poseKey1, poseKey2, pointKey, K,
66 body_P_sensor);
67}
68
69/* ************************************************************************* */
70TEST(ProjectionFactorRollingShutter, Equals) {
71 { // factors are equal
72 ProjectionFactorRollingShutter factor1(measurement, interp_params, model,
73 poseKey1, poseKey2, pointKey, K);
74 ProjectionFactorRollingShutter factor2(measurement, interp_params, model,
75 poseKey1, poseKey2, pointKey, K);
76 CHECK(assert_equal(factor1, factor2));
77 }
78 { // factors are NOT equal (keys are different)
79 ProjectionFactorRollingShutter factor1(measurement, interp_params, model,
80 poseKey1, poseKey2, pointKey, K);
81 ProjectionFactorRollingShutter factor2(measurement, interp_params, model,
82 poseKey1, poseKey1, pointKey, K);
83 CHECK(!assert_equal(factor1, factor2)); // not equal
84 }
85 { // factors are NOT equal (different interpolation)
86 ProjectionFactorRollingShutter factor1(measurement, 0.1, model, poseKey1,
87 poseKey1, pointKey, K);
88 ProjectionFactorRollingShutter factor2(measurement, 0.5, model, poseKey1,
89 poseKey2, pointKey, K);
90 CHECK(!assert_equal(factor1, factor2)); // not equal
91 }
92}
93
94/* ************************************************************************* */
95TEST(ProjectionFactorRollingShutter, EqualsWithTransform) {
96 { // factors are equal
97 ProjectionFactorRollingShutter factor1(measurement, interp_params, model,
98 poseKey1, poseKey2, pointKey, K,
99 body_P_sensor);
100 ProjectionFactorRollingShutter factor2(measurement, interp_params, model,
101 poseKey1, poseKey2, pointKey, K,
102 body_P_sensor);
103 CHECK(assert_equal(factor1, factor2));
104 }
105 { // factors are NOT equal
106 ProjectionFactorRollingShutter factor1(measurement, interp_params, model,
107 poseKey1, poseKey2, pointKey, K,
108 body_P_sensor);
109 Pose3 body_P_sensor2(
110 Rot3::RzRyRx(x: 0.0, y: 0.0, z: 0.0),
111 Point3(0.25, -0.10, 1.0)); // rotation different from body_P_sensor
112 ProjectionFactorRollingShutter factor2(measurement, interp_params, model,
113 poseKey1, poseKey2, pointKey, K,
114 body_P_sensor2);
115 CHECK(!assert_equal(factor1, factor2));
116 }
117}
118
119/* ************************************************************************* */
120TEST(ProjectionFactorRollingShutter, Error) {
121 {
122 // Create the factor with a measurement that is 3 pixels off in x
123 // Camera pose corresponds to the first camera
124 double t = 0.0;
125 ProjectionFactorRollingShutter factor(measurement, t, model, poseKey1,
126 poseKey2, pointKey, K);
127
128 // Set the linearization point
129 Pose3 pose1(Rot3(), Point3(0, 0, -6));
130 Pose3 pose2(Rot3(), Point3(0, 0, -4));
131 Point3 point(0.0, 0.0, 0.0);
132
133 // Use the factor to calculate the error
134 Vector actualError(factor.evaluateError(x: pose1, x: pose2, x: point));
135
136 // The expected error is (-3.0, 0.0) pixels / UnitCovariance
137 Vector expectedError = Vector2(-3.0, 0.0);
138
139 // Verify we get the expected error
140 CHECK(assert_equal(expectedError, actualError, 1e-9));
141 }
142 {
143 // Create the factor with a measurement that is 3 pixels off in x
144 // Camera pose is actually interpolated now
145 double t = 0.5;
146 ProjectionFactorRollingShutter factor(measurement, t, model, poseKey1,
147 poseKey2, pointKey, K);
148
149 // Set the linearization point
150 Pose3 pose1(Rot3(), Point3(0, 0, -8));
151 Pose3 pose2(Rot3(), Point3(0, 0, -4));
152 Point3 point(0.0, 0.0, 0.0);
153
154 // Use the factor to calculate the error
155 Vector actualError(factor.evaluateError(x: pose1, x: pose2, x: point));
156
157 // The expected error is (-3.0, 0.0) pixels / UnitCovariance
158 Vector expectedError = Vector2(-3.0, 0.0);
159
160 // Verify we get the expected error
161 CHECK(assert_equal(expectedError, actualError, 1e-9));
162 }
163 {
164 // Create measurement by projecting 3D landmark
165 double t = 0.3;
166 Pose3 pose1(Rot3::RzRyRx(x: 0.1, y: 0.0, z: 0.1), Point3(0, 0, 0));
167 Pose3 pose2(Rot3::RzRyRx(x: -0.1, y: -0.1, z: 0.0), Point3(0, 0, 1));
168 Pose3 poseInterp = interpolate<Pose3>(X: pose1, Y: pose2, t);
169 PinholeCamera<Cal3_S2> camera(poseInterp, *K);
170 Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera
171 Point2 measured = camera.project(pw: point);
172
173 // create factor
174 ProjectionFactorRollingShutter factor(measured, t, model, poseKey1,
175 poseKey2, pointKey, K);
176
177 // Use the factor to calculate the error
178 Vector actualError(factor.evaluateError(x: pose1, x: pose2, x: point));
179
180 // The expected error is zero
181 Vector expectedError = Vector2(0.0, 0.0);
182
183 // Verify we get the expected error
184 CHECK(assert_equal(expectedError, actualError, 1e-9));
185 }
186}
187
188/* ************************************************************************* */
189TEST(ProjectionFactorRollingShutter, ErrorWithTransform) {
190 // Create measurement by projecting 3D landmark
191 double t = 0.3;
192 Pose3 pose1(Rot3::RzRyRx(x: 0.1, y: 0.0, z: 0.1), Point3(0, 0, 0));
193 Pose3 pose2(Rot3::RzRyRx(x: -0.1, y: -0.1, z: 0.0), Point3(0, 0, 1));
194 Pose3 poseInterp = interpolate<Pose3>(X: pose1, Y: pose2, t);
195 Pose3 body_P_sensor3(Rot3::RzRyRx(x: -0.1, y: -0.1, z: 0.0), Point3(0, 0.2, 0.1));
196 PinholeCamera<Cal3_S2> camera(poseInterp * body_P_sensor3, *K);
197 Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera
198 Point2 measured = camera.project(pw: point);
199
200 // create factor
201 ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2,
202 pointKey, K, body_P_sensor3);
203
204 // Use the factor to calculate the error
205 Vector actualError(factor.evaluateError(x: pose1, x: pose2, x: point));
206
207 // The expected error is zero
208 Vector expectedError = Vector2(0.0, 0.0);
209
210 // Verify we get the expected error
211 CHECK(assert_equal(expectedError, actualError, 1e-9));
212}
213
214/* ************************************************************************* */
215TEST(ProjectionFactorRollingShutter, Jacobian) {
216 // Create measurement by projecting 3D landmark
217 double t = 0.3;
218 Pose3 pose1(Rot3::RzRyRx(x: 0.1, y: 0.0, z: 0.1), Point3(0, 0, 0));
219 Pose3 pose2(Rot3::RzRyRx(x: -0.1, y: -0.1, z: 0.0), Point3(0, 0, 1));
220 Pose3 poseInterp = interpolate<Pose3>(X: pose1, Y: pose2, t);
221 PinholeCamera<Cal3_S2> camera(poseInterp, *K);
222 Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera
223 Point2 measured = camera.project(pw: point);
224
225 // create factor
226 ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2,
227 pointKey, K);
228
229 // Use the factor to calculate the Jacobians
230 Matrix H1Actual, H2Actual, H3Actual;
231 factor.evaluateError(x: pose1, x: pose2, x: point, H&: H1Actual, H&: H2Actual, H&: H3Actual);
232
233 auto f = [&factor](const Pose3& p1, const Pose3& p2, const Point3& p3) {
234 return factor.evaluateError(x: p1, x: p2, x: p3);
235 };
236 // Expected Jacobians via numerical derivatives
237 Matrix H1Expected = numericalDerivative31<Vector, Pose3, Pose3, Point3>(h: f, x1: pose1, x2: pose2, x3: point);
238
239 Matrix H2Expected = numericalDerivative32<Vector, Pose3, Pose3, Point3>(h: f, x1: pose1, x2: pose2, x3: point);
240
241 Matrix H3Expected = numericalDerivative33<Vector, Pose3, Pose3, Point3>(h: f, x1: pose1, x2: pose2, x3: point);
242
243 CHECK(assert_equal(H1Expected, H1Actual, 1e-5));
244 CHECK(assert_equal(H2Expected, H2Actual, 1e-5));
245 CHECK(assert_equal(H3Expected, H3Actual, 1e-5));
246}
247
248/* ************************************************************************* */
249TEST(ProjectionFactorRollingShutter, JacobianWithTransform) {
250 // Create measurement by projecting 3D landmark
251 double t = 0.6;
252 Pose3 pose1(Rot3::RzRyRx(x: 0.1, y: 0.0, z: 0.1), Point3(0, 0, 0));
253 Pose3 pose2(Rot3::RzRyRx(x: -0.1, y: -0.1, z: 0.0), Point3(0, 0, 1));
254 Pose3 poseInterp = interpolate<Pose3>(X: pose1, Y: pose2, t);
255 Pose3 body_P_sensor3(Rot3::RzRyRx(x: -0.1, y: -0.1, z: 0.0), Point3(0, 0.2, 0.1));
256 PinholeCamera<Cal3_S2> camera(poseInterp * body_P_sensor3, *K);
257 Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera
258 Point2 measured = camera.project(pw: point);
259
260 // create factor
261 ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2,
262 pointKey, K, body_P_sensor3);
263
264 // Use the factor to calculate the Jacobians
265 Matrix H1Actual, H2Actual, H3Actual;
266 factor.evaluateError(x: pose1, x: pose2, x: point, H&: H1Actual, H&: H2Actual, H&: H3Actual);
267
268 auto f = [&factor](const Pose3& p1, const Pose3& p2, const Point3& p3) {
269 return factor.evaluateError(x: p1, x: p2, x: p3);
270 };
271 // Expected Jacobians via numerical derivatives
272 Matrix H1Expected = numericalDerivative31<Vector, Pose3, Pose3, Point3>(h: f, x1: pose1, x2: pose2, x3: point);
273
274 Matrix H2Expected = numericalDerivative32<Vector, Pose3, Pose3, Point3>(h: f, x1: pose1, x2: pose2, x3: point);
275
276 Matrix H3Expected = numericalDerivative33<Vector, Pose3, Pose3, Point3>(h: f, x1: pose1, x2: pose2, x3: point);
277
278 CHECK(assert_equal(H1Expected, H1Actual, 1e-5));
279 CHECK(assert_equal(H2Expected, H2Actual, 1e-5));
280 CHECK(assert_equal(H3Expected, H3Actual, 1e-5));
281}
282
283/* ************************************************************************* */
284TEST(ProjectionFactorRollingShutter, cheirality) {
285 // Create measurement by projecting 3D landmark behind camera
286 double t = 0.3;
287 Pose3 pose1(Rot3::RzRyRx(x: 0.1, y: 0.0, z: 0.1), Point3(0, 0, 0));
288 Pose3 pose2(Rot3::RzRyRx(x: -0.1, y: -0.1, z: 0.0), Point3(0, 0, 1));
289 Pose3 poseInterp = interpolate<Pose3>(X: pose1, Y: pose2, t);
290 PinholeCamera<Cal3_S2> camera(poseInterp, *K);
291 Point3 point(0.0, 0.0, -5.0); // 5 meters behind the camera
292
293#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
294 Point2 measured = Point2(0.0, 0.0); // project would throw an exception
295 { // check that exception is thrown if we set throwCheirality = true
296 bool throwCheirality = true;
297 bool verboseCheirality = true;
298 ProjectionFactorRollingShutter factor(measured, t, model, poseKey1,
299 poseKey2, pointKey, K,
300 throwCheirality, verboseCheirality);
301 CHECK_EXCEPTION(factor.evaluateError(pose1, pose2, point),
302 CheiralityException);
303 }
304 { // check that exception is NOT thrown if we set throwCheirality = false,
305 // and outputs are correct
306 bool throwCheirality = false; // default
307 bool verboseCheirality = false; // default
308 ProjectionFactorRollingShutter factor(measured, t, model, poseKey1,
309 poseKey2, pointKey, K,
310 throwCheirality, verboseCheirality);
311
312 // Use the factor to calculate the error
313 Matrix H1Actual, H2Actual, H3Actual;
314 Vector actualError(factor.evaluateError(x: pose1, x: pose2, x: point, H&: H1Actual,
315 H&: H2Actual, H&: H3Actual));
316
317 // The expected error is zero
318 Vector expectedError = Vector2::Constant(
319 value: 2.0 * K->fx()); // this is what we return when point is behind camera
320
321 // Verify we get the expected error
322 CHECK(assert_equal(expectedError, actualError, 1e-9));
323 CHECK(assert_equal(Matrix::Zero(2, 6), H1Actual, 1e-5));
324 CHECK(assert_equal(Matrix::Zero(2, 6), H2Actual, 1e-5));
325 CHECK(assert_equal(Matrix::Zero(2, 3), H3Actual, 1e-5));
326 }
327#else
328 {
329 // everything is well defined, hence this matches the test "Jacobian" above:
330 Point2 measured = camera.project(point);
331
332 // create factor
333 ProjectionFactorRollingShutter factor(measured, t, model, poseKey1,
334 poseKey2, pointKey, K);
335
336 // Use the factor to calculate the Jacobians
337 Matrix H1Actual, H2Actual, H3Actual;
338 factor.evaluateError(pose1, pose2, point, H1Actual, H2Actual, H3Actual);
339
340 // Expected Jacobians via numerical derivatives
341 Matrix H1Expected = numericalDerivative31<Vector, Pose3, Pose3, Point3>(
342 std::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
343 std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor,
344 std::placeholders::_1, std::placeholders::_2,
345 std::placeholders::_3, {}, {},
346 {})),
347 pose1, pose2, point);
348
349 Matrix H2Expected = numericalDerivative32<Vector, Pose3, Pose3, Point3>(
350 std::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
351 std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor,
352 std::placeholders::_1, std::placeholders::_2,
353 std::placeholders::_3, {}, {},
354 {})),
355 pose1, pose2, point);
356
357 Matrix H3Expected = numericalDerivative33<Vector, Pose3, Pose3, Point3>(
358 std::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
359 std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor,
360 std::placeholders::_1, std::placeholders::_2,
361 std::placeholders::_3, {}, {},
362 {})),
363 pose1, pose2, point);
364
365 CHECK(assert_equal(H1Expected, H1Actual, 1e-5));
366 CHECK(assert_equal(H2Expected, H2Actual, 1e-5));
367 CHECK(assert_equal(H3Expected, H3Actual, 1e-5));
368 }
369#endif
370}
371
372/* ************************************************************************* */
373int main() {
374 TestResult tr;
375 return TestRegistry::runAllTests(result&: tr);
376}
377/* ************************************************************************* */
378