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 testSmartProjectionPoseFactorRollingShutter.cpp
14 * @brief Unit tests for SmartProjectionPoseFactorRollingShutter Class
15 * @author Luca Carlone
16 * @date July 2021
17 */
18
19#include <CppUnitLite/TestHarness.h>
20#include <gtsam/base/numericalDerivative.h>
21#include <gtsam/base/serializationTestHelpers.h>
22#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
23#include <gtsam/slam/PoseTranslationPrior.h>
24#include <gtsam/slam/ProjectionFactor.h>
25#include <gtsam_unstable/slam/ProjectionFactorRollingShutter.h>
26#include <gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h>
27
28#include <iostream>
29
30#include "gtsam/slam/tests/smartFactorScenarios.h"
31#define DISABLE_TIMING
32
33using namespace gtsam;
34using namespace std::placeholders;
35
36static const double rankTol = 1.0;
37// Create a noise model for the pixel error
38static const double sigma = 0.1;
39static SharedIsotropic model(noiseModel::Isotropic::Sigma(dim: 2, sigma));
40
41// Convenience for named keys
42using symbol_shorthand::L;
43using symbol_shorthand::X;
44
45// tests data
46static Symbol x1('X', 1);
47static Symbol x2('X', 2);
48static Symbol x3('X', 3);
49static Symbol x4('X', 4);
50static Symbol l0('L', 0);
51static Pose3 body_P_sensor =
52 Pose3(Rot3::Ypr(y: -0.1, p: 0.2, r: -0.2), Point3(0.1, 0.0, 0.0));
53
54static Point2 measurement1(323.0, 240.0);
55static Point2 measurement2(200.0, 220.0);
56static Point2 measurement3(320.0, 10.0);
57static double interp_factor = 0.5;
58static double interp_factor1 = 0.3;
59static double interp_factor2 = 0.4;
60static double interp_factor3 = 0.5;
61
62static size_t cameraId1 = 0;
63
64/* ************************************************************************* */
65// default Cal3_S2 poses with rolling shutter effect
66namespace vanillaPoseRS {
67typedef PinholePose<Cal3_S2> Camera;
68typedef CameraSet<Camera> Cameras;
69static Cal3_S2::shared_ptr sharedK(new Cal3_S2(fov, w, h));
70Pose3 interp_pose1 = interpolate<Pose3>(X: level_pose, Y: pose_right, t: interp_factor1);
71Pose3 interp_pose2 = interpolate<Pose3>(X: pose_right, Y: pose_above, t: interp_factor2);
72Pose3 interp_pose3 = interpolate<Pose3>(X: pose_above, Y: level_pose, t: interp_factor3);
73Camera cam1(interp_pose1, sharedK);
74Camera cam2(interp_pose2, sharedK);
75Camera cam3(interp_pose3, sharedK);
76SmartProjectionParams params(
77 gtsam::HESSIAN,
78 gtsam::ZERO_ON_DEGENERACY); // only config that works with RS factors
79} // namespace vanillaPoseRS
80
81LevenbergMarquardtParams lmParams;
82typedef SmartProjectionPoseFactorRollingShutter<PinholePose<Cal3_S2>>
83 SmartFactorRS;
84
85/* ************************************************************************* */
86TEST(SmartProjectionPoseFactorRollingShutter, Constructor) {
87 using namespace vanillaPoseRS;
88 std::shared_ptr<Cameras> cameraRig(new Cameras());
89 cameraRig->push_back(x: Camera(Pose3::Identity(), sharedK));
90 SmartFactorRS::shared_ptr factor1(
91 new SmartFactorRS(model, cameraRig, params));
92}
93
94/* ************************************************************************* */
95TEST(SmartProjectionPoseFactorRollingShutter, Constructor2) {
96 using namespace vanillaPoseRS;
97 std::shared_ptr<Cameras> cameraRig(new Cameras());
98 cameraRig->push_back(x: Camera(Pose3::Identity(), sharedK));
99 params.setRankTolerance(rankTol);
100 SmartFactorRS factor1(model, cameraRig, params);
101}
102
103/* ************************************************************************* */
104TEST(SmartProjectionPoseFactorRollingShutter, add) {
105 using namespace vanillaPoseRS;
106 std::shared_ptr<Cameras> cameraRig(new Cameras());
107 cameraRig->push_back(x: Camera(Pose3::Identity(), sharedK));
108 SmartFactorRS::shared_ptr factor1(
109 new SmartFactorRS(model, cameraRig, params));
110 factor1->add(measured: measurement1, world_P_body_key1: x1, world_P_body_key2: x2, alpha: interp_factor);
111}
112
113/* ************************************************************************* */
114TEST(SmartProjectionPoseFactorRollingShutter, Equals) {
115 using namespace vanillaPoseRS;
116
117 // create fake measurements
118 Point2Vector measurements;
119 measurements.push_back(x: measurement1);
120 measurements.push_back(x: measurement2);
121 measurements.push_back(x: measurement3);
122
123 std::vector<std::pair<Key, Key>> key_pairs;
124 key_pairs.push_back(x: std::make_pair(x&: x1, y&: x2));
125 key_pairs.push_back(x: std::make_pair(x&: x2, y&: x3));
126 key_pairs.push_back(x: std::make_pair(x&: x3, y&: x4));
127
128 std::vector<double> interp_factors;
129 interp_factors.push_back(x: interp_factor1);
130 interp_factors.push_back(x: interp_factor2);
131 interp_factors.push_back(x: interp_factor3);
132
133 FastVector<size_t> cameraIds{0, 0, 0};
134
135 std::shared_ptr<Cameras> cameraRig(new Cameras());
136 cameraRig->push_back(x: Camera(body_P_sensor, sharedK));
137
138 // create by adding a batch of measurements with a bunch of calibrations
139 SmartFactorRS::shared_ptr factor2(
140 new SmartFactorRS(model, cameraRig, params));
141 factor2->add(measurements, world_P_body_key_pairs: key_pairs, alphas: interp_factors, cameraIds);
142
143 // create by adding a batch of measurements with a single calibrations
144 SmartFactorRS::shared_ptr factor3(
145 new SmartFactorRS(model, cameraRig, params));
146 factor3->add(measurements, world_P_body_key_pairs: key_pairs, alphas: interp_factors, cameraIds);
147
148 { // create equal factors and show equal returns true
149 SmartFactorRS::shared_ptr factor1(
150 new SmartFactorRS(model, cameraRig, params));
151 factor1->add(measured: measurement1, world_P_body_key1: x1, world_P_body_key2: x2, alpha: interp_factor1, cameraId: cameraId1);
152 factor1->add(measured: measurement2, world_P_body_key1: x2, world_P_body_key2: x3, alpha: interp_factor2, cameraId: cameraId1);
153 factor1->add(measured: measurement3, world_P_body_key1: x3, world_P_body_key2: x4, alpha: interp_factor3, cameraId: cameraId1);
154
155 EXPECT(factor1->equals(*factor2));
156 EXPECT(factor1->equals(*factor3));
157 }
158 { // create equal factors and show equal returns true (use default cameraId)
159 SmartFactorRS::shared_ptr factor1(
160 new SmartFactorRS(model, cameraRig, params));
161 factor1->add(measured: measurement1, world_P_body_key1: x1, world_P_body_key2: x2, alpha: interp_factor1);
162 factor1->add(measured: measurement2, world_P_body_key1: x2, world_P_body_key2: x3, alpha: interp_factor2);
163 factor1->add(measured: measurement3, world_P_body_key1: x3, world_P_body_key2: x4, alpha: interp_factor3);
164
165 EXPECT(factor1->equals(*factor2));
166 EXPECT(factor1->equals(*factor3));
167 }
168 { // create equal factors and show equal returns true (use default cameraId)
169 SmartFactorRS::shared_ptr factor1(
170 new SmartFactorRS(model, cameraRig, params));
171 factor1->add(measurements, world_P_body_key_pairs: key_pairs, alphas: interp_factors);
172
173 EXPECT(factor1->equals(*factor2));
174 EXPECT(factor1->equals(*factor3));
175 }
176 { // create slightly different factors (different keys) and show equal
177 // returns false (use default cameraIds)
178 SmartFactorRS::shared_ptr factor1(
179 new SmartFactorRS(model, cameraRig, params));
180 factor1->add(measured: measurement1, world_P_body_key1: x1, world_P_body_key2: x2, alpha: interp_factor1, cameraId: cameraId1);
181 factor1->add(measured: measurement2, world_P_body_key1: x2, world_P_body_key2: x2, alpha: interp_factor2,
182 cameraId: cameraId1); // different!
183 factor1->add(measured: measurement3, world_P_body_key1: x3, world_P_body_key2: x4, alpha: interp_factor3, cameraId: cameraId1);
184
185 EXPECT(!factor1->equals(*factor2));
186 EXPECT(!factor1->equals(*factor3));
187 }
188 { // create slightly different factors (different extrinsics) and show equal
189 // returns false
190 std::shared_ptr<Cameras> cameraRig2(new Cameras());
191 cameraRig2->push_back(x: Camera(body_P_sensor * body_P_sensor, sharedK));
192 SmartFactorRS::shared_ptr factor1(
193 new SmartFactorRS(model, cameraRig2, params));
194 factor1->add(measured: measurement1, world_P_body_key1: x1, world_P_body_key2: x2, alpha: interp_factor1, cameraId: cameraId1);
195 factor1->add(measured: measurement2, world_P_body_key1: x2, world_P_body_key2: x3, alpha: interp_factor2,
196 cameraId: cameraId1); // different!
197 factor1->add(measured: measurement3, world_P_body_key1: x3, world_P_body_key2: x4, alpha: interp_factor3, cameraId: cameraId1);
198
199 EXPECT(!factor1->equals(*factor2));
200 EXPECT(!factor1->equals(*factor3));
201 }
202 { // create slightly different factors (different interp factors) and show
203 // equal returns false
204 SmartFactorRS::shared_ptr factor1(
205 new SmartFactorRS(model, cameraRig, params));
206 factor1->add(measured: measurement1, world_P_body_key1: x1, world_P_body_key2: x2, alpha: interp_factor1, cameraId: cameraId1);
207 factor1->add(measured: measurement2, world_P_body_key1: x2, world_P_body_key2: x3, alpha: interp_factor1,
208 cameraId: cameraId1); // different!
209 factor1->add(measured: measurement3, world_P_body_key1: x3, world_P_body_key2: x4, alpha: interp_factor3, cameraId: cameraId1);
210
211 EXPECT(!factor1->equals(*factor2));
212 EXPECT(!factor1->equals(*factor3));
213 }
214}
215
216static const int DimBlock = 12; ///< size of the variable stacking 2 poses from
217 ///< which the observation pose is interpolated
218static const int ZDim = 2; ///< Measurement dimension (Point2)
219typedef Eigen::Matrix<double, ZDim, DimBlock>
220 MatrixZD; // F blocks (derivatives wrt camera)
221typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD>>
222 FBlocks; // vector of F blocks
223
224/* *************************************************************************/
225TEST(SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians) {
226 using namespace vanillaPoseRS;
227
228 // Project two landmarks into two cameras
229 Point2 level_uv = cam1.project(pw: landmark1);
230 Point2 level_uv_right = cam2.project(pw: landmark1);
231 Pose3 body_P_sensorId = Pose3::Identity();
232
233 std::shared_ptr<Cameras> cameraRig(new Cameras());
234 cameraRig->push_back(x: Camera(body_P_sensorId, sharedK));
235
236 SmartFactorRS factor(model, cameraRig, params);
237 factor.add(measured: level_uv, world_P_body_key1: x1, world_P_body_key2: x2, alpha: interp_factor1);
238 factor.add(measured: level_uv_right, world_P_body_key1: x2, world_P_body_key2: x3, alpha: interp_factor2);
239
240 Values values; // it's a pose factor, hence these are poses
241 values.insert(j: x1, val: level_pose);
242 values.insert(j: x2, val: pose_right);
243 values.insert(j: x3, val: pose_above);
244
245 double actualError = factor.error(values);
246 double expectedError = 0.0;
247 EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7);
248
249 // Check triangulation
250 factor.triangulateSafe(cameras: factor.cameras(values));
251 TriangulationResult point = factor.point();
252 EXPECT(point.valid()); // check triangulated point is valid
253 EXPECT(assert_equal(
254 landmark1,
255 *point)); // check triangulation result matches expected 3D landmark
256
257 // Check Jacobians
258 // -- actual Jacobians
259 FBlocks actualFs;
260 Matrix actualE;
261 Vector actualb;
262 factor.computeJacobiansWithTriangulatedPoint(Fs&: actualFs, E&: actualE, b&: actualb,
263 values);
264 EXPECT(actualE.rows() == 4);
265 EXPECT(actualE.cols() == 3);
266 EXPECT(actualb.rows() == 4);
267 EXPECT(actualb.cols() == 1);
268 EXPECT(actualFs.size() == 2);
269
270 // -- expected Jacobians from ProjectionFactorsRollingShutter
271 ProjectionFactorRollingShutter factor1(level_uv, interp_factor1, model, x1,
272 x2, l0, sharedK, body_P_sensorId);
273 Matrix expectedF11, expectedF12, expectedE1;
274 Vector expectedb1 = factor1.evaluateError(
275 x: level_pose, x: pose_right, x: landmark1, H&: expectedF11, H&: expectedF12, H&: expectedE1);
276 EXPECT(
277 assert_equal(expectedF11, Matrix(actualFs[0].block(0, 0, 2, 6)), 1e-5));
278 EXPECT(
279 assert_equal(expectedF12, Matrix(actualFs[0].block(0, 6, 2, 6)), 1e-5));
280 EXPECT(assert_equal(expectedE1, Matrix(actualE.block(0, 0, 2, 3)), 1e-5));
281 // by definition computeJacobiansWithTriangulatedPoint returns minus
282 // reprojectionError
283 EXPECT(assert_equal(expectedb1, -Vector(actualb.segment<2>(0)), 1e-5));
284
285 ProjectionFactorRollingShutter factor2(level_uv_right, interp_factor2, model,
286 x2, x3, l0, sharedK, body_P_sensorId);
287 Matrix expectedF21, expectedF22, expectedE2;
288 Vector expectedb2 = factor2.evaluateError(
289 x: pose_right, x: pose_above, x: landmark1, H&: expectedF21, H&: expectedF22, H&: expectedE2);
290 EXPECT(
291 assert_equal(expectedF21, Matrix(actualFs[1].block(0, 0, 2, 6)), 1e-5));
292 EXPECT(
293 assert_equal(expectedF22, Matrix(actualFs[1].block(0, 6, 2, 6)), 1e-5));
294 EXPECT(assert_equal(expectedE2, Matrix(actualE.block(2, 0, 2, 3)), 1e-5));
295 // by definition computeJacobiansWithTriangulatedPoint returns minus
296 // reprojectionError
297 EXPECT(assert_equal(expectedb2, -Vector(actualb.segment<2>(2)), 1e-5));
298}
299
300/* *************************************************************************/
301TEST(SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians) {
302 // also includes non-identical extrinsic calibration
303 using namespace vanillaPoseRS;
304
305 // Project two landmarks into two cameras
306 Point2 pixelError(0.5, 1.0);
307 Point2 level_uv = cam1.project(pw: landmark1) + pixelError;
308 Point2 level_uv_right = cam2.project(pw: landmark1);
309 Pose3 body_P_sensorNonId = body_P_sensor;
310
311 std::shared_ptr<Cameras> cameraRig(new Cameras());
312 cameraRig->push_back(x: Camera(body_P_sensorNonId, sharedK));
313
314 SmartFactorRS factor(model, cameraRig, params);
315 factor.add(measured: level_uv, world_P_body_key1: x1, world_P_body_key2: x2, alpha: interp_factor1);
316 factor.add(measured: level_uv_right, world_P_body_key1: x2, world_P_body_key2: x3, alpha: interp_factor2);
317
318 Values values; // it's a pose factor, hence these are poses
319 values.insert(j: x1, val: level_pose);
320 values.insert(j: x2, val: pose_right);
321 values.insert(j: x3, val: pose_above);
322
323 // Perform triangulation
324 factor.triangulateSafe(cameras: factor.cameras(values));
325 TriangulationResult point = factor.point();
326 EXPECT(point.valid()); // check triangulated point is valid
327 Point3 landmarkNoisy = *point;
328
329 // Check Jacobians
330 // -- actual Jacobians
331 FBlocks actualFs;
332 Matrix actualE;
333 Vector actualb;
334 factor.computeJacobiansWithTriangulatedPoint(Fs&: actualFs, E&: actualE, b&: actualb,
335 values);
336 EXPECT(actualE.rows() == 4);
337 EXPECT(actualE.cols() == 3);
338 EXPECT(actualb.rows() == 4);
339 EXPECT(actualb.cols() == 1);
340 EXPECT(actualFs.size() == 2);
341
342 // -- expected Jacobians from ProjectionFactorsRollingShutter
343 ProjectionFactorRollingShutter factor1(level_uv, interp_factor1, model, x1,
344 x2, l0, sharedK, body_P_sensorNonId);
345 Matrix expectedF11, expectedF12, expectedE1;
346 Vector expectedb1 =
347 factor1.evaluateError(x: level_pose, x: pose_right, x: landmarkNoisy, H&: expectedF11,
348 H&: expectedF12, H&: expectedE1);
349 EXPECT(
350 assert_equal(expectedF11, Matrix(actualFs[0].block(0, 0, 2, 6)), 1e-5));
351 EXPECT(
352 assert_equal(expectedF12, Matrix(actualFs[0].block(0, 6, 2, 6)), 1e-5));
353 EXPECT(assert_equal(expectedE1, Matrix(actualE.block(0, 0, 2, 3)), 1e-5));
354 // by definition computeJacobiansWithTriangulatedPoint returns minus
355 // reprojectionError
356 EXPECT(assert_equal(expectedb1, -Vector(actualb.segment<2>(0)), 1e-5));
357
358 ProjectionFactorRollingShutter factor2(level_uv_right, interp_factor2, model,
359 x2, x3, l0, sharedK,
360 body_P_sensorNonId);
361 Matrix expectedF21, expectedF22, expectedE2;
362 Vector expectedb2 =
363 factor2.evaluateError(x: pose_right, x: pose_above, x: landmarkNoisy, H&: expectedF21,
364 H&: expectedF22, H&: expectedE2);
365 EXPECT(
366 assert_equal(expectedF21, Matrix(actualFs[1].block(0, 0, 2, 6)), 1e-5));
367 EXPECT(
368 assert_equal(expectedF22, Matrix(actualFs[1].block(0, 6, 2, 6)), 1e-5));
369 EXPECT(assert_equal(expectedE2, Matrix(actualE.block(2, 0, 2, 3)), 1e-5));
370 // by definition computeJacobiansWithTriangulatedPoint returns minus
371 // reprojectionError
372 EXPECT(assert_equal(expectedb2, -Vector(actualb.segment<2>(2)), 1e-5));
373
374 // Check errors
375 double actualError = factor.error(values); // from smart factor
376 NonlinearFactorGraph nfg;
377 nfg.add(factorOrContainer: factor1);
378 nfg.add(factorOrContainer: factor2);
379 values.insert(j: l0, val: landmarkNoisy);
380 double expectedError = nfg.error(values);
381 EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7);
382}
383
384/* *************************************************************************/
385TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses) {
386 using namespace vanillaPoseRS;
387 Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3;
388
389 // Project three landmarks into three cameras
390 projectToMultipleCameras(cam1, cam2, cam3, landmark: landmark1, measurements_cam&: measurements_lmk1);
391 projectToMultipleCameras(cam1, cam2, cam3, landmark: landmark2, measurements_cam&: measurements_lmk2);
392 projectToMultipleCameras(cam1, cam2, cam3, landmark: landmark3, measurements_cam&: measurements_lmk3);
393
394 // create inputs
395 std::vector<std::pair<Key, Key>> key_pairs;
396 key_pairs.push_back(x: std::make_pair(x&: x1, y&: x2));
397 key_pairs.push_back(x: std::make_pair(x&: x2, y&: x3));
398 key_pairs.push_back(x: std::make_pair(x&: x3, y&: x1));
399
400 std::vector<double> interp_factors;
401 interp_factors.push_back(x: interp_factor1);
402 interp_factors.push_back(x: interp_factor2);
403 interp_factors.push_back(x: interp_factor3);
404
405 std::shared_ptr<Cameras> cameraRig(new Cameras());
406 cameraRig->push_back(x: Camera(Pose3::Identity(), sharedK));
407
408 SmartFactorRS::shared_ptr smartFactor1(
409 new SmartFactorRS(model, cameraRig, params));
410 smartFactor1->add(measurements: measurements_lmk1, world_P_body_key_pairs: key_pairs, alphas: interp_factors);
411
412 SmartFactorRS::shared_ptr smartFactor2(
413 new SmartFactorRS(model, cameraRig, params));
414 smartFactor2->add(measurements: measurements_lmk2, world_P_body_key_pairs: key_pairs, alphas: interp_factors);
415
416 SmartFactorRS::shared_ptr smartFactor3(
417 new SmartFactorRS(model, cameraRig, params));
418 smartFactor3->add(measurements: measurements_lmk3, world_P_body_key_pairs: key_pairs, alphas: interp_factors);
419
420 const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(dim: 6, sigma: 0.10);
421
422 NonlinearFactorGraph graph;
423 graph.push_back(factor: smartFactor1);
424 graph.push_back(factor: smartFactor2);
425 graph.push_back(factor: smartFactor3);
426 graph.addPrior(key: x1, prior: level_pose, model: noisePrior);
427 graph.addPrior(key: x2, prior: pose_right, model: noisePrior);
428
429 Values groundTruth;
430 groundTruth.insert(j: x1, val: level_pose);
431 groundTruth.insert(j: x2, val: pose_right);
432 groundTruth.insert(j: x3, val: pose_above);
433 DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9);
434
435 // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10),
436 // Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
437 Pose3 noise_pose = Pose3(Rot3::Ypr(y: -M_PI / 100, p: 0., r: -M_PI / 100),
438 Point3(0.1, 0.1, 0.1)); // smaller noise
439 Values values;
440 values.insert(j: x1, val: level_pose);
441 values.insert(j: x2, val: pose_right);
442 // initialize third pose with some noise, we expect it to move back to
443 // original pose_above
444 values.insert(j: x3, val: pose_above * noise_pose);
445 EXPECT( // check that the pose is actually noisy
446 assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656,
447 -0.0313952598, -0.000986635786, 0.0314107591,
448 -0.999013364, -0.0313952598),
449 Point3(0.1, -0.1, 1.9)),
450 values.at<Pose3>(x3)));
451
452 Values result;
453 LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
454 result = optimizer.optimize();
455 EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-6));
456}
457
458/* *************************************************************************/
459TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_multiCam) {
460 using namespace vanillaPoseRS;
461 Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3;
462
463 // Project three landmarks into three cameras
464 projectToMultipleCameras(cam1, cam2, cam3, landmark: landmark1, measurements_cam&: measurements_lmk1);
465 projectToMultipleCameras(cam1, cam2, cam3, landmark: landmark2, measurements_cam&: measurements_lmk2);
466 projectToMultipleCameras(cam1, cam2, cam3, landmark: landmark3, measurements_cam&: measurements_lmk3);
467
468 // create inputs
469 std::vector<std::pair<Key, Key>> key_pairs;
470 key_pairs.push_back(x: std::make_pair(x&: x1, y&: x2));
471 key_pairs.push_back(x: std::make_pair(x&: x2, y&: x3));
472 key_pairs.push_back(x: std::make_pair(x&: x3, y&: x1));
473
474 std::vector<double> interp_factors;
475 interp_factors.push_back(x: interp_factor1);
476 interp_factors.push_back(x: interp_factor2);
477 interp_factors.push_back(x: interp_factor3);
478
479 std::shared_ptr<Cameras> cameraRig(new Cameras());
480 cameraRig->push_back(x: Camera(body_P_sensor, sharedK));
481 cameraRig->push_back(x: Camera(Pose3::Identity(), sharedK));
482
483 SmartFactorRS::shared_ptr smartFactor1(
484 new SmartFactorRS(model, cameraRig, params));
485 smartFactor1->add(measurements: measurements_lmk1, world_P_body_key_pairs: key_pairs, alphas: interp_factors, cameraIds: {1, 1, 1});
486
487 SmartFactorRS::shared_ptr smartFactor2(
488 new SmartFactorRS(model, cameraRig, params));
489 smartFactor2->add(measurements: measurements_lmk2, world_P_body_key_pairs: key_pairs, alphas: interp_factors, cameraIds: {1, 1, 1});
490
491 SmartFactorRS::shared_ptr smartFactor3(
492 new SmartFactorRS(model, cameraRig, params));
493 smartFactor3->add(measurements: measurements_lmk3, world_P_body_key_pairs: key_pairs, alphas: interp_factors, cameraIds: {1, 1, 1});
494
495 const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(dim: 6, sigma: 0.10);
496
497 NonlinearFactorGraph graph;
498 graph.push_back(factor: smartFactor1);
499 graph.push_back(factor: smartFactor2);
500 graph.push_back(factor: smartFactor3);
501 graph.addPrior(key: x1, prior: level_pose, model: noisePrior);
502 graph.addPrior(key: x2, prior: pose_right, model: noisePrior);
503
504 Values groundTruth;
505 groundTruth.insert(j: x1, val: level_pose);
506 groundTruth.insert(j: x2, val: pose_right);
507 groundTruth.insert(j: x3, val: pose_above); // pose above is the pose of the camera
508 DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9);
509
510 // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10),
511 // Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
512 Pose3 noise_pose = Pose3(Rot3::Ypr(y: -M_PI / 100, p: 0., r: -M_PI / 100),
513 Point3(0.1, 0.1, 0.1)); // smaller noise
514 Values values;
515 values.insert(j: x1, val: level_pose);
516 values.insert(j: x2, val: pose_right);
517 // initialize third pose with some noise, we expect it to move back to
518 // original pose_above
519 values.insert(j: x3, val: pose_above * noise_pose);
520 EXPECT( // check that the pose is actually noisy
521 assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656,
522 -0.0313952598, -0.000986635786, 0.0314107591,
523 -0.999013364, -0.0313952598),
524 Point3(0.1, -0.1, 1.9)),
525 values.at<Pose3>(x3)));
526
527 Values result;
528 LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
529 result = optimizer.optimize();
530 EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-6));
531}
532
533/* *************************************************************************/
534TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_multiCam2) {
535 using namespace vanillaPoseRS;
536
537 Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3;
538
539 // create arbitrary body_T_sensor (transforms from sensor to body)
540 Pose3 body_T_sensor1 = Pose3(Rot3::Ypr(y: -0.03, p: 0., r: 0.01), Point3(1, 1, 1));
541 Pose3 body_T_sensor2 = Pose3(Rot3::Ypr(y: -0.1, p: 0., r: 0.05), Point3(0, 0, 1));
542 Pose3 body_T_sensor3 = Pose3(Rot3::Ypr(y: -0.3, p: 0., r: -0.05), Point3(0, 1, 1));
543
544 Camera camera1(interp_pose1 * body_T_sensor1, sharedK);
545 Camera camera2(interp_pose2 * body_T_sensor2, sharedK);
546 Camera camera3(interp_pose3 * body_T_sensor3, sharedK);
547
548 // Project three landmarks into three cameras
549 projectToMultipleCameras(cam1: camera1, cam2: camera2, cam3: camera3, landmark: landmark1,
550 measurements_cam&: measurements_lmk1);
551 projectToMultipleCameras(cam1: camera1, cam2: camera2, cam3: camera3, landmark: landmark2,
552 measurements_cam&: measurements_lmk2);
553 projectToMultipleCameras(cam1: camera1, cam2: camera2, cam3: camera3, landmark: landmark3,
554 measurements_cam&: measurements_lmk3);
555
556 // create inputs
557 std::vector<std::pair<Key, Key>> key_pairs;
558 key_pairs.push_back(x: std::make_pair(x&: x1, y&: x2));
559 key_pairs.push_back(x: std::make_pair(x&: x2, y&: x3));
560 key_pairs.push_back(x: std::make_pair(x&: x3, y&: x1));
561
562 std::vector<double> interp_factors;
563 interp_factors.push_back(x: interp_factor1);
564 interp_factors.push_back(x: interp_factor2);
565 interp_factors.push_back(x: interp_factor3);
566
567 std::shared_ptr<Cameras> cameraRig(new Cameras());
568 cameraRig->push_back(x: Camera(body_T_sensor1, sharedK));
569 cameraRig->push_back(x: Camera(body_T_sensor2, sharedK));
570 cameraRig->push_back(x: Camera(body_T_sensor3, sharedK));
571
572 SmartFactorRS::shared_ptr smartFactor1(
573 new SmartFactorRS(model, cameraRig, params));
574 smartFactor1->add(measurements: measurements_lmk1, world_P_body_key_pairs: key_pairs, alphas: interp_factors, cameraIds: {0, 1, 2});
575
576 SmartFactorRS::shared_ptr smartFactor2(
577 new SmartFactorRS(model, cameraRig, params));
578 smartFactor2->add(measurements: measurements_lmk2, world_P_body_key_pairs: key_pairs, alphas: interp_factors, cameraIds: {0, 1, 2});
579
580 SmartFactorRS::shared_ptr smartFactor3(
581 new SmartFactorRS(model, cameraRig, params));
582 smartFactor3->add(measurements: measurements_lmk3, world_P_body_key_pairs: key_pairs, alphas: interp_factors, cameraIds: {0, 1, 2});
583
584 const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(dim: 6, sigma: 0.10);
585
586 NonlinearFactorGraph graph;
587 graph.push_back(factor: smartFactor1);
588 graph.push_back(factor: smartFactor2);
589 graph.push_back(factor: smartFactor3);
590 graph.addPrior(key: x1, prior: level_pose, model: noisePrior);
591 graph.addPrior(key: x2, prior: pose_right, model: noisePrior);
592
593 Values groundTruth;
594 groundTruth.insert(j: x1, val: level_pose);
595 groundTruth.insert(j: x2, val: pose_right);
596 groundTruth.insert(j: x3, val: pose_above); // pose above is the pose of the camera
597 DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9);
598
599 // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10),
600 // Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
601 Pose3 noise_pose = Pose3(Rot3::Ypr(y: -M_PI / 100, p: 0., r: -M_PI / 100),
602 Point3(0.1, 0.1, 0.1)); // smaller noise
603 Values values;
604 values.insert(j: x1, val: level_pose);
605 values.insert(j: x2, val: pose_right);
606 // initialize third pose with some noise, we expect it to move back to
607 // original pose_above
608 values.insert(j: x3, val: pose_above * noise_pose);
609 EXPECT( // check that the pose is actually noisy
610 assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656,
611 -0.0313952598, -0.000986635786, 0.0314107591,
612 -0.999013364, -0.0313952598),
613 Point3(0.1, -0.1, 1.9)),
614 values.at<Pose3>(x3)));
615
616 Values result;
617 LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
618 result = optimizer.optimize();
619 EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-4));
620}
621
622/* *************************************************************************/
623TEST(SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses) {
624 // here we replicate a test in SmartProjectionPoseFactor by setting
625 // interpolation factors to 0 and 1 (such that the rollingShutter measurements
626 // falls back to standard pixel measurements) Note: this is a quite extreme
627 // test since in typical camera you would not have more than 1 measurement per
628 // landmark at each interpolated pose
629 using namespace vanillaPoseRS;
630
631 // Default cameras for simple derivatives
632 static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0));
633
634 Rot3 R = Rot3::Identity();
635 Pose3 pose1 = Pose3(R, Point3(0, 0, 0));
636 Pose3 pose2 = Pose3(R, Point3(1, 0, 0));
637 Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple);
638 Pose3 body_P_sensorId = Pose3::Identity();
639
640 // one landmarks 1m in front of camera
641 Point3 landmark1(0, 0, 10);
642
643 Point2Vector measurements_lmk1;
644
645 // Project 2 landmarks into 2 cameras
646 measurements_lmk1.push_back(x: cam1.project(pw: landmark1));
647 measurements_lmk1.push_back(x: cam2.project(pw: landmark1));
648
649 std::shared_ptr<Cameras> cameraRig(new Cameras());
650 cameraRig->push_back(x: Camera(body_P_sensorId, sharedKSimple));
651
652 SmartFactorRS::shared_ptr smartFactor1(
653 new SmartFactorRS(model, cameraRig, params));
654 double interp_factor = 0; // equivalent to measurement taken at pose 1
655 smartFactor1->add(measured: measurements_lmk1[0], world_P_body_key1: x1, world_P_body_key2: x2, alpha: interp_factor);
656 interp_factor = 1; // equivalent to measurement taken at pose 2
657 smartFactor1->add(measured: measurements_lmk1[1], world_P_body_key1: x1, world_P_body_key2: x2, alpha: interp_factor);
658
659 SmartFactorRS::Cameras cameras;
660 cameras.push_back(x: cam1);
661 cameras.push_back(x: cam2);
662
663 // Make sure triangulation works
664 EXPECT(smartFactor1->triangulateSafe(cameras));
665 EXPECT(!smartFactor1->isDegenerate());
666 EXPECT(!smartFactor1->isPointBehindCamera());
667 std::optional<Point3> p = smartFactor1->point();
668 EXPECT(p);
669 EXPECT(assert_equal(landmark1, *p));
670
671 VectorValues zeroDelta;
672 Vector6 delta;
673 delta.setZero();
674 zeroDelta.insert(j: x1, value: delta);
675 zeroDelta.insert(j: x2, value: delta);
676
677 VectorValues perturbedDelta;
678 delta.setOnes();
679 perturbedDelta.insert(j: x1, value: delta);
680 perturbedDelta.insert(j: x2, value: delta);
681 double expectedError = 2500;
682
683 // After eliminating the point, A1 and A2 contain 2-rank information on
684 // cameras:
685 Matrix16 A1, A2;
686 A1 << -10, 0, 0, 0, 1, 0;
687 A2 << 10, 0, 1, 0, -1, 0;
688 A1 *= 10. / sigma;
689 A2 *= 10. / sigma;
690 Matrix expectedInformation; // filled below
691
692 // createHessianFactor
693 Matrix66 G11 = 0.5 * A1.transpose() * A1;
694 Matrix66 G12 = 0.5 * A1.transpose() * A2;
695 Matrix66 G22 = 0.5 * A2.transpose() * A2;
696
697 Vector6 g1;
698 g1.setZero();
699 Vector6 g2;
700 g2.setZero();
701
702 double f = 0;
703
704 RegularHessianFactor<6> expected(x1, x2, G11, G12, g1, G22, g2, f);
705 expectedInformation = expected.information();
706
707 Values values;
708 values.insert(j: x1, val: pose1);
709 values.insert(j: x2, val: pose2);
710 std::shared_ptr<RegularHessianFactor<6>> actual =
711 smartFactor1->createHessianFactor(values);
712 EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6));
713 EXPECT(assert_equal(expected, *actual, 1e-6));
714 EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6);
715 EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6);
716}
717
718/* *************************************************************************/
719TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI) {
720 using namespace vanillaPoseRS;
721 Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3;
722
723 // Project three landmarks into three cameras
724 projectToMultipleCameras(cam1, cam2, cam3, landmark: landmark1, measurements_cam&: measurements_lmk1);
725 projectToMultipleCameras(cam1, cam2, cam3, landmark: landmark2, measurements_cam&: measurements_lmk2);
726 projectToMultipleCameras(cam1, cam2, cam3, landmark: landmark3, measurements_cam&: measurements_lmk3);
727
728 // create inputs
729 std::vector<std::pair<Key, Key>> key_pairs;
730 key_pairs.push_back(x: std::make_pair(x&: x1, y&: x2));
731 key_pairs.push_back(x: std::make_pair(x&: x2, y&: x3));
732 key_pairs.push_back(x: std::make_pair(x&: x3, y&: x1));
733
734 std::vector<double> interp_factors;
735 interp_factors.push_back(x: interp_factor1);
736 interp_factors.push_back(x: interp_factor2);
737 interp_factors.push_back(x: interp_factor3);
738
739 double excludeLandmarksFutherThanDist = 1e10; // very large
740 SmartProjectionParams params;
741 params.setRankTolerance(1.0);
742 params.setLinearizationMode(gtsam::HESSIAN);
743 params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY);
744 params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist);
745 params.setEnableEPI(true);
746
747 std::shared_ptr<Cameras> cameraRig(new Cameras());
748 cameraRig->push_back(x: Camera(Pose3::Identity(), sharedK));
749
750 SmartFactorRS smartFactor1(model, cameraRig, params);
751 smartFactor1.add(measurements: measurements_lmk1, world_P_body_key_pairs: key_pairs, alphas: interp_factors);
752
753 SmartFactorRS smartFactor2(model, cameraRig, params);
754 smartFactor2.add(measurements: measurements_lmk2, world_P_body_key_pairs: key_pairs, alphas: interp_factors);
755
756 SmartFactorRS smartFactor3(model, cameraRig, params);
757 smartFactor3.add(measurements: measurements_lmk3, world_P_body_key_pairs: key_pairs, alphas: interp_factors);
758
759 const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(dim: 6, sigma: 0.10);
760
761 NonlinearFactorGraph graph;
762 graph.push_back(factor: smartFactor1);
763 graph.push_back(factor: smartFactor2);
764 graph.push_back(factor: smartFactor3);
765 graph.addPrior(key: x1, prior: level_pose, model: noisePrior);
766 graph.addPrior(key: x2, prior: pose_right, model: noisePrior);
767
768 Pose3 noise_pose = Pose3(Rot3::Ypr(y: -M_PI / 100, p: 0., r: -M_PI / 100),
769 Point3(0.1, 0.1, 0.1)); // smaller noise
770 Values values;
771 values.insert(j: x1, val: level_pose);
772 values.insert(j: x2, val: pose_right);
773 // initialize third pose with some noise, we expect it to move back to
774 // original pose_above
775 values.insert(j: x3, val: pose_above * noise_pose);
776
777 // Optimization should correct 3rd pose
778 Values result;
779 LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
780 result = optimizer.optimize();
781 EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-6));
782}
783
784/* *************************************************************************/
785TEST(SmartProjectionPoseFactorRollingShutter,
786 optimization_3poses_landmarkDistance) {
787 using namespace vanillaPoseRS;
788 Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3;
789
790 // Project three landmarks into three cameras
791 projectToMultipleCameras(cam1, cam2, cam3, landmark: landmark1, measurements_cam&: measurements_lmk1);
792 projectToMultipleCameras(cam1, cam2, cam3, landmark: landmark2, measurements_cam&: measurements_lmk2);
793 projectToMultipleCameras(cam1, cam2, cam3, landmark: landmark3, measurements_cam&: measurements_lmk3);
794
795 // create inputs
796 std::vector<std::pair<Key, Key>> key_pairs;
797 key_pairs.push_back(x: std::make_pair(x&: x1, y&: x2));
798 key_pairs.push_back(x: std::make_pair(x&: x2, y&: x3));
799 key_pairs.push_back(x: std::make_pair(x&: x3, y&: x1));
800
801 std::vector<double> interp_factors;
802 interp_factors.push_back(x: interp_factor1);
803 interp_factors.push_back(x: interp_factor2);
804 interp_factors.push_back(x: interp_factor3);
805
806 double excludeLandmarksFutherThanDist = 2;
807 SmartProjectionParams params;
808 params.setRankTolerance(1.0);
809 params.setLinearizationMode(gtsam::HESSIAN);
810 // params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); // this would give an
811 // exception as expected
812 params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY);
813 params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist);
814 params.setEnableEPI(false);
815
816 std::shared_ptr<Cameras> cameraRig(new Cameras());
817 cameraRig->push_back(x: Camera(Pose3::Identity(), sharedK));
818
819 SmartFactorRS smartFactor1(model, cameraRig, params);
820 smartFactor1.add(measurements: measurements_lmk1, world_P_body_key_pairs: key_pairs, alphas: interp_factors);
821
822 SmartFactorRS smartFactor2(model, cameraRig, params);
823 smartFactor2.add(measurements: measurements_lmk2, world_P_body_key_pairs: key_pairs, alphas: interp_factors);
824
825 SmartFactorRS smartFactor3(model, cameraRig, params);
826 smartFactor3.add(measurements: measurements_lmk3, world_P_body_key_pairs: key_pairs, alphas: interp_factors);
827
828 const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(dim: 6, sigma: 0.10);
829
830 NonlinearFactorGraph graph;
831 graph.push_back(factor: smartFactor1);
832 graph.push_back(factor: smartFactor2);
833 graph.push_back(factor: smartFactor3);
834 graph.addPrior(key: x1, prior: level_pose, model: noisePrior);
835 graph.addPrior(key: x2, prior: pose_right, model: noisePrior);
836
837 Pose3 noise_pose = Pose3(Rot3::Ypr(y: -M_PI / 100, p: 0., r: -M_PI / 100),
838 Point3(0.1, 0.1, 0.1)); // smaller noise
839 Values values;
840 values.insert(j: x1, val: level_pose);
841 values.insert(j: x2, val: pose_right);
842 // initialize third pose with some noise, we expect it to move back to
843 // original pose_above
844 values.insert(j: x3, val: pose_above * noise_pose);
845
846 // All factors are disabled (due to the distance threshold) and pose should
847 // remain where it is
848 Values result;
849 LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
850 result = optimizer.optimize();
851 EXPECT(assert_equal(values.at<Pose3>(x3), result.at<Pose3>(x3)));
852}
853
854/* *************************************************************************/
855TEST(SmartProjectionPoseFactorRollingShutter,
856 optimization_3poses_dynamicOutlierRejection) {
857 using namespace vanillaPoseRS;
858 // add fourth landmark
859 Point3 landmark4(5, -0.5, 1);
860
861 Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3,
862 measurements_lmk4;
863 // Project 4 landmarks into cameras
864 projectToMultipleCameras(cam1, cam2, cam3, landmark: landmark1, measurements_cam&: measurements_lmk1);
865 projectToMultipleCameras(cam1, cam2, cam3, landmark: landmark2, measurements_cam&: measurements_lmk2);
866 projectToMultipleCameras(cam1, cam2, cam3, landmark: landmark3, measurements_cam&: measurements_lmk3);
867 projectToMultipleCameras(cam1, cam2, cam3, landmark: landmark4, measurements_cam&: measurements_lmk4);
868 measurements_lmk4.at(n: 0) =
869 measurements_lmk4.at(n: 0) + Point2(10, 10); // add outlier
870
871 // create inputs
872 std::vector<std::pair<Key, Key>> key_pairs;
873 key_pairs.push_back(x: std::make_pair(x&: x1, y&: x2));
874 key_pairs.push_back(x: std::make_pair(x&: x2, y&: x3));
875 key_pairs.push_back(x: std::make_pair(x&: x3, y&: x1));
876
877 std::vector<double> interp_factors;
878 interp_factors.push_back(x: interp_factor1);
879 interp_factors.push_back(x: interp_factor2);
880 interp_factors.push_back(x: interp_factor3);
881
882 double excludeLandmarksFutherThanDist = 1e10;
883 double dynamicOutlierRejectionThreshold =
884 3; // max 3 pixel of average reprojection error
885
886 SmartProjectionParams params;
887 params.setRankTolerance(1.0);
888 params.setLinearizationMode(gtsam::HESSIAN);
889 params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY);
890 params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist);
891 params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold);
892 params.setEnableEPI(false);
893
894 std::shared_ptr<Cameras> cameraRig(new Cameras());
895 cameraRig->push_back(x: Camera(Pose3::Identity(), sharedK));
896
897 SmartFactorRS::shared_ptr smartFactor1(
898 new SmartFactorRS(model, cameraRig, params));
899 smartFactor1->add(measurements: measurements_lmk1, world_P_body_key_pairs: key_pairs, alphas: interp_factors);
900
901 SmartFactorRS::shared_ptr smartFactor2(
902 new SmartFactorRS(model, cameraRig, params));
903 smartFactor2->add(measurements: measurements_lmk2, world_P_body_key_pairs: key_pairs, alphas: interp_factors);
904
905 SmartFactorRS::shared_ptr smartFactor3(
906 new SmartFactorRS(model, cameraRig, params));
907 smartFactor3->add(measurements: measurements_lmk3, world_P_body_key_pairs: key_pairs, alphas: interp_factors);
908
909 SmartFactorRS::shared_ptr smartFactor4(
910 new SmartFactorRS(model, cameraRig, params));
911 smartFactor4->add(measurements: measurements_lmk4, world_P_body_key_pairs: key_pairs, alphas: interp_factors);
912
913 const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(dim: 6, sigma: 0.10);
914
915 NonlinearFactorGraph graph;
916 graph.push_back(factor: smartFactor1);
917 graph.push_back(factor: smartFactor2);
918 graph.push_back(factor: smartFactor3);
919 graph.push_back(factor: smartFactor4);
920 graph.addPrior(key: x1, prior: level_pose, model: noisePrior);
921 graph.addPrior(key: x2, prior: pose_right, model: noisePrior);
922
923 Pose3 noise_pose = Pose3(
924 Rot3::Ypr(y: -M_PI / 100, p: 0., r: -M_PI / 100),
925 Point3(0.01, 0.01,
926 0.01)); // smaller noise, otherwise outlier rejection will kick in
927 Values values;
928 values.insert(j: x1, val: level_pose);
929 values.insert(j: x2, val: pose_right);
930 // initialize third pose with some noise, we expect it to move back to
931 // original pose_above
932 values.insert(j: x3, val: pose_above * noise_pose);
933
934 // Optimization should correct 3rd pose
935 Values result;
936 LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
937 result = optimizer.optimize();
938 EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-6));
939}
940
941/* *************************************************************************/
942TEST(SmartProjectionPoseFactorRollingShutter,
943 hessianComparedToProjFactorsRollingShutter) {
944 using namespace vanillaPoseRS;
945 Point2Vector measurements_lmk1;
946
947 // Project three landmarks into three cameras
948 projectToMultipleCameras(cam1, cam2, cam3, landmark: landmark1, measurements_cam&: measurements_lmk1);
949
950 // create inputs
951 std::vector<std::pair<Key, Key>> key_pairs;
952 key_pairs.push_back(x: std::make_pair(x&: x1, y&: x2));
953 key_pairs.push_back(x: std::make_pair(x&: x2, y&: x3));
954 key_pairs.push_back(x: std::make_pair(x&: x3, y&: x1));
955
956 std::vector<double> interp_factors;
957 interp_factors.push_back(x: interp_factor1);
958 interp_factors.push_back(x: interp_factor2);
959 interp_factors.push_back(x: interp_factor3);
960
961 std::shared_ptr<Cameras> cameraRig(new Cameras());
962 cameraRig->push_back(x: Camera(Pose3::Identity(), sharedK));
963
964 SmartFactorRS::shared_ptr smartFactor1(
965 new SmartFactorRS(model, cameraRig, params));
966 smartFactor1->add(measurements: measurements_lmk1, world_P_body_key_pairs: key_pairs, alphas: interp_factors);
967
968 Pose3 noise_pose = Pose3(Rot3::Ypr(y: -M_PI / 100, p: 0., r: -M_PI / 100),
969 Point3(0.1, 0.1, 0.1)); // smaller noise
970 Values values;
971 values.insert(j: x1, val: level_pose);
972 values.insert(j: x2, val: pose_right);
973 // initialize third pose with some noise to get a nontrivial linearization
974 // point
975 values.insert(j: x3, val: pose_above * noise_pose);
976 EXPECT( // check that the pose is actually noisy
977 assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656,
978 -0.0313952598, -0.000986635786, 0.0314107591,
979 -0.999013364, -0.0313952598),
980 Point3(0.1, -0.1, 1.9)),
981 values.at<Pose3>(x3)));
982
983 // linearization point for the poses
984 Pose3 pose1 = level_pose;
985 Pose3 pose2 = pose_right;
986 Pose3 pose3 = pose_above * noise_pose;
987
988 // ==== check Hessian of smartFactor1 =====
989 // -- compute actual Hessian
990 std::shared_ptr<GaussianFactor> linearfactor1 =
991 smartFactor1->linearize(values);
992 Matrix actualHessian = linearfactor1->information();
993
994 // -- compute expected Hessian from manual Schur complement from Jacobians
995 // linearization point for the 3D point
996 smartFactor1->triangulateSafe(cameras: smartFactor1->cameras(values));
997 TriangulationResult point = smartFactor1->point();
998 EXPECT(point.valid()); // check triangulated point is valid
999
1000 // Use the factor to calculate the Jacobians
1001 Matrix F = Matrix::Zero(rows: 2 * 3, cols: 6 * 3);
1002 Matrix E = Matrix::Zero(rows: 2 * 3, cols: 3);
1003 Vector b = Vector::Zero(size: 6);
1004
1005 // create projection factors rolling shutter
1006 ProjectionFactorRollingShutter factor11(measurements_lmk1[0], interp_factor1,
1007 model, x1, x2, l0, sharedK);
1008 Matrix H1Actual, H2Actual, H3Actual;
1009 // note: b is minus the reprojection error, cf the smart factor jacobian
1010 // computation
1011 b.segment<2>(start: 0) = -factor11.evaluateError(x: pose1, x: pose2, x: *point, H&: H1Actual,
1012 H&: H2Actual, H&: H3Actual);
1013 F.block<2, 6>(startRow: 0, startCol: 0) = H1Actual;
1014 F.block<2, 6>(startRow: 0, startCol: 6) = H2Actual;
1015 E.block<2, 3>(startRow: 0, startCol: 0) = H3Actual;
1016
1017 ProjectionFactorRollingShutter factor12(measurements_lmk1[1], interp_factor2,
1018 model, x2, x3, l0, sharedK);
1019 b.segment<2>(start: 2) = -factor12.evaluateError(x: pose2, x: pose3, x: *point, H&: H1Actual,
1020 H&: H2Actual, H&: H3Actual);
1021 F.block<2, 6>(startRow: 2, startCol: 6) = H1Actual;
1022 F.block<2, 6>(startRow: 2, startCol: 12) = H2Actual;
1023 E.block<2, 3>(startRow: 2, startCol: 0) = H3Actual;
1024
1025 ProjectionFactorRollingShutter factor13(measurements_lmk1[2], interp_factor3,
1026 model, x3, x1, l0, sharedK);
1027 b.segment<2>(start: 4) = -factor13.evaluateError(x: pose3, x: pose1, x: *point, H&: H1Actual,
1028 H&: H2Actual, H&: H3Actual);
1029 F.block<2, 6>(startRow: 4, startCol: 12) = H1Actual;
1030 F.block<2, 6>(startRow: 4, startCol: 0) = H2Actual;
1031 E.block<2, 3>(startRow: 4, startCol: 0) = H3Actual;
1032
1033 // whiten
1034 F = (1 / sigma) * F;
1035 E = (1 / sigma) * E;
1036 b = (1 / sigma) * b;
1037 //* G = F' * F - F' * E * P * E' * F
1038 Matrix P = (E.transpose() * E).inverse();
1039 Matrix expectedHessian =
1040 F.transpose() * F - (F.transpose() * E * P * E.transpose() * F);
1041 EXPECT(assert_equal(expectedHessian, actualHessian, 1e-6));
1042
1043 // ==== check Information vector of smartFactor1 =====
1044 GaussianFactorGraph gfg;
1045 gfg.add(factor: linearfactor1);
1046 Matrix actualHessian_v2 = gfg.hessian().first;
1047 EXPECT(assert_equal(actualHessian_v2, actualHessian,
1048 1e-6)); // sanity check on hessian
1049
1050 // -- compute actual information vector
1051 Vector actualInfoVector = gfg.hessian().second;
1052
1053 // -- compute expected information vector from manual Schur complement from
1054 // Jacobians
1055 //* g = F' * (b - E * P * E' * b)
1056 Vector expectedInfoVector = F.transpose() * (b - E * P * E.transpose() * b);
1057 EXPECT(assert_equal(expectedInfoVector, actualInfoVector, 1e-6));
1058
1059 // ==== check error of smartFactor1 (again) =====
1060 NonlinearFactorGraph nfg_projFactorsRS;
1061 nfg_projFactorsRS.add(factorOrContainer: factor11);
1062 nfg_projFactorsRS.add(factorOrContainer: factor12);
1063 nfg_projFactorsRS.add(factorOrContainer: factor13);
1064 values.insert(j: l0, val: *point);
1065
1066 double actualError = smartFactor1->error(values);
1067 double expectedError = nfg_projFactorsRS.error(values);
1068 EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7);
1069}
1070
1071/* *************************************************************************/
1072TEST(SmartProjectionPoseFactorRollingShutter,
1073 hessianComparedToProjFactorsRollingShutter_measurementsFromSamePose) {
1074 // in this test we make sure the fact works even if we have multiple pixel
1075 // measurements of the same landmark at a single pose, a setup that occurs in
1076 // multi-camera systems
1077
1078 using namespace vanillaPoseRS;
1079 Point2Vector measurements_lmk1;
1080
1081 // Project three landmarks into three cameras
1082 projectToMultipleCameras(cam1, cam2, cam3, landmark: landmark1, measurements_cam&: measurements_lmk1);
1083
1084 // create redundant measurements:
1085 Camera::MeasurementVector measurements_lmk1_redundant = measurements_lmk1;
1086 measurements_lmk1_redundant.push_back(
1087 x: measurements_lmk1.at(n: 0)); // we readd the first measurement
1088
1089 // create inputs
1090 std::vector<std::pair<Key, Key>> key_pairs;
1091 key_pairs.push_back(x: std::make_pair(x&: x1, y&: x2));
1092 key_pairs.push_back(x: std::make_pair(x&: x2, y&: x3));
1093 key_pairs.push_back(x: std::make_pair(x&: x3, y&: x1));
1094 key_pairs.push_back(x: std::make_pair(x&: x1, y&: x2));
1095
1096 std::vector<double> interp_factors;
1097 interp_factors.push_back(x: interp_factor1);
1098 interp_factors.push_back(x: interp_factor2);
1099 interp_factors.push_back(x: interp_factor3);
1100 interp_factors.push_back(x: interp_factor1);
1101
1102 std::shared_ptr<Cameras> cameraRig(new Cameras());
1103 cameraRig->push_back(x: Camera(Pose3::Identity(), sharedK));
1104
1105 SmartFactorRS::shared_ptr smartFactor1(
1106 new SmartFactorRS(model, cameraRig, params));
1107 smartFactor1->add(measurements: measurements_lmk1_redundant, world_P_body_key_pairs: key_pairs, alphas: interp_factors);
1108
1109 Pose3 noise_pose = Pose3(Rot3::Ypr(y: -M_PI / 100, p: 0., r: -M_PI / 100),
1110 Point3(0.1, 0.1, 0.1)); // smaller noise
1111 Values values;
1112 values.insert(j: x1, val: level_pose);
1113 values.insert(j: x2, val: pose_right);
1114 // initialize third pose with some noise to get a nontrivial linearization
1115 // point
1116 values.insert(j: x3, val: pose_above * noise_pose);
1117 EXPECT( // check that the pose is actually noisy
1118 assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656,
1119 -0.0313952598, -0.000986635786, 0.0314107591,
1120 -0.999013364, -0.0313952598),
1121 Point3(0.1, -0.1, 1.9)),
1122 values.at<Pose3>(x3)));
1123
1124 // linearization point for the poses
1125 Pose3 pose1 = level_pose;
1126 Pose3 pose2 = pose_right;
1127 Pose3 pose3 = pose_above * noise_pose;
1128
1129 // ==== check Hessian of smartFactor1 =====
1130 // -- compute actual Hessian
1131 std::shared_ptr<GaussianFactor> linearfactor1 =
1132 smartFactor1->linearize(values);
1133 Matrix actualHessian = linearfactor1->information();
1134
1135 // -- compute expected Hessian from manual Schur complement from Jacobians
1136 // linearization point for the 3D point
1137 smartFactor1->triangulateSafe(cameras: smartFactor1->cameras(values));
1138 TriangulationResult point = smartFactor1->point();
1139 EXPECT(point.valid()); // check triangulated point is valid
1140
1141 // Use standard ProjectionFactorRollingShutter factor to calculate the
1142 // Jacobians
1143 Matrix F = Matrix::Zero(rows: 2 * 4, cols: 6 * 3);
1144 Matrix E = Matrix::Zero(rows: 2 * 4, cols: 3);
1145 Vector b = Vector::Zero(size: 8);
1146
1147 // create projection factors rolling shutter
1148 ProjectionFactorRollingShutter factor11(measurements_lmk1_redundant[0],
1149 interp_factor1, model, x1, x2, l0,
1150 sharedK);
1151 Matrix H1Actual, H2Actual, H3Actual;
1152 // note: b is minus the reprojection error, cf the smart factor jacobian
1153 // computation
1154 b.segment<2>(start: 0) = -factor11.evaluateError(x: pose1, x: pose2, x: *point, H&: H1Actual,
1155 H&: H2Actual, H&: H3Actual);
1156 F.block<2, 6>(startRow: 0, startCol: 0) = H1Actual;
1157 F.block<2, 6>(startRow: 0, startCol: 6) = H2Actual;
1158 E.block<2, 3>(startRow: 0, startCol: 0) = H3Actual;
1159
1160 ProjectionFactorRollingShutter factor12(measurements_lmk1_redundant[1],
1161 interp_factor2, model, x2, x3, l0,
1162 sharedK);
1163 b.segment<2>(start: 2) = -factor12.evaluateError(x: pose2, x: pose3, x: *point, H&: H1Actual,
1164 H&: H2Actual, H&: H3Actual);
1165 F.block<2, 6>(startRow: 2, startCol: 6) = H1Actual;
1166 F.block<2, 6>(startRow: 2, startCol: 12) = H2Actual;
1167 E.block<2, 3>(startRow: 2, startCol: 0) = H3Actual;
1168
1169 ProjectionFactorRollingShutter factor13(measurements_lmk1_redundant[2],
1170 interp_factor3, model, x3, x1, l0,
1171 sharedK);
1172 b.segment<2>(start: 4) = -factor13.evaluateError(x: pose3, x: pose1, x: *point, H&: H1Actual,
1173 H&: H2Actual, H&: H3Actual);
1174 F.block<2, 6>(startRow: 4, startCol: 12) = H1Actual;
1175 F.block<2, 6>(startRow: 4, startCol: 0) = H2Actual;
1176 E.block<2, 3>(startRow: 4, startCol: 0) = H3Actual;
1177
1178 ProjectionFactorRollingShutter factor14(measurements_lmk1_redundant[3],
1179 interp_factor1, model, x1, x2, l0,
1180 sharedK);
1181 b.segment<2>(start: 6) = -factor11.evaluateError(x: pose1, x: pose2, x: *point, H&: H1Actual,
1182 H&: H2Actual, H&: H3Actual);
1183 F.block<2, 6>(startRow: 6, startCol: 0) = H1Actual;
1184 F.block<2, 6>(startRow: 6, startCol: 6) = H2Actual;
1185 E.block<2, 3>(startRow: 6, startCol: 0) = H3Actual;
1186
1187 // whiten
1188 F = (1 / sigma) * F;
1189 E = (1 / sigma) * E;
1190 b = (1 / sigma) * b;
1191 //* G = F' * F - F' * E * P * E' * F
1192 Matrix P = (E.transpose() * E).inverse();
1193 Matrix expectedHessian =
1194 F.transpose() * F - (F.transpose() * E * P * E.transpose() * F);
1195 EXPECT(assert_equal(expectedHessian, actualHessian, 1e-6));
1196
1197 // ==== check Information vector of smartFactor1 =====
1198 GaussianFactorGraph gfg;
1199 gfg.add(factor: linearfactor1);
1200 Matrix actualHessian_v2 = gfg.hessian().first;
1201 EXPECT(assert_equal(actualHessian_v2, actualHessian,
1202 1e-6)); // sanity check on hessian
1203
1204 // -- compute actual information vector
1205 Vector actualInfoVector = gfg.hessian().second;
1206
1207 // -- compute expected information vector from manual Schur complement from
1208 // Jacobians
1209 //* g = F' * (b - E * P * E' * b)
1210 Vector expectedInfoVector = F.transpose() * (b - E * P * E.transpose() * b);
1211 EXPECT(assert_equal(expectedInfoVector, actualInfoVector, 1e-6));
1212
1213 // ==== check error of smartFactor1 (again) =====
1214 NonlinearFactorGraph nfg_projFactorsRS;
1215 nfg_projFactorsRS.add(factorOrContainer: factor11);
1216 nfg_projFactorsRS.add(factorOrContainer: factor12);
1217 nfg_projFactorsRS.add(factorOrContainer: factor13);
1218 nfg_projFactorsRS.add(factorOrContainer: factor14);
1219 values.insert(j: l0, val: *point);
1220
1221 double actualError = smartFactor1->error(values);
1222 double expectedError = nfg_projFactorsRS.error(values);
1223 EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7);
1224}
1225
1226/* *************************************************************************/
1227TEST(SmartProjectionPoseFactorRollingShutter,
1228 optimization_3poses_measurementsFromSamePose) {
1229 using namespace vanillaPoseRS;
1230 Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3;
1231
1232 // Project three landmarks into three cameras
1233 projectToMultipleCameras(cam1, cam2, cam3, landmark: landmark1, measurements_cam&: measurements_lmk1);
1234 projectToMultipleCameras(cam1, cam2, cam3, landmark: landmark2, measurements_cam&: measurements_lmk2);
1235 projectToMultipleCameras(cam1, cam2, cam3, landmark: landmark3, measurements_cam&: measurements_lmk3);
1236
1237 // create inputs
1238 std::vector<std::pair<Key, Key>> key_pairs;
1239 key_pairs.push_back(x: std::make_pair(x&: x1, y&: x2));
1240 key_pairs.push_back(x: std::make_pair(x&: x2, y&: x3));
1241 key_pairs.push_back(x: std::make_pair(x&: x3, y&: x1));
1242
1243 std::vector<double> interp_factors;
1244 interp_factors.push_back(x: interp_factor1);
1245 interp_factors.push_back(x: interp_factor2);
1246 interp_factors.push_back(x: interp_factor3);
1247
1248 // For first factor, we create redundant measurement (taken by the same keys
1249 // as factor 1, to make sure the redundancy in the keys does not create
1250 // problems)
1251 Camera::MeasurementVector& measurements_lmk1_redundant = measurements_lmk1;
1252 measurements_lmk1_redundant.push_back(
1253 x: measurements_lmk1.at(n: 0)); // we readd the first measurement
1254 std::vector<std::pair<Key, Key>> key_pairs_redundant = key_pairs;
1255 key_pairs_redundant.push_back(
1256 x: key_pairs.at(n: 0)); // we readd the first pair of keys
1257 std::vector<double> interp_factors_redundant = interp_factors;
1258 interp_factors_redundant.push_back(
1259 x: interp_factors.at(n: 0)); // we readd the first interp factor
1260
1261 std::shared_ptr<Cameras> cameraRig(new Cameras());
1262 cameraRig->push_back(x: Camera(Pose3::Identity(), sharedK));
1263
1264 SmartFactorRS::shared_ptr smartFactor1(
1265 new SmartFactorRS(model, cameraRig, params));
1266 smartFactor1->add(measurements: measurements_lmk1_redundant, world_P_body_key_pairs: key_pairs_redundant,
1267 alphas: interp_factors_redundant);
1268
1269 SmartFactorRS::shared_ptr smartFactor2(
1270 new SmartFactorRS(model, cameraRig, params));
1271 smartFactor2->add(measurements: measurements_lmk2, world_P_body_key_pairs: key_pairs, alphas: interp_factors);
1272
1273 SmartFactorRS::shared_ptr smartFactor3(
1274 new SmartFactorRS(model, cameraRig, params));
1275 smartFactor3->add(measurements: measurements_lmk3, world_P_body_key_pairs: key_pairs, alphas: interp_factors);
1276
1277 const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(dim: 6, sigma: 0.10);
1278
1279 NonlinearFactorGraph graph;
1280 graph.push_back(factor: smartFactor1);
1281 graph.push_back(factor: smartFactor2);
1282 graph.push_back(factor: smartFactor3);
1283 graph.addPrior(key: x1, prior: level_pose, model: noisePrior);
1284 graph.addPrior(key: x2, prior: pose_right, model: noisePrior);
1285
1286 Values groundTruth;
1287 groundTruth.insert(j: x1, val: level_pose);
1288 groundTruth.insert(j: x2, val: pose_right);
1289 groundTruth.insert(j: x3, val: pose_above);
1290 DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9);
1291
1292 // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10),
1293 // Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
1294 Pose3 noise_pose = Pose3(Rot3::Ypr(y: -M_PI / 100, p: 0., r: -M_PI / 100),
1295 Point3(0.1, 0.1, 0.1)); // smaller noise
1296 Values values;
1297 values.insert(j: x1, val: level_pose);
1298 values.insert(j: x2, val: pose_right);
1299 // initialize third pose with some noise, we expect it to move back to
1300 // original pose_above
1301 values.insert(j: x3, val: pose_above * noise_pose);
1302 EXPECT( // check that the pose is actually noisy
1303 assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656,
1304 -0.0313952598, -0.000986635786, 0.0314107591,
1305 -0.999013364, -0.0313952598),
1306 Point3(0.1, -0.1, 1.9)),
1307 values.at<Pose3>(x3)));
1308
1309 Values result;
1310 LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
1311 result = optimizer.optimize();
1312 EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-5));
1313}
1314
1315#ifndef DISABLE_TIMING
1316#include <gtsam/base/timing.h>
1317//-Total: 0 CPU (0 times, 0 wall, 0.21 children, min: 0 max: 0)
1318//| -SF RS LINEARIZE: 0.14 CPU
1319//(10000 times, 0.131202 wall, 0.14 children, min: 0 max: 0)
1320//| -RS LINEARIZE: 0.06 CPU
1321//(10000 times, 0.066951 wall, 0.06 children, min: 0 max: 0)
1322/* *************************************************************************/
1323TEST(SmartProjectionPoseFactorRollingShutter, timing) {
1324 using namespace vanillaPose;
1325
1326 // Default cameras for simple derivatives
1327 static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0));
1328 SmartProjectionParams params(
1329 gtsam::HESSIAN,
1330 gtsam::ZERO_ON_DEGENERACY); // only config that works with RS factors
1331
1332 Rot3 R = Rot3::Identity();
1333 Pose3 pose1 = Pose3(R, Point3(0, 0, 0));
1334 Pose3 pose2 = Pose3(R, Point3(1, 0, 0));
1335 Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple);
1336 Pose3 body_P_sensorId = Pose3::Identity();
1337
1338 // one landmarks 1m in front of camera
1339 Point3 landmark1(0, 0, 10);
1340
1341 Point2Vector measurements_lmk1;
1342
1343 // Project 2 landmarks into 2 cameras
1344 measurements_lmk1.push_back(cam1.project(landmark1));
1345 measurements_lmk1.push_back(cam2.project(landmark1));
1346
1347 size_t nrTests = 10000;
1348
1349 for (size_t i = 0; i < nrTests; i++) {
1350 std::shared_ptr<Cameras> cameraRig(new Cameras());
1351 cameraRig->push_back(Camera(body_P_sensorId, sharedKSimple));
1352
1353 SmartFactorRS::shared_ptr smartFactorRS(new SmartFactorRS(
1354 model, cameraRig, params));
1355 double interp_factor = 0; // equivalent to measurement taken at pose 1
1356 smartFactorRS->add(measurements_lmk1[0], x1, x2, interp_factor);
1357 interp_factor = 1; // equivalent to measurement taken at pose 2
1358 smartFactorRS->add(measurements_lmk1[1], x1, x2, interp_factor);
1359
1360 Values values;
1361 values.insert(x1, pose1);
1362 values.insert(x2, pose2);
1363 gttic_(SF_RS_LINEARIZE);
1364 smartFactorRS->linearize(values);
1365 gttoc_(SF_RS_LINEARIZE);
1366 }
1367
1368 for (size_t i = 0; i < nrTests; i++) {
1369 SmartFactor::shared_ptr smartFactor(
1370 new SmartFactor(model, sharedKSimple, params));
1371 smartFactor->add(measurements_lmk1[0], x1);
1372 smartFactor->add(measurements_lmk1[1], x2);
1373
1374 Values values;
1375 values.insert(x1, pose1);
1376 values.insert(x2, pose2);
1377 gttic_(RS_LINEARIZE);
1378 smartFactor->linearize(values);
1379 gttoc_(RS_LINEARIZE);
1380 }
1381 tictoc_print_();
1382}
1383#endif
1384
1385#include <gtsam/geometry/SphericalCamera.h>
1386/* ************************************************************************* */
1387// spherical Camera with rolling shutter effect
1388namespace sphericalCameraRS {
1389typedef SphericalCamera Camera;
1390typedef CameraSet<Camera> Cameras;
1391typedef SmartProjectionPoseFactorRollingShutter<Camera> SmartFactorRS_spherical;
1392Pose3 interp_pose1 = interpolate<Pose3>(X: level_pose, Y: pose_right, t: interp_factor1);
1393Pose3 interp_pose2 = interpolate<Pose3>(X: pose_right, Y: pose_above, t: interp_factor2);
1394Pose3 interp_pose3 = interpolate<Pose3>(X: pose_above, Y: level_pose, t: interp_factor3);
1395static EmptyCal::shared_ptr emptyK(new EmptyCal());
1396Camera cam1(interp_pose1, emptyK);
1397Camera cam2(interp_pose2, emptyK);
1398Camera cam3(interp_pose3, emptyK);
1399} // namespace sphericalCameraRS
1400
1401/* *************************************************************************/
1402TEST(SmartProjectionPoseFactorRollingShutter,
1403 optimization_3poses_sphericalCameras) {
1404 using namespace sphericalCameraRS;
1405 std::vector<Unit3> measurements_lmk1, measurements_lmk2, measurements_lmk3;
1406
1407 // Project three landmarks into three cameras
1408 projectToMultipleCameras<Camera>(cam1, cam2, cam3, landmark: landmark1,
1409 measurements_cam&: measurements_lmk1);
1410 projectToMultipleCameras<Camera>(cam1, cam2, cam3, landmark: landmark2,
1411 measurements_cam&: measurements_lmk2);
1412 projectToMultipleCameras<Camera>(cam1, cam2, cam3, landmark: landmark3,
1413 measurements_cam&: measurements_lmk3);
1414
1415 // create inputs
1416 std::vector<std::pair<Key, Key>> key_pairs;
1417 key_pairs.push_back(x: std::make_pair(x&: x1, y&: x2));
1418 key_pairs.push_back(x: std::make_pair(x&: x2, y&: x3));
1419 key_pairs.push_back(x: std::make_pair(x&: x3, y&: x1));
1420
1421 std::vector<double> interp_factors;
1422 interp_factors.push_back(x: interp_factor1);
1423 interp_factors.push_back(x: interp_factor2);
1424 interp_factors.push_back(x: interp_factor3);
1425
1426 SmartProjectionParams params(
1427 gtsam::HESSIAN,
1428 gtsam::ZERO_ON_DEGENERACY); // only config that works with RS factors
1429 params.setRankTolerance(0.1);
1430
1431 std::shared_ptr<Cameras> cameraRig(new Cameras());
1432 cameraRig->push_back(x: Camera(Pose3::Identity(), emptyK));
1433
1434 SmartFactorRS_spherical::shared_ptr smartFactor1(
1435 new SmartFactorRS_spherical(model, cameraRig, params));
1436 smartFactor1->add(measurements: measurements_lmk1, world_P_body_key_pairs: key_pairs, alphas: interp_factors);
1437
1438 SmartFactorRS_spherical::shared_ptr smartFactor2(
1439 new SmartFactorRS_spherical(model, cameraRig, params));
1440 smartFactor2->add(measurements: measurements_lmk2, world_P_body_key_pairs: key_pairs, alphas: interp_factors);
1441
1442 SmartFactorRS_spherical::shared_ptr smartFactor3(
1443 new SmartFactorRS_spherical(model, cameraRig, params));
1444 smartFactor3->add(measurements: measurements_lmk3, world_P_body_key_pairs: key_pairs, alphas: interp_factors);
1445
1446 const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(dim: 6, sigma: 0.10);
1447
1448 NonlinearFactorGraph graph;
1449 graph.push_back(factor: smartFactor1);
1450 graph.push_back(factor: smartFactor2);
1451 graph.push_back(factor: smartFactor3);
1452 graph.addPrior(key: x1, prior: level_pose, model: noisePrior);
1453 graph.addPrior(key: x2, prior: pose_right, model: noisePrior);
1454
1455 Values groundTruth;
1456 groundTruth.insert(j: x1, val: level_pose);
1457 groundTruth.insert(j: x2, val: pose_right);
1458 groundTruth.insert(j: x3, val: pose_above);
1459 DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9);
1460
1461 // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10),
1462 // Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
1463 Pose3 noise_pose = Pose3(Rot3::Ypr(y: -M_PI / 100, p: 0., r: -M_PI / 100),
1464 Point3(0.1, 0.1, 0.1)); // smaller noise
1465 Values values;
1466 values.insert(j: x1, val: level_pose);
1467 values.insert(j: x2, val: pose_right);
1468 // initialize third pose with some noise, we expect it to move back to
1469 // original pose_above
1470 values.insert(j: x3, val: pose_above * noise_pose);
1471 EXPECT( // check that the pose is actually noisy
1472 assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656,
1473 -0.0313952598, -0.000986635786, 0.0314107591,
1474 -0.999013364, -0.0313952598),
1475 Point3(0.1, -0.1, 1.9)),
1476 values.at<Pose3>(x3)));
1477
1478 Values result;
1479 LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
1480 result = optimizer.optimize();
1481 EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-6));
1482}
1483
1484/* ************************************************************************* */
1485int main() {
1486 TestResult tr;
1487 return TestRegistry::runAllTests(result&: tr);
1488}
1489/* ************************************************************************* */
1490