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 TestSmartStereoProjectionPoseFactor.cpp
14 * @brief Unit tests for ProjectionFactor Class
15 * @author Chris Beall
16 * @author Luca Carlone
17 * @author Zsolt Kira
18 * @date Sept 2013
19 */
20
21#include <gtsam/slam/tests/smartFactorScenarios.h>
22#include <gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h>
23#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
24#include <gtsam/slam/PoseTranslationPrior.h>
25#include <gtsam/slam/ProjectionFactor.h>
26#include <gtsam/slam/StereoFactor.h>
27#include <CppUnitLite/TestHarness.h>
28#include <iostream>
29
30using namespace std;
31using namespace gtsam;
32
33namespace {
34// make a realistic calibration matrix
35static double b = 1;
36
37static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fov, w, h, b));
38static Cal3_S2Stereo::shared_ptr K2(new Cal3_S2Stereo(1500, 1200, 0, 640, 480,
39 b));
40
41static SmartStereoProjectionParams params;
42
43// static bool manageDegeneracy = true;
44// Create a noise model for the pixel error
45static SharedNoiseModel model(noiseModel::Isotropic::Sigma(dim: 3, sigma: 0.1));
46
47// Convenience for named keys
48using symbol_shorthand::L;
49using symbol_shorthand::X;
50
51// tests data
52static Symbol x1('X', 1);
53static Symbol x2('X', 2);
54static Symbol x3('X', 3);
55
56static Key poseKey1(x1);
57static StereoPoint2 measurement1(
58 323.0, 300.0, 240.0); // potentially use more reasonable measurement value?
59static Pose3 body_P_sensor1(Rot3::RzRyRx(x: -M_PI_2, y: 0.0, z: -M_PI_2),
60 Point3(0.25, -0.10, 1.0));
61
62static double missing_uR = std::numeric_limits<double>::quiet_NaN();
63
64vector<StereoPoint2> stereo_projectToMultipleCameras(const StereoCamera& cam1,
65 const StereoCamera& cam2,
66 const StereoCamera& cam3,
67 Point3 landmark) {
68 vector<StereoPoint2> measurements_cam;
69
70 StereoPoint2 cam1_uv1 = cam1.project(point: landmark);
71 StereoPoint2 cam2_uv1 = cam2.project(point: landmark);
72 StereoPoint2 cam3_uv1 = cam3.project(point: landmark);
73 measurements_cam.push_back(x: cam1_uv1);
74 measurements_cam.push_back(x: cam2_uv1);
75 measurements_cam.push_back(x: cam3_uv1);
76
77 return measurements_cam;
78}
79
80LevenbergMarquardtParams lm_params;
81} // namespace
82
83/* ************************************************************************* */
84TEST( SmartStereoProjectionPoseFactor, params) {
85 SmartStereoProjectionParams p;
86
87 // check default values and "get"
88 EXPECT(p.getLinearizationMode() == HESSIAN);
89 EXPECT(p.getDegeneracyMode() == IGNORE_DEGENERACY);
90 EXPECT_DOUBLES_EQUAL(p.getRetriangulationThreshold(), 1e-5, 1e-9);
91 EXPECT(p.getVerboseCheirality() == false);
92 EXPECT(p.getThrowCheirality() == false);
93
94 // check "set"
95 p.setLinearizationMode(JACOBIAN_SVD);
96 p.setDegeneracyMode(ZERO_ON_DEGENERACY);
97 p.setRankTolerance(100);
98 p.setEnableEPI(true);
99 p.setLandmarkDistanceThreshold(200);
100 p.setDynamicOutlierRejectionThreshold(3);
101 p.setRetriangulationThreshold(1e-2);
102
103 EXPECT(p.getLinearizationMode() == JACOBIAN_SVD);
104 EXPECT(p.getDegeneracyMode() == ZERO_ON_DEGENERACY);
105 EXPECT_DOUBLES_EQUAL(p.getTriangulationParameters().rankTolerance, 100, 1e-5);
106 EXPECT(p.getTriangulationParameters().enableEPI == true);
107 EXPECT_DOUBLES_EQUAL(p.getTriangulationParameters().landmarkDistanceThreshold, 200, 1e-5);
108 EXPECT_DOUBLES_EQUAL(p.getTriangulationParameters().dynamicOutlierRejectionThreshold, 3, 1e-5);
109 EXPECT_DOUBLES_EQUAL(p.getRetriangulationThreshold(), 1e-2, 1e-5);
110}
111
112/* ************************************************************************* */
113TEST( SmartStereoProjectionPoseFactor, Constructor) {
114 SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor(model));
115}
116
117/* ************************************************************************* */
118TEST( SmartStereoProjectionPoseFactor, Constructor2) {
119 SmartStereoProjectionPoseFactor factor1(model, params);
120}
121
122/* ************************************************************************* */
123TEST( SmartStereoProjectionPoseFactor, Constructor3) {
124 SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor(model));
125 factor1->add(measured: measurement1, poseKey: poseKey1, K);
126}
127
128/* ************************************************************************* */
129TEST( SmartStereoProjectionPoseFactor, Constructor4) {
130 SmartStereoProjectionPoseFactor factor1(model, params);
131 factor1.add(measured: measurement1, poseKey: poseKey1, K);
132}
133
134/* ************************************************************************* */
135TEST( SmartStereoProjectionPoseFactor, Equals ) {
136 SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor(model));
137 factor1->add(measured: measurement1, poseKey: poseKey1, K);
138
139 SmartStereoProjectionPoseFactor::shared_ptr factor2(new SmartStereoProjectionPoseFactor(model));
140 factor2->add(measured: measurement1, poseKey: poseKey1, K);
141
142 CHECK(assert_equal(*factor1, *factor2));
143}
144
145/* *************************************************************************/
146TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ) {
147 // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
148 Pose3 level_pose = Pose3(Rot3::Ypr(y: -M_PI / 2, p: 0., r: -M_PI / 2),
149 Point3(0, 0, 1));
150 StereoCamera level_camera(level_pose, K2);
151
152 // create second camera 1 meter to the right of first camera
153 Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1, 0, 0));
154 StereoCamera level_camera_right(level_pose_right, K2);
155
156 // landmark ~5 meters infront of camera
157 Point3 landmark(5, 0.5, 1.2);
158
159 // 1. Project two landmarks into two cameras and triangulate
160 StereoPoint2 level_uv = level_camera.project(point: landmark);
161 StereoPoint2 level_uv_right = level_camera_right.project(point: landmark);
162
163 Values values;
164 values.insert(j: x1, val: level_pose);
165 values.insert(j: x2, val: level_pose_right);
166
167 SmartStereoProjectionPoseFactor factor1(model);
168 factor1.add(measured: level_uv, poseKey: x1, K: K2);
169 factor1.add(measured: level_uv_right, poseKey: x2, K: K2);
170
171 double actualError = factor1.error(values);
172 double expectedError = 0.0;
173 EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7);
174
175 SmartStereoProjectionPoseFactor::Cameras cameras = factor1.cameras(values);
176 double actualError2 = factor1.totalReprojectionError(cameras);
177 EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7);
178
179 // test vector of errors
180 //Vector actual = factor1.unwhitenedError(values);
181 //EXPECT(assert_equal(zero(4),actual,1e-8));
182}
183
184/* *************************************************************************/
185TEST( SmartProjectionPoseFactor, noiselessWithMissingMeasurements ) {
186
187 // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
188 Pose3 level_pose = Pose3(Rot3::Ypr(y: -M_PI / 2, p: 0., r: -M_PI / 2),
189 Point3(0, 0, 1));
190 StereoCamera level_camera(level_pose, K2);
191
192 // create second camera 1 meter to the right of first camera
193 Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1, 0, 0));
194 StereoCamera level_camera_right(level_pose_right, K2);
195
196 // landmark ~5 meters in front of camera
197 Point3 landmark(5, 0.5, 1.2);
198
199 // 1. Project two landmarks into two cameras and triangulate
200 StereoPoint2 level_uv = level_camera.project(point: landmark);
201 StereoPoint2 level_uv_right = level_camera_right.project(point: landmark);
202 StereoPoint2 level_uv_right_missing(level_uv_right.uL(),missing_uR,level_uv_right.v());
203
204 Values values;
205 values.insert(j: x1, val: level_pose);
206 values.insert(j: x2, val: level_pose_right);
207
208 SmartStereoProjectionPoseFactor factor1(model);
209 factor1.add(measured: level_uv, poseKey: x1, K: K2);
210 factor1.add(measured: level_uv_right_missing, poseKey: x2, K: K2);
211
212 double actualError = factor1.error(values);
213 double expectedError = 0.0;
214 EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7);
215
216 // TEST TRIANGULATION WITH MISSING VALUES: i) right pixel of second camera is missing:
217 SmartStereoProjectionPoseFactor::Cameras cameras = factor1.cameras(values);
218 double actualError2 = factor1.totalReprojectionError(cameras);
219 EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7);
220
221 CameraSet<StereoCamera> cams{level_camera, level_camera_right};
222 TriangulationResult result = factor1.triangulateSafe(cameras: cams);
223 CHECK(result);
224 EXPECT(assert_equal(landmark, *result, 1e-7));
225
226 // TEST TRIANGULATION WITH MISSING VALUES: ii) right pixels of both cameras are missing:
227 SmartStereoProjectionPoseFactor factor2(model);
228 StereoPoint2 level_uv_missing(level_uv.uL(),missing_uR,level_uv.v());
229 factor2.add(measured: level_uv_missing, poseKey: x1, K: K2);
230 factor2.add(measured: level_uv_right_missing, poseKey: x2, K: K2);
231 result = factor2.triangulateSafe(cameras: cams);
232 CHECK(result);
233 EXPECT(assert_equal(landmark, *result, 1e-7));
234}
235
236/* *************************************************************************/
237TEST( SmartStereoProjectionPoseFactor, noisy ) {
238 // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
239 Pose3 level_pose = Pose3(Rot3::Ypr(y: -M_PI / 2, p: 0., r: -M_PI / 2),
240 Point3(0, 0, 1));
241 StereoCamera level_camera(level_pose, K2);
242
243 // create second camera 1 meter to the right of first camera
244 Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1, 0, 0));
245 StereoCamera level_camera_right(level_pose_right, K2);
246
247 // landmark ~5 meters infront of camera
248 Point3 landmark(5, 0.5, 1.2);
249
250 // 1. Project two landmarks into two cameras and triangulate
251 StereoPoint2 pixelError(0.2, 0.2, 0);
252 StereoPoint2 level_uv = level_camera.project(point: landmark) + pixelError;
253 StereoPoint2 level_uv_right = level_camera_right.project(point: landmark);
254
255 Values values;
256 values.insert(j: x1, val: level_pose);
257 Pose3 noise_pose = Pose3(Rot3::Ypr(y: -M_PI / 10, p: 0., r: -M_PI / 10),
258 Point3(0.5, 0.1, 0.3));
259 values.insert(j: x2, val: level_pose_right.compose(g: noise_pose));
260
261 SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor(model));
262 factor1->add(measured: level_uv, poseKey: x1, K);
263 factor1->add(measured: level_uv_right, poseKey: x2, K);
264
265 double actualError1 = factor1->error(values);
266
267 SmartStereoProjectionPoseFactor::shared_ptr factor2(new SmartStereoProjectionPoseFactor(model));
268 vector<StereoPoint2> measurements;
269 measurements.push_back(x: level_uv);
270 measurements.push_back(x: level_uv_right);
271
272 vector<std::shared_ptr<Cal3_S2Stereo> > Ks; ///< shared pointer to calibration object (one for each camera)
273 Ks.push_back(x: K);
274 Ks.push_back(x: K);
275
276 KeyVector views;
277 views.push_back(x: x1);
278 views.push_back(x: x2);
279
280 factor2->add(measurements, poseKeys: views, Ks);
281
282 double actualError2 = factor2->error(values);
283
284 DOUBLES_EQUAL(actualError1, actualError2, 1e-7);
285}
286
287/* *************************************************************************/
288TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) {
289
290 // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
291 Pose3 pose1 = Pose3(Rot3::Ypr(y: -M_PI / 2, p: 0., r: -M_PI / 2), Point3(0, 0, 1));
292 StereoCamera cam1(pose1, K2);
293
294 // create second camera 1 meter to the right of first camera
295 Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0));
296 StereoCamera cam2(pose2, K2);
297
298 // create third camera 1 meter above the first camera
299 Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0));
300 StereoCamera cam3(pose3, K2);
301
302 // three landmarks ~5 meters infront of camera
303 Point3 landmark1(5, 0.5, 1.2);
304 Point3 landmark2(5, -0.5, 1.2);
305 Point3 landmark3(3, 0, 3.0);
306
307 // 1. Project three landmarks into three cameras and triangulate
308 vector<StereoPoint2> measurements_l1 = stereo_projectToMultipleCameras(cam1,
309 cam2, cam3, landmark: landmark1);
310 vector<StereoPoint2> measurements_l2 = stereo_projectToMultipleCameras(cam1,
311 cam2, cam3, landmark: landmark2);
312 vector<StereoPoint2> measurements_l3 = stereo_projectToMultipleCameras(cam1,
313 cam2, cam3, landmark: landmark3);
314
315 KeyVector views;
316 views.push_back(x: x1);
317 views.push_back(x: x2);
318 views.push_back(x: x3);
319
320 SmartStereoProjectionParams smart_params;
321 smart_params.triangulation.enableEPI = true;
322 SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(model, smart_params));
323 smartFactor1->add(measurements: measurements_l1, poseKeys: views, K: K2);
324
325 SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(model, smart_params));
326 smartFactor2->add(measurements: measurements_l2, poseKeys: views, K: K2);
327
328 SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(model, smart_params));
329 smartFactor3->add(measurements: measurements_l3, poseKeys: views, K: K2);
330
331 const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(dim: 6, sigma: 0.10);
332
333 NonlinearFactorGraph graph;
334 graph.push_back(factor: smartFactor1);
335 graph.push_back(factor: smartFactor2);
336 graph.push_back(factor: smartFactor3);
337 graph.addPrior(key: x1, prior: pose1, model: noisePrior);
338 graph.addPrior(key: x2, prior: pose2, model: noisePrior);
339
340 // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
341 Pose3 noise_pose = Pose3(Rot3::Ypr(y: -M_PI / 100, p: 0., r: -M_PI / 100),
342 Point3(0.1, 0.1, 0.1)); // smaller noise
343 Values values;
344 values.insert(j: x1, val: pose1);
345 values.insert(j: x2, val: pose2);
346 // initialize third pose with some noise, we expect it to move back to original pose3
347 values.insert(j: x3, val: pose3 * noise_pose);
348 EXPECT(
349 assert_equal(
350 Pose3(
351 Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598,
352 -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598),
353 Point3(0.1, -0.1, 1.9)), values.at<Pose3>(x3)));
354
355 // cout << std::setprecision(10) << "\n----SmartStereoFactor graph initial error: " << graph.error(values) << endl;
356 EXPECT_DOUBLES_EQUAL(833953.92789459578, graph.error(values), 1e-7); // initial error
357
358 // get triangulated landmarks from smart factors
359 Point3 landmark1_smart = *smartFactor1->point();
360 Point3 landmark2_smart = *smartFactor2->point();
361 Point3 landmark3_smart = *smartFactor3->point();
362
363 Values result;
364 gttic_(SmartStereoProjectionPoseFactor);
365 LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
366 result = optimizer.optimize();
367 gttoc_(SmartStereoProjectionPoseFactor);
368 tictoc_finishedIteration_();
369
370 EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-5);
371
372// cout << std::setprecision(10) << "SmartStereoFactor graph optimized error: " << graph.error(result) << endl;
373
374 GaussianFactorGraph::shared_ptr GFG = graph.linearize(linearizationPoint: result);
375 VectorValues delta = GFG->optimize();
376 VectorValues expected = VectorValues::Zero(other: delta);
377 EXPECT(assert_equal(expected, delta, 1e-6));
378
379 // result.print("results of 3 camera, 3 landmark optimization \n");
380 EXPECT(assert_equal(pose3, result.at<Pose3>(x3)));
381
382 /* ***************************************************************
383 * Same problem with regular Stereo factors, expect same error!
384 * ****************************************************************/
385
386// landmark1_smart.print("landmark1_smart");
387// landmark2_smart.print("landmark2_smart");
388// landmark3_smart.print("landmark3_smart");
389
390 // add landmarks to values
391 values.insert(j: L(j: 1), val: landmark1_smart);
392 values.insert(j: L(j: 2), val: landmark2_smart);
393 values.insert(j: L(j: 3), val: landmark3_smart);
394
395 // add factors
396 NonlinearFactorGraph graph2;
397
398 graph2.addPrior(key: x1, prior: pose1, model: noisePrior);
399 graph2.addPrior(key: x2, prior: pose2, model: noisePrior);
400
401 typedef GenericStereoFactor<Pose3, Point3> ProjectionFactor;
402
403 bool verboseCheirality = true;
404
405 graph2.push_back(factor: ProjectionFactor(measurements_l1[0], model, x1, L(j: 1), K2, false, verboseCheirality));
406 graph2.push_back(factor: ProjectionFactor(measurements_l1[1], model, x2, L(j: 1), K2, false, verboseCheirality));
407 graph2.push_back(factor: ProjectionFactor(measurements_l1[2], model, x3, L(j: 1), K2, false, verboseCheirality));
408
409 graph2.push_back(factor: ProjectionFactor(measurements_l2[0], model, x1, L(j: 2), K2, false, verboseCheirality));
410 graph2.push_back(factor: ProjectionFactor(measurements_l2[1], model, x2, L(j: 2), K2, false, verboseCheirality));
411 graph2.push_back(factor: ProjectionFactor(measurements_l2[2], model, x3, L(j: 2), K2, false, verboseCheirality));
412
413 graph2.push_back(factor: ProjectionFactor(measurements_l3[0], model, x1, L(j: 3), K2, false, verboseCheirality));
414 graph2.push_back(factor: ProjectionFactor(measurements_l3[1], model, x2, L(j: 3), K2, false, verboseCheirality));
415 graph2.push_back(factor: ProjectionFactor(measurements_l3[2], model, x3, L(j: 3), K2, false, verboseCheirality));
416
417// cout << std::setprecision(10) << "\n----StereoFactor graph initial error: " << graph2.error(values) << endl;
418 EXPECT_DOUBLES_EQUAL(833953.92789459578, graph2.error(values), 1e-7);
419
420 LevenbergMarquardtOptimizer optimizer2(graph2, values, lm_params);
421 Values result2 = optimizer2.optimize();
422 EXPECT_DOUBLES_EQUAL(0, graph2.error(result2), 1e-5);
423
424// cout << std::setprecision(10) << "StereoFactor graph optimized error: " << graph2.error(result2) << endl;
425
426}
427/* *************************************************************************/
428TEST( SmartStereoProjectionPoseFactor, body_P_sensor ) {
429
430 // camera has some displacement
431 Pose3 body_P_sensor = Pose3(Rot3::Ypr(y: -0.01, p: 0., r: -0.05), Point3(0.1, 0, 0.1));
432 // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
433 Pose3 pose1 = Pose3(Rot3::Ypr(y: -M_PI / 2, p: 0., r: -M_PI / 2), Point3(0, 0, 1));
434 StereoCamera cam1(pose1.compose(g: body_P_sensor), K2);
435
436 // create second camera 1 meter to the right of first camera
437 Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0));
438 StereoCamera cam2(pose2.compose(g: body_P_sensor), K2);
439
440 // create third camera 1 meter above the first camera
441 Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0));
442 StereoCamera cam3(pose3.compose(g: body_P_sensor), K2);
443
444 // three landmarks ~5 meters infront of camera
445 Point3 landmark1(5, 0.5, 1.2);
446 Point3 landmark2(5, -0.5, 1.2);
447 Point3 landmark3(3, 0, 3.0);
448
449 // 1. Project three landmarks into three cameras and triangulate
450 vector<StereoPoint2> measurements_l1 = stereo_projectToMultipleCameras(cam1,
451 cam2, cam3, landmark: landmark1);
452 vector<StereoPoint2> measurements_l2 = stereo_projectToMultipleCameras(cam1,
453 cam2, cam3, landmark: landmark2);
454 vector<StereoPoint2> measurements_l3 = stereo_projectToMultipleCameras(cam1,
455 cam2, cam3, landmark: landmark3);
456
457 KeyVector views;
458 views.push_back(x: x1);
459 views.push_back(x: x2);
460 views.push_back(x: x3);
461
462 SmartStereoProjectionParams smart_params;
463 smart_params.triangulation.enableEPI = true;
464 SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(model, smart_params, body_P_sensor));
465 smartFactor1->add(measurements: measurements_l1, poseKeys: views, K: K2);
466
467 SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(model, smart_params, body_P_sensor));
468 smartFactor2->add(measurements: measurements_l2, poseKeys: views, K: K2);
469
470 SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(model, smart_params, body_P_sensor));
471 smartFactor3->add(measurements: measurements_l3, poseKeys: views, K: K2);
472
473 const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(dim: 6, sigma: 0.10);
474
475 NonlinearFactorGraph graph;
476 graph.push_back(factor: smartFactor1);
477 graph.push_back(factor: smartFactor2);
478 graph.push_back(factor: smartFactor3);
479 graph.addPrior(key: x1, prior: pose1, model: noisePrior);
480 graph.addPrior(key: x2, prior: pose2, model: noisePrior);
481
482 // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
483 Pose3 noise_pose = Pose3(Rot3::Ypr(y: -M_PI / 100, p: 0., r: -M_PI / 100),
484 Point3(0.1, 0.1, 0.1)); // smaller noise
485 Values values;
486 values.insert(j: x1, val: pose1);
487 values.insert(j: x2, val: pose2);
488 // initialize third pose with some noise, we expect it to move back to original pose3
489 values.insert(j: x3, val: pose3 * noise_pose);
490 EXPECT(
491 assert_equal(
492 Pose3(
493 Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598,
494 -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598),
495 Point3(0.1, -0.1, 1.9)), values.at<Pose3>(x3)));
496
497 // cout << std::setprecision(10) << "\n----SmartStereoFactor graph initial error: " << graph.error(values) << endl;
498 EXPECT_DOUBLES_EQUAL(953392.32838422502, graph.error(values), 1e-7); // initial error
499
500 Values result;
501 gttic_(SmartStereoProjectionPoseFactor);
502 LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
503 result = optimizer.optimize();
504 gttoc_(SmartStereoProjectionPoseFactor);
505 tictoc_finishedIteration_();
506
507 EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-5);
508
509 // result.print("results of 3 camera, 3 landmark optimization \n");
510 EXPECT(assert_equal(pose3, result.at<Pose3>(x3)));
511}
512/* *************************************************************************/
513TEST( SmartStereoProjectionPoseFactor, body_P_sensor_monocular ){
514 // make a realistic calibration matrix
515 double fov = 60; // degrees
516 size_t w=640,h=480;
517
518 Cal3_S2::shared_ptr K(new Cal3_S2(fov,w,h));
519
520 // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
521 Pose3 cameraPose1 = Pose3(Rot3::Ypr(y: -M_PI/2, p: 0., r: -M_PI/2), gtsam::Point3(0,0,1)); // body poses
522 Pose3 cameraPose2 = cameraPose1 * Pose3(Rot3(), Point3(1,0,0));
523 Pose3 cameraPose3 = cameraPose1 * Pose3(Rot3(), Point3(0,-1,0));
524
525 PinholeCamera<Cal3_S2> cam1(cameraPose1, *K); // with camera poses
526 PinholeCamera<Cal3_S2> cam2(cameraPose2, *K);
527 PinholeCamera<Cal3_S2> cam3(cameraPose3, *K);
528
529 // create arbitrary body_Pose_sensor (transforms from sensor to body)
530 Pose3 sensor_to_body = Pose3(Rot3::Ypr(y: -M_PI/2, p: 0., r: -M_PI/2), gtsam::Point3(1, 1, 1)); // Pose3(); //
531
532 // These are the poses we want to estimate, from camera measurements
533 Pose3 bodyPose1 = cameraPose1.compose(g: sensor_to_body.inverse());
534 Pose3 bodyPose2 = cameraPose2.compose(g: sensor_to_body.inverse());
535 Pose3 bodyPose3 = cameraPose3.compose(g: sensor_to_body.inverse());
536
537 // three landmarks ~5 meters infront of camera
538 Point3 landmark1(5, 0.5, 1.2);
539 Point3 landmark2(5, -0.5, 1.2);
540 Point3 landmark3(5, 0, 3.0);
541
542 Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
543
544 // Project three landmarks into three cameras
545 projectToMultipleCameras(cam1, cam2, cam3, landmark: landmark1, measurements_cam&: measurements_cam1);
546 projectToMultipleCameras(cam1, cam2, cam3, landmark: landmark2, measurements_cam&: measurements_cam2);
547 projectToMultipleCameras(cam1, cam2, cam3, landmark: landmark3, measurements_cam&: measurements_cam3);
548
549 // Create smart factors
550 KeyVector views;
551 views.push_back(x: x1);
552 views.push_back(x: x2);
553 views.push_back(x: x3);
554
555 // convert measurement to (degenerate) stereoPoint2 (with right pixel being NaN)
556 vector<StereoPoint2> measurements_cam1_stereo, measurements_cam2_stereo, measurements_cam3_stereo;
557 for(size_t k=0; k<measurements_cam1.size();k++)
558 measurements_cam1_stereo.push_back(x: StereoPoint2(measurements_cam1[k].x() , missing_uR , measurements_cam1[k].y()));
559
560 for(size_t k=0; k<measurements_cam2.size();k++)
561 measurements_cam2_stereo.push_back(x: StereoPoint2(measurements_cam2[k].x() , missing_uR , measurements_cam2[k].y()));
562
563 for(size_t k=0; k<measurements_cam3.size();k++)
564 measurements_cam3_stereo.push_back(x: StereoPoint2(measurements_cam3[k].x() , missing_uR , measurements_cam3[k].y()));
565
566 SmartStereoProjectionParams params;
567 params.setRankTolerance(1.0);
568 params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY);
569 params.setEnableEPI(false);
570
571 Cal3_S2Stereo::shared_ptr Kmono(new Cal3_S2Stereo(fov,w,h,b));
572 SmartStereoProjectionPoseFactor smartFactor1(model, params, sensor_to_body);
573 smartFactor1.add(measurements: measurements_cam1_stereo, poseKeys: views, K: Kmono);
574
575 SmartStereoProjectionPoseFactor smartFactor2(model, params, sensor_to_body);
576 smartFactor2.add(measurements: measurements_cam2_stereo, poseKeys: views, K: Kmono);
577
578 SmartStereoProjectionPoseFactor smartFactor3(model, params, sensor_to_body);
579 smartFactor3.add(measurements: measurements_cam3_stereo, poseKeys: views, K: Kmono);
580
581 const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(dim: 6, sigma: 0.10);
582
583 // Put all factors in factor graph, adding priors
584 NonlinearFactorGraph graph;
585 graph.push_back(factor: smartFactor1);
586 graph.push_back(factor: smartFactor2);
587 graph.push_back(factor: smartFactor3);
588 graph.addPrior(key: x1, prior: bodyPose1, model: noisePrior);
589 graph.addPrior(key: x2, prior: bodyPose2, model: noisePrior);
590
591 // Check errors at ground truth poses
592 Values gtValues;
593 gtValues.insert(j: x1, val: bodyPose1);
594 gtValues.insert(j: x2, val: bodyPose2);
595 gtValues.insert(j: x3, val: bodyPose3);
596 double actualError = graph.error(values: gtValues);
597 double expectedError = 0.0;
598 DOUBLES_EQUAL(expectedError, actualError, 1e-7)
599
600 Pose3 noise_pose = Pose3(Rot3::Ypr(y: -M_PI/100, p: 0., r: -M_PI/100), gtsam::Point3(0.1,0.1,0.1));
601 Values values;
602 values.insert(j: x1, val: bodyPose1);
603 values.insert(j: x2, val: bodyPose2);
604 // initialize third pose with some noise, we expect it to move back to original pose3
605 values.insert(j: x3, val: bodyPose3*noise_pose);
606
607 LevenbergMarquardtParams lmParams;
608 Values result;
609 LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
610 result = optimizer.optimize();
611 EXPECT(assert_equal(bodyPose3,result.at<Pose3>(x3)));
612}
613/* *************************************************************************/
614TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) {
615
616 KeyVector views;
617 views.push_back(x: x1);
618 views.push_back(x: x2);
619 views.push_back(x: x3);
620
621 // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
622 Pose3 pose1 = Pose3(Rot3::Ypr(y: -M_PI / 2, p: 0., r: -M_PI / 2), Point3(0, 0, 1));
623 StereoCamera cam1(pose1, K);
624 // create second camera 1 meter to the right of first camera
625 Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0));
626 StereoCamera cam2(pose2, K);
627 // create third camera 1 meter above the first camera
628 Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0));
629 StereoCamera cam3(pose3, K);
630
631 // three landmarks ~5 meters infront of camera
632 Point3 landmark1(5, 0.5, 1.2);
633 Point3 landmark2(5, -0.5, 1.2);
634 Point3 landmark3(3, 0, 3.0);
635
636 // 1. Project three landmarks into three cameras and triangulate
637 vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1,
638 cam2, cam3, landmark: landmark1);
639 vector<StereoPoint2> measurements_cam2 = stereo_projectToMultipleCameras(cam1,
640 cam2, cam3, landmark: landmark2);
641 vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1,
642 cam2, cam3, landmark: landmark3);
643
644 SmartStereoProjectionParams params;
645 params.setLinearizationMode(JACOBIAN_SVD);
646
647 SmartStereoProjectionPoseFactor::shared_ptr smartFactor1( new SmartStereoProjectionPoseFactor(model, params));
648 smartFactor1->add(measurements: measurements_cam1, poseKeys: views, K);
649
650 SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(model, params));
651 smartFactor2->add(measurements: measurements_cam2, poseKeys: views, K);
652
653 SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(model, params));
654 smartFactor3->add(measurements: measurements_cam3, poseKeys: views, K);
655
656 const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(dim: 6, sigma: 0.10);
657
658 NonlinearFactorGraph graph;
659 graph.push_back(factor: smartFactor1);
660 graph.push_back(factor: smartFactor2);
661 graph.push_back(factor: smartFactor3);
662 graph.addPrior(key: x1, prior: pose1, model: noisePrior);
663 graph.addPrior(key: x2, prior: pose2, model: noisePrior);
664
665 // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
666 Pose3 noise_pose = Pose3(Rot3::Ypr(y: -M_PI / 100, p: 0., r: -M_PI / 100),
667 Point3(0.1, 0.1, 0.1)); // smaller noise
668 Values values;
669 values.insert(j: x1, val: pose1);
670 values.insert(j: x2, val: pose2);
671 values.insert(j: x3, val: pose3 * noise_pose);
672
673 Values result;
674 LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
675 result = optimizer.optimize();
676 EXPECT(assert_equal(pose3, result.at<Pose3>(x3)));
677}
678
679/* *************************************************************************/
680TEST( SmartStereoProjectionPoseFactor, jacobianSVDwithMissingValues ) {
681
682 KeyVector views;
683 views.push_back(x: x1);
684 views.push_back(x: x2);
685 views.push_back(x: x3);
686
687 // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
688 Pose3 pose1 = Pose3(Rot3::Ypr(y: -M_PI / 2, p: 0., r: -M_PI / 2), Point3(0, 0, 1));
689 StereoCamera cam1(pose1, K);
690 // create second camera 1 meter to the right of first camera
691 Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0));
692 StereoCamera cam2(pose2, K);
693 // create third camera 1 meter above the first camera
694 Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0));
695 StereoCamera cam3(pose3, K);
696
697 // three landmarks ~5 meters infront of camera
698 Point3 landmark1(5, 0.5, 1.2);
699 Point3 landmark2(5, -0.5, 1.2);
700 Point3 landmark3(3, 0, 3.0);
701
702 // 1. Project three landmarks into three cameras and triangulate
703 vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1,
704 cam2, cam3, landmark: landmark1);
705 vector<StereoPoint2> measurements_cam2 = stereo_projectToMultipleCameras(cam1,
706 cam2, cam3, landmark: landmark2);
707 vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1,
708 cam2, cam3, landmark: landmark3);
709
710 // DELETE SOME MEASUREMENTS
711 StereoPoint2 sp = measurements_cam1[1];
712 measurements_cam1[1] = StereoPoint2(sp.uL(), missing_uR, sp.v());
713 sp = measurements_cam2[2];
714 measurements_cam2[2] = StereoPoint2(sp.uL(), missing_uR, sp.v());
715
716 SmartStereoProjectionParams params;
717 params.setLinearizationMode(JACOBIAN_SVD);
718
719 SmartStereoProjectionPoseFactor::shared_ptr smartFactor1( new SmartStereoProjectionPoseFactor(model, params));
720 smartFactor1->add(measurements: measurements_cam1, poseKeys: views, K);
721
722 SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(model, params));
723 smartFactor2->add(measurements: measurements_cam2, poseKeys: views, K);
724
725 SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(model, params));
726 smartFactor3->add(measurements: measurements_cam3, poseKeys: views, K);
727
728 const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(dim: 6, sigma: 0.10);
729
730 NonlinearFactorGraph graph;
731 graph.push_back(factor: smartFactor1);
732 graph.push_back(factor: smartFactor2);
733 graph.push_back(factor: smartFactor3);
734 graph.addPrior(key: x1, prior: pose1, model: noisePrior);
735 graph.addPrior(key: x2, prior: pose2, model: noisePrior);
736
737 // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
738 Pose3 noise_pose = Pose3(Rot3::Ypr(y: -M_PI / 100, p: 0., r: -M_PI / 100),
739 Point3(0.1, 0.1, 0.1)); // smaller noise
740 Values values;
741 values.insert(j: x1, val: pose1);
742 values.insert(j: x2, val: pose2);
743 values.insert(j: x3, val: pose3 * noise_pose);
744
745 Values result;
746 LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
747 result = optimizer.optimize();
748 EXPECT(assert_equal(pose3, result.at<Pose3>(x3),1e-7));
749}
750
751/* *************************************************************************/
752TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) {
753
754// double excludeLandmarksFutherThanDist = 2;
755
756 KeyVector views;
757 views.push_back(x: x1);
758 views.push_back(x: x2);
759 views.push_back(x: x3);
760
761 // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
762 Pose3 pose1 = Pose3(Rot3::Ypr(y: -M_PI / 2, p: 0., r: -M_PI / 2), Point3(0, 0, 1));
763 StereoCamera cam1(pose1, K);
764 // create second camera 1 meter to the right of first camera
765 Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0));
766 StereoCamera cam2(pose2, K);
767 // create third camera 1 meter above the first camera
768 Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0));
769 StereoCamera cam3(pose3, K);
770
771 // three landmarks ~5 meters infront of camera
772 Point3 landmark1(5, 0.5, 1.2);
773 Point3 landmark2(5, -0.5, 1.2);
774 Point3 landmark3(3, 0, 3.0);
775
776 // 1. Project three landmarks into three cameras and triangulate
777 vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1,
778 cam2, cam3, landmark: landmark1);
779 vector<StereoPoint2> measurements_cam2 = stereo_projectToMultipleCameras(cam1,
780 cam2, cam3, landmark: landmark2);
781 vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1,
782 cam2, cam3, landmark: landmark3);
783
784 SmartStereoProjectionParams params;
785 params.setLinearizationMode(JACOBIAN_SVD);
786 params.setLandmarkDistanceThreshold(2);
787
788 SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(model, params));
789 smartFactor1->add(measurements: measurements_cam1, poseKeys: views, K);
790
791 SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(model, params));
792 smartFactor2->add(measurements: measurements_cam2, poseKeys: views, K);
793
794 SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(model, params));
795 smartFactor3->add(measurements: measurements_cam3, poseKeys: views, K);
796
797 const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(dim: 6, sigma: 0.10);
798
799 NonlinearFactorGraph graph;
800 graph.push_back(factor: smartFactor1);
801 graph.push_back(factor: smartFactor2);
802 graph.push_back(factor: smartFactor3);
803 graph.addPrior(key: x1, prior: pose1, model: noisePrior);
804 graph.addPrior(key: x2, prior: pose2, model: noisePrior);
805
806 // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
807 Pose3 noise_pose = Pose3(Rot3::Ypr(y: -M_PI / 100, p: 0., r: -M_PI / 100),
808 Point3(0.1, 0.1, 0.1)); // smaller noise
809 Values values;
810 values.insert(j: x1, val: pose1);
811 values.insert(j: x2, val: pose2);
812 values.insert(j: x3, val: pose3 * noise_pose);
813
814 // All factors are disabled and pose should remain where it is
815 Values result;
816 LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
817 result = optimizer.optimize();
818 EXPECT(assert_equal(values.at<Pose3>(x3), result.at<Pose3>(x3)));
819}
820
821/* *************************************************************************/
822TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
823
824 KeyVector views;
825 views.push_back(x: x1);
826 views.push_back(x: x2);
827 views.push_back(x: x3);
828
829 // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
830 Pose3 pose1 = Pose3(Rot3::Ypr(y: -M_PI / 2, p: 0., r: -M_PI / 2), Point3(0, 0, 1));
831 StereoCamera cam1(pose1, K);
832 // create second camera 1 meter to the right of first camera
833 Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0));
834 StereoCamera cam2(pose2, K);
835 // create third camera 1 meter above the first camera
836 Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0));
837 StereoCamera cam3(pose3, K);
838
839 // three landmarks ~5 meters infront of camera
840 Point3 landmark1(5, 0.5, 1.2);
841 Point3 landmark2(5, -0.5, 1.2);
842 Point3 landmark3(3, 0, 3.0);
843 Point3 landmark4(5, -0.5, 1);
844
845 // 1. Project four landmarks into three cameras and triangulate
846 vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1,
847 cam2, cam3, landmark: landmark1);
848 vector<StereoPoint2> measurements_cam2 = stereo_projectToMultipleCameras(cam1,
849 cam2, cam3, landmark: landmark2);
850 vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1,
851 cam2, cam3, landmark: landmark3);
852 vector<StereoPoint2> measurements_cam4 = stereo_projectToMultipleCameras(cam1,
853 cam2, cam3, landmark: landmark4);
854
855 measurements_cam4.at(n: 0) = measurements_cam4.at(n: 0) + StereoPoint2(10, 10, 1); // add outlier
856
857 SmartStereoProjectionParams params;
858 params.setLinearizationMode(JACOBIAN_SVD);
859 params.setDynamicOutlierRejectionThreshold(1);
860
861
862 SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(model, params));
863 smartFactor1->add(measurements: measurements_cam1, poseKeys: views, K);
864
865 SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(model, params));
866 smartFactor2->add(measurements: measurements_cam2, poseKeys: views, K);
867
868 SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(model, params));
869 smartFactor3->add(measurements: measurements_cam3, poseKeys: views, K);
870
871 SmartStereoProjectionPoseFactor::shared_ptr smartFactor4(new SmartStereoProjectionPoseFactor(model, params));
872 smartFactor4->add(measurements: measurements_cam4, poseKeys: views, K);
873
874 // same as factor 4, but dynamic outlier rejection is off
875 SmartStereoProjectionPoseFactor::shared_ptr smartFactor4b(new SmartStereoProjectionPoseFactor(model));
876 smartFactor4b->add(measurements: measurements_cam4, poseKeys: views, K);
877
878 const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(dim: 6, sigma: 0.10);
879
880 NonlinearFactorGraph graph;
881 graph.push_back(factor: smartFactor1);
882 graph.push_back(factor: smartFactor2);
883 graph.push_back(factor: smartFactor3);
884 graph.push_back(factor: smartFactor4);
885 graph.addPrior(key: x1, prior: pose1, model: noisePrior);
886 graph.addPrior(key: x2, prior: pose2, model: noisePrior);
887
888 Pose3 noise_pose = Pose3(Rot3::Ypr(y: -M_PI / 100, p: 0., r: -M_PI / 100),
889 Point3(0.1, 0.1, 0.1)); // smaller noise
890 Values values;
891 values.insert(j: x1, val: pose1);
892 values.insert(j: x2, val: pose2);
893 values.insert(j: x3, val: pose3);
894
895 EXPECT_DOUBLES_EQUAL(0, smartFactor1->error(values), 1e-9);
896 EXPECT_DOUBLES_EQUAL(0, smartFactor2->error(values), 1e-9);
897 EXPECT_DOUBLES_EQUAL(0, smartFactor3->error(values), 1e-9);
898 // zero error due to dynamic outlier rejection
899 EXPECT_DOUBLES_EQUAL(0, smartFactor4->error(values), 1e-9);
900
901 // dynamic outlier rejection is off
902 EXPECT_DOUBLES_EQUAL(6147.3947317473921, smartFactor4b->error(values), 1e-9);
903
904 // Factors 1-3 should have valid point, factor 4 should not
905 EXPECT(smartFactor1->point());
906 EXPECT(smartFactor2->point());
907 EXPECT(smartFactor3->point());
908 EXPECT(smartFactor4->point().outlier());
909 EXPECT(smartFactor4b->point());
910
911 // Factor 4 is disabled, pose 3 stays put
912 Values result;
913 LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
914 result = optimizer.optimize();
915 EXPECT(assert_equal(pose3, result.at<Pose3>(x3)));
916}
917//
918///* *************************************************************************/
919//TEST( SmartStereoProjectionPoseFactor, jacobianQ ){
920//
921// KeyVector views;
922// views.push_back(x1);
923// views.push_back(x2);
924// views.push_back(x3);
925//
926// // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
927// Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1));
928// StereoCamera cam1(pose1, K);
929// // create second camera 1 meter to the right of first camera
930// Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0));
931// StereoCamera cam2(pose2, K);
932// // create third camera 1 meter above the first camera
933// Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0));
934// StereoCamera cam3(pose3, K);
935//
936// // three landmarks ~5 meters infront of camera
937// Point3 landmark1(5, 0.5, 1.2);
938// Point3 landmark2(5, -0.5, 1.2);
939// Point3 landmark3(3, 0, 3.0);
940//
941// vector<StereoPoint2> measurements_cam1, measurements_cam2, measurements_cam3;
942//
943// // 1. Project three landmarks into three cameras and triangulate
944// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
945// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
946// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
947//
948// SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(1, -1, false, false, JACOBIAN_Q));
949// smartFactor1->add(measurements_cam1, views, model, K);
950//
951// SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(1, -1, false, false, JACOBIAN_Q));
952// smartFactor2->add(measurements_cam2, views, model, K);
953//
954// SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(1, -1, false, false, JACOBIAN_Q));
955// smartFactor3->add(measurements_cam3, views, model, K);
956//
957// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
958//
959// NonlinearFactorGraph graph;
960// graph.push_back(smartFactor1);
961// graph.push_back(smartFactor2);
962// graph.push_back(smartFactor3);
963// graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
964// graph.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));
965//
966// // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
967// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/100, 0., -M_PI/100), Point3(0.1,0.1,0.1)); // smaller noise
968// Values values;
969// values.insert(x1, pose1);
970// values.insert(x2, pose2);
971// values.insert(x3, pose3*noise_pose);
972//
973//// Values result;
974// LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
975// result = optimizer.optimize();
976// EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
977//}
978//
979///* *************************************************************************/
980//TEST( SmartStereoProjectionPoseFactor, 3poses_projection_factor ){
981//
982// KeyVector views;
983// views.push_back(x1);
984// views.push_back(x2);
985// views.push_back(x3);
986//
987// // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
988// Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1));
989// StereoCamera cam1(pose1, K2);
990//
991// // create second camera 1 meter to the right of first camera
992// Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0));
993// StereoCamera cam2(pose2, K2);
994//
995// // create third camera 1 meter above the first camera
996// Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0));
997// StereoCamera cam3(pose3, K2);
998//
999// // three landmarks ~5 meters infront of camera
1000// Point3 landmark1(5, 0.5, 1.2);
1001// Point3 landmark2(5, -0.5, 1.2);
1002// Point3 landmark3(3, 0, 3.0);
1003//
1004// typedef GenericStereoFactor<Pose3, Point3> ProjectionFactor;
1005// NonlinearFactorGraph graph;
1006//
1007// // 1. Project three landmarks into three cameras and triangulate
1008// graph.push_back(ProjectionFactor(cam1.project(landmark1), model, x1, L(1), K2));
1009// graph.push_back(ProjectionFactor(cam2.project(landmark1), model, x2, L(1), K2));
1010// graph.push_back(ProjectionFactor(cam3.project(landmark1), model, x3, L(1), K2));
1011//
1012// graph.push_back(ProjectionFactor(cam1.project(landmark2), model, x1, L(2), K2));
1013// graph.push_back(ProjectionFactor(cam2.project(landmark2), model, x2, L(2), K2));
1014// graph.push_back(ProjectionFactor(cam3.project(landmark2), model, x3, L(2), K2));
1015//
1016// graph.push_back(ProjectionFactor(cam1.project(landmark3), model, x1, L(3), K2));
1017// graph.push_back(ProjectionFactor(cam2.project(landmark3), model, x2, L(3), K2));
1018// graph.push_back(ProjectionFactor(cam3.project(landmark3), model, x3, L(3), K2));
1019//
1020// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
1021// graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
1022// graph.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));
1023//
1024// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3));
1025// Values values;
1026// values.insert(x1, pose1);
1027// values.insert(x2, pose2);
1028// values.insert(x3, pose3* noise_pose);
1029// values.insert(L(1), landmark1);
1030// values.insert(L(2), landmark2);
1031// values.insert(L(3), landmark3);
1032//
1033// LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
1034// Values result = optimizer.optimize();
1035//
1036// EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
1037//}
1038//
1039/* *************************************************************************/
1040TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
1041
1042 KeyVector views;
1043 views.push_back(x: x1);
1044 views.push_back(x: x2);
1045 views.push_back(x: x3);
1046
1047 // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
1048 Pose3 pose1 = Pose3(Rot3::Ypr(y: -M_PI / 2, p: 0., r: -M_PI / 2), Point3(0, 0, 1));
1049 StereoCamera cam1(pose1, K);
1050
1051 // create second camera
1052 Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(x: -0.05, y: 0.0, z: -0.05), Point3(0, 0, 0));
1053 StereoCamera cam2(pose2, K);
1054
1055 // create third camera
1056 Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(x: -0.05, y: 0.0, z: -0.05), Point3(0, 0, 0));
1057 StereoCamera cam3(pose3, K);
1058
1059 // three landmarks ~5 meters infront of camera
1060 Point3 landmark1(5, 0.5, 1.2);
1061 Point3 landmark2(5, -0.5, 1.2);
1062 Point3 landmark3(3, 0, 3.0);
1063
1064 // Project three landmarks into three cameras and triangulate
1065 vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1,
1066 cam2, cam3, landmark: landmark1);
1067 vector<StereoPoint2> measurements_cam2 = stereo_projectToMultipleCameras(cam1,
1068 cam2, cam3, landmark: landmark2);
1069 vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1,
1070 cam2, cam3, landmark: landmark3);
1071
1072 SmartStereoProjectionParams params;
1073 params.setRankTolerance(10);
1074
1075 SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(model, params));
1076 smartFactor1->add(measurements: measurements_cam1, poseKeys: views, K);
1077
1078 SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(model, params));
1079 smartFactor2->add(measurements: measurements_cam2, poseKeys: views, K);
1080
1081 SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(model, params));
1082 smartFactor3->add(measurements: measurements_cam3, poseKeys: views, K);
1083
1084 // Create graph to optimize
1085 NonlinearFactorGraph graph;
1086 graph.push_back(factor: smartFactor1);
1087 graph.push_back(factor: smartFactor2);
1088 graph.push_back(factor: smartFactor3);
1089
1090 Values values;
1091 values.insert(j: x1, val: pose1);
1092 values.insert(j: x2, val: pose2);
1093 // initialize third pose with some noise, we expect it to move back to original pose3
1094 Pose3 noise_pose = Pose3(Rot3::Ypr(y: -M_PI / 100, p: 0., r: -M_PI / 100),
1095 Point3(0.1, 0.1, 0.1)); // smaller noise
1096 values.insert(j: x3, val: pose3 * noise_pose);
1097
1098 // TODO: next line throws Cheirality exception on Mac
1099 std::shared_ptr<GaussianFactor> hessianFactor1 = smartFactor1->linearize(
1100 values);
1101 std::shared_ptr<GaussianFactor> hessianFactor2 = smartFactor2->linearize(
1102 values);
1103 std::shared_ptr<GaussianFactor> hessianFactor3 = smartFactor3->linearize(
1104 values);
1105
1106 Matrix CumulativeInformation = hessianFactor1->information()
1107 + hessianFactor2->information() + hessianFactor3->information();
1108
1109 std::shared_ptr<GaussianFactorGraph> GaussianGraph = graph.linearize(
1110 linearizationPoint: values);
1111 Matrix GraphInformation = GaussianGraph->hessian().first;
1112
1113 // Check Hessian
1114 EXPECT(assert_equal(GraphInformation, CumulativeInformation, 1e-8));
1115
1116 Matrix AugInformationMatrix = hessianFactor1->augmentedInformation()
1117 + hessianFactor2->augmentedInformation()
1118 + hessianFactor3->augmentedInformation();
1119
1120 // Check Information vector
1121 Vector InfoVector = AugInformationMatrix.block(startRow: 0, startCol: 18, blockRows: 18, blockCols: 1); // 18x18 Hessian + information vector
1122
1123 // Check Hessian
1124 EXPECT(assert_equal(InfoVector, GaussianGraph->hessian().second, 1e-8));
1125}
1126//
1127///* *************************************************************************/
1128//TEST( SmartStereoProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_factor ){
1129//
1130// KeyVector views;
1131// views.push_back(x1);
1132// views.push_back(x2);
1133// views.push_back(x3);
1134//
1135// // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
1136// Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1));
1137// StereoCamera cam1(pose1, K2);
1138//
1139// // create second camera 1 meter to the right of first camera
1140// Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0));
1141// StereoCamera cam2(pose2, K2);
1142//
1143// // create third camera 1 meter above the first camera
1144// Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0));
1145// StereoCamera cam3(pose3, K2);
1146//
1147// // three landmarks ~5 meters infront of camera
1148// Point3 landmark1(5, 0.5, 1.2);
1149// Point3 landmark2(5, -0.5, 1.2);
1150//
1151// vector<StereoPoint2> measurements_cam1, measurements_cam2, measurements_cam3;
1152//
1153// // 1. Project three landmarks into three cameras and triangulate
1154// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
1155// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
1156//
1157// double rankTol = 50;
1158// SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(rankTol, linThreshold, manageDegeneracy));
1159// smartFactor1->add(measurements_cam1, views, model, K2);
1160//
1161// SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(rankTol, linThreshold, manageDegeneracy));
1162// smartFactor2->add(measurements_cam2, views, model, K2);
1163//
1164// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
1165// const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10);
1166// Point3 positionPrior = Point3(0,0,1);
1167//
1168// NonlinearFactorGraph graph;
1169// graph.push_back(smartFactor1);
1170// graph.push_back(smartFactor2);
1171// graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
1172// graph.push_back(PoseTranslationPrior<Pose3>(x2, positionPrior, noisePriorTranslation));
1173// graph.push_back(PoseTranslationPrior<Pose3>(x3, positionPrior, noisePriorTranslation));
1174//
1175// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.1,0.1,0.1)); // smaller noise
1176// Values values;
1177// values.insert(x1, pose1);
1178// values.insert(x2, pose2*noise_pose);
1179// // initialize third pose with some noise, we expect it to move back to original pose3
1180// values.insert(x3, pose3*noise_pose*noise_pose);
1181//
1182// Values result;
1183// gttic_(SmartStereoProjectionPoseFactor);
1184// LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
1185// result = optimizer.optimize();
1186// gttoc_(SmartStereoProjectionPoseFactor);
1187// tictoc_finishedIteration_();
1188//
1189// // result.print("results of 3 camera, 3 landmark optimization \n");
1190// // EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
1191//}
1192//
1193///* *************************************************************************/
1194//TEST( SmartStereoProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ){
1195//
1196// KeyVector views;
1197// views.push_back(x1);
1198// views.push_back(x2);
1199// views.push_back(x3);
1200//
1201// // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
1202// Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1));
1203// StereoCamera cam1(pose1, K);
1204//
1205// // create second camera 1 meter to the right of first camera
1206// Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0));
1207// StereoCamera cam2(pose2, K);
1208//
1209// // create third camera 1 meter above the first camera
1210// Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0));
1211// StereoCamera cam3(pose3, K);
1212//
1213// // three landmarks ~5 meters infront of camera
1214// Point3 landmark1(5, 0.5, 1.2);
1215// Point3 landmark2(5, -0.5, 1.2);
1216// Point3 landmark3(3, 0, 3.0);
1217//
1218// vector<StereoPoint2> measurements_cam1, measurements_cam2, measurements_cam3;
1219//
1220// // 1. Project three landmarks into three cameras and triangulate
1221// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
1222// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
1223// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
1224//
1225// double rankTol = 10;
1226//
1227// SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(rankTol, linThreshold, manageDegeneracy));
1228// smartFactor1->add(measurements_cam1, views, model, K);
1229//
1230// SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(rankTol, linThreshold, manageDegeneracy));
1231// smartFactor2->add(measurements_cam2, views, model, K);
1232//
1233// SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(rankTol, linThreshold, manageDegeneracy));
1234// smartFactor3->add(measurements_cam3, views, model, K);
1235//
1236// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
1237// const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10);
1238// Point3 positionPrior = Point3(0,0,1);
1239//
1240// NonlinearFactorGraph graph;
1241// graph.push_back(smartFactor1);
1242// graph.push_back(smartFactor2);
1243// graph.push_back(smartFactor3);
1244// graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
1245// graph.push_back(PoseTranslationPrior<Pose3>(x2, positionPrior, noisePriorTranslation));
1246// graph.push_back(PoseTranslationPrior<Pose3>(x3, positionPrior, noisePriorTranslation));
1247//
1248// // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
1249// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/100, 0., -M_PI/100), Point3(0.1,0.1,0.1)); // smaller noise
1250// Values values;
1251// values.insert(x1, pose1);
1252// values.insert(x2, pose2);
1253// // initialize third pose with some noise, we expect it to move back to original pose3
1254// values.insert(x3, pose3*noise_pose);
1255//
1256// Values result;
1257// gttic_(SmartStereoProjectionPoseFactor);
1258// LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
1259// result = optimizer.optimize();
1260// gttoc_(SmartStereoProjectionPoseFactor);
1261// tictoc_finishedIteration_();
1262//
1263// // result.print("results of 3 camera, 3 landmark optimization \n");
1264// // EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
1265//}
1266//
1267///* *************************************************************************/
1268//TEST( SmartStereoProjectionPoseFactor, Hessian ){
1269//
1270// KeyVector views;
1271// views.push_back(x1);
1272// views.push_back(x2);
1273//
1274// // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
1275// Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1));
1276// StereoCamera cam1(pose1, K2);
1277//
1278// // create second camera 1 meter to the right of first camera
1279// Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0));
1280// StereoCamera cam2(pose2, K2);
1281//
1282// // three landmarks ~5 meters infront of camera
1283// Point3 landmark1(5, 0.5, 1.2);
1284//
1285// // 1. Project three landmarks into three cameras and triangulate
1286// StereoPoint2 cam1_uv1 = cam1.project(landmark1);
1287// StereoPoint2 cam2_uv1 = cam2.project(landmark1);
1288// vector<StereoPoint2> measurements_cam1;
1289// measurements_cam1.push_back(cam1_uv1);
1290// measurements_cam1.push_back(cam2_uv1);
1291//
1292// SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor());
1293// smartFactor1->add(measurements_cam1,views, model, K2);
1294//
1295// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3));
1296// Values values;
1297// values.insert(x1, pose1);
1298// values.insert(x2, pose2);
1299//
1300// std::shared_ptr<GaussianFactor> hessianFactor = smartFactor1->linearize(values);
1301//
1302// // compute triangulation from linearization point
1303// // compute reprojection errors (sum squared)
1304// // compare with hessianFactor.info(): the bottom right element is the squared sum of the reprojection errors (normalized by the covariance)
1305// // check that it is correctly scaled when using noiseProjection = [1/4 0; 0 1/4]
1306//}
1307//
1308
1309/* *************************************************************************/
1310TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) {
1311 KeyVector views;
1312 views.push_back(x: x1);
1313 views.push_back(x: x2);
1314 views.push_back(x: x3);
1315
1316 // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
1317 Pose3 pose1 = Pose3(Rot3::Ypr(y: -M_PI / 2, p: 0., r: -M_PI / 2), Point3(0, 0, 1));
1318 StereoCamera cam1(pose1, K);
1319
1320 // create second camera 1 meter to the right of first camera
1321 Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0));
1322 StereoCamera cam2(pose2, K);
1323
1324 // create third camera 1 meter above the first camera
1325 Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0));
1326 StereoCamera cam3(pose3, K);
1327
1328 Point3 landmark1(5, 0.5, 1.2);
1329
1330 vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1,
1331 cam2, cam3, landmark: landmark1);
1332
1333 SmartStereoProjectionPoseFactor::shared_ptr smartFactorInstance(new SmartStereoProjectionPoseFactor(model));
1334 smartFactorInstance->add(measurements: measurements_cam1, poseKeys: views, K);
1335
1336 Values values;
1337 values.insert(j: x1, val: pose1);
1338 values.insert(j: x2, val: pose2);
1339 values.insert(j: x3, val: pose3);
1340
1341 std::shared_ptr<GaussianFactor> hessianFactor =
1342 smartFactorInstance->linearize(values);
1343 // hessianFactor->print("Hessian factor \n");
1344
1345 Pose3 poseDrift = Pose3(Rot3::Ypr(y: -M_PI / 2, p: 0., r: -M_PI / 2), Point3(0, 0, 0));
1346
1347 Values rotValues;
1348 rotValues.insert(j: x1, val: poseDrift.compose(g: pose1));
1349 rotValues.insert(j: x2, val: poseDrift.compose(g: pose2));
1350 rotValues.insert(j: x3, val: poseDrift.compose(g: pose3));
1351
1352 std::shared_ptr<GaussianFactor> hessianFactorRot =
1353 smartFactorInstance->linearize(values: rotValues);
1354 // hessianFactorRot->print("Hessian factor \n");
1355
1356 // Hessian is invariant to rotations in the nondegenerate case
1357 EXPECT(
1358 assert_equal(hessianFactor->information(),
1359 hessianFactorRot->information(), 1e-7));
1360
1361 Pose3 poseDrift2 = Pose3(Rot3::Ypr(y: -M_PI / 2, p: -M_PI / 3, r: -M_PI / 2),
1362 Point3(10, -4, 5));
1363
1364 Values tranValues;
1365 tranValues.insert(j: x1, val: poseDrift2.compose(g: pose1));
1366 tranValues.insert(j: x2, val: poseDrift2.compose(g: pose2));
1367 tranValues.insert(j: x3, val: poseDrift2.compose(g: pose3));
1368
1369 std::shared_ptr<GaussianFactor> hessianFactorRotTran =
1370 smartFactorInstance->linearize(values: tranValues);
1371
1372 // Hessian is invariant to rotations and translations in the nondegenerate case
1373 EXPECT(
1374 assert_equal(hessianFactor->information(),
1375 hessianFactorRotTran->information(), 1e-6));
1376}
1377
1378/* *************************************************************************/
1379TEST( SmartStereoProjectionPoseFactor, HessianWithRotationNonDegenerate ) {
1380
1381 KeyVector views;
1382 views.push_back(x: x1);
1383 views.push_back(x: x2);
1384 views.push_back(x: x3);
1385
1386 // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
1387 Pose3 pose1 = Pose3(Rot3::Ypr(y: -M_PI / 2, p: 0., r: -M_PI / 2), Point3(0, 0, 1));
1388 StereoCamera cam1(pose1, K2);
1389
1390 // Second and third cameras in same place, which is a degenerate configuration
1391 Pose3 pose2 = pose1;
1392 Pose3 pose3 = pose1;
1393 StereoCamera cam2(pose2, K2);
1394 StereoCamera cam3(pose3, K2);
1395
1396 Point3 landmark1(5, 0.5, 1.2);
1397
1398 vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1,
1399 cam2, cam3, landmark: landmark1);
1400
1401 SmartStereoProjectionPoseFactor::shared_ptr smartFactor(new SmartStereoProjectionPoseFactor(model));
1402 smartFactor->add(measurements: measurements_cam1, poseKeys: views, K: K2);
1403
1404 Values values;
1405 values.insert(j: x1, val: pose1);
1406 values.insert(j: x2, val: pose2);
1407 values.insert(j: x3, val: pose3);
1408
1409 std::shared_ptr<GaussianFactor> hessianFactor = smartFactor->linearize(
1410 values);
1411
1412 // check that it is non degenerate
1413 EXPECT(smartFactor->isValid());
1414
1415 Pose3 poseDrift = Pose3(Rot3::Ypr(y: -M_PI / 2, p: 0., r: -M_PI / 2), Point3(0, 0, 0));
1416
1417 Values rotValues;
1418 rotValues.insert(j: x1, val: poseDrift.compose(g: pose1));
1419 rotValues.insert(j: x2, val: poseDrift.compose(g: pose2));
1420 rotValues.insert(j: x3, val: poseDrift.compose(g: pose3));
1421
1422 std::shared_ptr<GaussianFactor> hessianFactorRot = smartFactor->linearize(
1423 values: rotValues);
1424
1425 // check that it is non degenerate
1426 EXPECT(smartFactor->isValid());
1427
1428 // Hessian is invariant to rotations in the nondegenerate case
1429 EXPECT(
1430 assert_equal(hessianFactor->information(),
1431 hessianFactorRot->information(), 1e-6));
1432
1433 Pose3 poseDrift2 = Pose3(Rot3::Ypr(y: -M_PI / 2, p: -M_PI / 3, r: -M_PI / 2),
1434 Point3(10, -4, 5));
1435
1436 Values tranValues;
1437 tranValues.insert(j: x1, val: poseDrift2.compose(g: pose1));
1438 tranValues.insert(j: x2, val: poseDrift2.compose(g: pose2));
1439 tranValues.insert(j: x3, val: poseDrift2.compose(g: pose3));
1440
1441 std::shared_ptr<GaussianFactor> hessianFactorRotTran =
1442 smartFactor->linearize(values: tranValues);
1443
1444 double error;
1445#ifdef GTSAM_USE_EIGEN_MKL
1446 error = 1e-5;
1447#else
1448 error = 1e-6;
1449#endif
1450 // Hessian is invariant to rotations and translations in the degenerate case
1451 EXPECT(assert_equal(hessianFactor->information(), hessianFactorRotTran->information(), error));
1452}
1453
1454/* ************************************************************************* */
1455int main() {
1456 TestResult tr;
1457 return TestRegistry::runAllTests(result&: tr);
1458}
1459/* ************************************************************************* */
1460
1461