testSmartProjectionFactor.cpp
Go to the documentation of this file.
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 
22 #include "smartFactorScenarios.h"
23 #include <gtsam/slam/SmartProjectionFactor.h>
24 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
25 #include <CppUnitLite/TestHarness.h>
26 #include <iostream>
27 
28 namespace {
29 static const bool isDebugTest = false;
30 static const Symbol l1('l', 1), l2('l', 2), l3('l', 3);
31 static const Key c1 = 1, c2 = 2, c3 = 3;
32 static const Point2 measurement1(323.0, 240.0);
33 static const double rankTol = 1.0;
34 }
35 
36 template<class CALIBRATION>
37 PinholeCamera<CALIBRATION> perturbCameraPoseAndCalibration(
38  const PinholeCamera<CALIBRATION>& camera) {
39  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10),
40  Point3(0.5, 0.1, 0.3));
41  Pose3 cameraPose = camera.pose();
42  Pose3 perturbedCameraPose = cameraPose.compose(noise_pose);
43  typename gtsam::traits<CALIBRATION>::TangentVector d;
44  d.setRandom();
45  d *= 0.1;
46  CALIBRATION perturbedCalibration = camera.calibration().retract(d);
47  return PinholeCamera<CALIBRATION>(perturbedCameraPose, perturbedCalibration);
48 }
49 
50 /* ************************************************************************* */
51 TEST(SmartProjectionFactor, perturbCameraPose) {
52  using namespace vanilla;
53  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10),
54  Point3(0.5, 0.1, 0.3));
55  Pose3 perturbed_level_pose = level_pose.compose(noise_pose);
56  Camera actualCamera(perturbed_level_pose, K2);
57 
58  Camera expectedCamera = perturbCameraPose(level_camera);
59  CHECK(assert_equal(expectedCamera, actualCamera));
60 }
61 
62 /* ************************************************************************* */
63 TEST(SmartProjectionFactor, Constructor) {
64  using namespace vanilla;
65  SmartFactor::shared_ptr factor1(new SmartFactor(unit2));
66 }
67 
68 /* ************************************************************************* */
69 TEST(SmartProjectionFactor, Constructor2) {
70  using namespace vanilla;
71  auto myParams = params;
72  myParams.setRankTolerance(rankTol);
73  SmartFactor factor1(unit2, myParams);
74 }
75 
76 /* ************************************************************************* */
77 TEST(SmartProjectionFactor, Constructor3) {
78  using namespace vanilla;
79  SmartFactor::shared_ptr factor1(new SmartFactor(unit2));
80  factor1->add(measurement1, c1);
81 }
82 
83 /* ************************************************************************* */
84 TEST(SmartProjectionFactor, Constructor4) {
85  using namespace vanilla;
86  auto myParams = params;
87  myParams.setRankTolerance(rankTol);
88  SmartFactor factor1(unit2, myParams);
89  factor1.add(measurement1, c1);
90 }
91 
92 /* ************************************************************************* */
93 TEST(SmartProjectionFactor, Equals ) {
94  using namespace vanilla;
95  SmartFactor::shared_ptr factor1(new SmartFactor(unit2));
96  factor1->add(measurement1, c1);
97 
98  SmartFactor::shared_ptr factor2(new SmartFactor(unit2));
99  factor2->add(measurement1, c1);
100 }
101 
102 /* *************************************************************************/
103 TEST(SmartProjectionFactor, noiseless ) {
104  using namespace vanilla;
105 
106  Values values;
107  values.insert(c1, level_camera);
108  values.insert(c2, level_camera_right);
109 
110  SmartFactor::shared_ptr factor1(new SmartFactor(unit2));
111  factor1->add(level_uv, c1);
112  factor1->add(level_uv_right, c2);
113 
114  double expectedError = 0.0;
115  DOUBLES_EQUAL(expectedError, factor1->error(values), 1e-7);
116  CHECK(
117  assert_equal(Z_4x1,
118  factor1->reprojectionErrorAfterTriangulation(values), 1e-7));
119 }
120 
121 /* *************************************************************************/
122 TEST(SmartProjectionFactor, noisy ) {
123 
124  using namespace vanilla;
125 
126  // Project one landmark into two cameras and add noise on first
127  Point2 level_uv = level_camera.project(landmark1) + Point2(0.2, 0.2);
128  Point2 level_uv_right = level_camera_right.project(landmark1);
129 
130  Values values;
131  values.insert(c1, level_camera);
132  Camera perturbed_level_camera_right = perturbCameraPose(level_camera_right);
133  values.insert(c2, perturbed_level_camera_right);
134 
135  SmartFactor::shared_ptr factor1(new SmartFactor(unit2));
136  factor1->add(level_uv, c1);
137  factor1->add(level_uv_right, c2);
138 
139  // Point is now uninitialized before a triangulation event
140  EXPECT(!factor1->point());
141 
142  double expectedError = 58640;
143  double actualError1 = factor1->error(values);
144  EXPECT_DOUBLES_EQUAL(expectedError, actualError1, 1);
145 
146  // Check triangulated point
147  CHECK(factor1->point());
148  EXPECT(
149  assert_equal(Point3(13.7587, 1.43851, -1.14274), *factor1->point(), 1e-4));
150 
151  // Check whitened errors
152  Vector expected(4);
153  expected << -7, 235, 58, -242;
154  SmartFactor::Cameras cameras1 = factor1->cameras(values);
155  Point3 point1 = *factor1->point();
156  Vector actual = factor1->whitenedError(cameras1, point1);
157  EXPECT(assert_equal(expected, actual, 1));
158 
159  SmartFactor::shared_ptr factor2(new SmartFactor(unit2));
160  Point2Vector measurements;
161  measurements.push_back(level_uv);
162  measurements.push_back(level_uv_right);
163 
164  KeyVector views {c1, c2};
165 
166  factor2->add(measurements, views);
167 
168  double actualError2 = factor2->error(values);
169  EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1);
170 }
171 
172 /* *************************************************************************/
173 TEST(SmartProjectionFactor, perturbPoseAndOptimize ) {
174 
175  using namespace vanilla;
176 
177  // Project three landmarks into three cameras
178  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
179  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
180  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
181  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
182 
183  // Create and fill smartfactors
184  SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2));
185  SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2));
186  SmartFactor::shared_ptr smartFactor3(new SmartFactor(unit2));
187  KeyVector views {c1, c2, c3};
188  smartFactor1->add(measurements_cam1, views);
189  smartFactor2->add(measurements_cam2, views);
190  smartFactor3->add(measurements_cam3, views);
191 
192  // Create factor graph and add priors on two cameras
193  NonlinearFactorGraph graph;
194  graph.push_back(smartFactor1);
195  graph.push_back(smartFactor2);
196  graph.push_back(smartFactor3);
197  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6 + 5, 1e-5);
198  graph.addPrior(c1, cam1, noisePrior);
199  graph.addPrior(c2, cam2, noisePrior);
200 
201  // Create initial estimate
202  Values initial;
203  initial.insert(c1, cam1);
204  initial.insert(c2, cam2);
205  initial.insert(c3, perturbCameraPose(cam3));
206  if (isDebugTest)
207  initial.at<Camera>(c3).print("Smart: Pose3 before optimization: ");
208 
209  // Points are now uninitialized before a triangulation event
210  EXPECT(!smartFactor1->point());
211  EXPECT(!smartFactor2->point());
212  EXPECT(!smartFactor3->point());
213 
214  EXPECT_DOUBLES_EQUAL(75711, smartFactor1->error(initial), 1);
215  EXPECT_DOUBLES_EQUAL(58524, smartFactor2->error(initial), 1);
216  EXPECT_DOUBLES_EQUAL(77564, smartFactor3->error(initial), 1);
217 
218  // Error should trigger this and initialize the points, abort if not so
219  CHECK(smartFactor1->point());
220  CHECK(smartFactor2->point());
221  CHECK(smartFactor3->point());
222 
223  EXPECT(
224  assert_equal(Point3(2.57696, -0.182566, 1.04085), *smartFactor1->point(),
225  1e-4));
226  EXPECT(
227  assert_equal(Point3(2.80114, -0.702153, 1.06594), *smartFactor2->point(),
228  1e-4));
229  EXPECT(
230  assert_equal(Point3(1.82593, -0.289569, 2.13438), *smartFactor3->point(),
231  1e-4));
232 
233  // Check whitened errors
234  Vector expected(6);
235  expected << 256, 29, -26, 29, -206, -202;
236  Point3 point1 = *smartFactor1->point();
237  SmartFactor::Cameras cameras1 = smartFactor1->cameras(initial);
238  Vector reprojectionError = cameras1.reprojectionError(point1,
239  measurements_cam1);
240  EXPECT(assert_equal(expected, reprojectionError, 1));
241  Vector actual = smartFactor1->whitenedError(cameras1, point1);
242  EXPECT(assert_equal(expected, actual, 1));
243 
244  // Optimize
245  LevenbergMarquardtParams lmParams;
246  if (isDebugTest) {
247  lmParams.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
248  lmParams.verbosity = NonlinearOptimizerParams::ERROR;
249  }
250  LevenbergMarquardtOptimizer optimizer(graph, initial, lmParams);
251  Values result = optimizer.optimize();
252 
253  EXPECT(assert_equal(landmark1, *smartFactor1->point(), 1e-5));
254  EXPECT(assert_equal(landmark2, *smartFactor2->point(), 1e-5));
255  EXPECT(assert_equal(landmark3, *smartFactor3->point(), 1e-5));
256 
257  // GaussianFactorGraph::shared_ptr GFG = graph.linearize(initial);
258  // VectorValues delta = GFG->optimize();
259 
260  if (isDebugTest)
261  result.at<Camera>(c3).print("Smart: Pose3 after optimization: ");
262  EXPECT(assert_equal(cam1, result.at<Camera>(c1)));
263  EXPECT(assert_equal(cam2, result.at<Camera>(c2)));
264  EXPECT(assert_equal(result.at<Camera>(c3).pose(), cam3.pose(), 1e-4));
265  EXPECT(
266  assert_equal(result.at<Camera>(c3).calibration(), cam3.calibration(), 2));
267  if (isDebugTest)
268  tictoc_print_();
269 }
270 
271 /* *************************************************************************/
272 TEST(SmartProjectionFactor, perturbPoseAndOptimizeFromSfM_tracks ) {
273 
274  using namespace vanilla;
275 
276  // Project three landmarks into three cameras
277  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
278  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
279  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
280  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
281 
282  KeyVector views {c1, c2, c3};
283 
284  SfmTrack track1;
285  for (size_t i = 0; i < 3; ++i) {
286  track1.measurements.emplace_back(i + 1, measurements_cam1.at(i));
287  }
288  SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2));
289  smartFactor1->add(track1);
290 
291  SfmTrack track2;
292  for (size_t i = 0; i < 3; ++i) {
293  track2.measurements.emplace_back(i + 1, measurements_cam2.at(i));
294  }
295  SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2));
296  smartFactor2->add(track2);
297 
298  SmartFactor::shared_ptr smartFactor3(new SmartFactor(unit2));
299  smartFactor3->add(measurements_cam3, views);
300 
301  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6 + 5, 1e-5);
302 
303  NonlinearFactorGraph graph;
304  graph.push_back(smartFactor1);
305  graph.push_back(smartFactor2);
306  graph.push_back(smartFactor3);
307  graph.addPrior(c1, cam1, noisePrior);
308  graph.addPrior(c2, cam2, noisePrior);
309 
310  Values values;
311  values.insert(c1, cam1);
312  values.insert(c2, cam2);
313  values.insert(c3, perturbCameraPose(cam3));
314  if (isDebugTest)
315  values.at<Camera>(c3).print("Smart: Pose3 before optimization: ");
316 
317  LevenbergMarquardtParams lmParams;
318  if (isDebugTest)
319  lmParams.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
320  if (isDebugTest)
321  lmParams.verbosity = NonlinearOptimizerParams::ERROR;
322 
323  Values result;
324  LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
325  result = optimizer.optimize();
326 
327  // GaussianFactorGraph::shared_ptr GFG = graph.linearize(values);
328  // VectorValues delta = GFG->optimize();
329 
330  if (isDebugTest)
331  result.at<Camera>(c3).print("Smart: Pose3 after optimization: ");
332  EXPECT(assert_equal(cam1, result.at<Camera>(c1)));
333  EXPECT(assert_equal(cam2, result.at<Camera>(c2)));
334  EXPECT(assert_equal(result.at<Camera>(c3).pose(), cam3.pose(), 1e-4));
335  EXPECT(
336  assert_equal(result.at<Camera>(c3).calibration(), cam3.calibration(), 2));
337  if (isDebugTest)
338  tictoc_print_();
339 }
340 
341 /* *************************************************************************/
342 TEST(SmartProjectionFactor, perturbCamerasAndOptimize ) {
343 
344  using namespace vanilla;
345 
346  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3,
347  measurements_cam4, measurements_cam5;
348 
349  // 1. Project three landmarks into three cameras and triangulate
350  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
351  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
352  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
353  projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4);
354  projectToMultipleCameras(cam1, cam2, cam3, landmark5, measurements_cam5);
355 
356  KeyVector views {c1, c2, c3};
357 
358  SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2));
359  smartFactor1->add(measurements_cam1, views);
360 
361  SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2));
362  smartFactor2->add(measurements_cam2, views);
363 
364  SmartFactor::shared_ptr smartFactor3(new SmartFactor(unit2));
365  smartFactor3->add(measurements_cam3, views);
366 
367  SmartFactor::shared_ptr smartFactor4(new SmartFactor(unit2));
368  smartFactor4->add(measurements_cam4, views);
369 
370  SmartFactor::shared_ptr smartFactor5(new SmartFactor(unit2));
371  smartFactor5->add(measurements_cam5, views);
372 
373  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6 + 5, 1e-5);
374 
375  NonlinearFactorGraph graph;
376  graph.push_back(smartFactor1);
377  graph.push_back(smartFactor2);
378  graph.push_back(smartFactor3);
379  graph.push_back(smartFactor4);
380  graph.push_back(smartFactor5);
381  graph.addPrior(c1, cam1, noisePrior);
382  graph.addPrior(c2, cam2, noisePrior);
383 
384  Values values;
385  values.insert(c1, cam1);
386  values.insert(c2, cam2);
387  values.insert(c3, perturbCameraPoseAndCalibration(cam3));
388  if (isDebugTest)
389  values.at<Camera>(c3).print("Smart: Pose3 before optimization: ");
390 
391  LevenbergMarquardtParams lmParams;
392  lmParams.relativeErrorTol = 1e-8;
393  lmParams.absoluteErrorTol = 0;
394  lmParams.maxIterations = 20;
395  if (isDebugTest)
396  lmParams.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
397  if (isDebugTest)
398  lmParams.verbosity = NonlinearOptimizerParams::ERROR;
399 
400  Values result;
401  LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
402  result = optimizer.optimize();
403 
404  // GaussianFactorGraph::shared_ptr GFG = graph.linearize(values);
405  // VectorValues delta = GFG->optimize();
406 
407  if (isDebugTest)
408  result.at<Camera>(c3).print("Smart: Pose3 after optimization: ");
409  EXPECT(assert_equal(cam1, result.at<Camera>(c1)));
410  EXPECT(assert_equal(cam2, result.at<Camera>(c2)));
411  EXPECT(assert_equal(result.at<Camera>(c3).pose(), cam3.pose(), 1e-1));
412  EXPECT(
413  assert_equal(result.at<Camera>(c3).calibration(), cam3.calibration(), 20));
414  if (isDebugTest)
415  tictoc_print_();
416 }
417 
418 /* *************************************************************************/
419 TEST(SmartProjectionFactor, Cal3Bundler ) {
420 
421  using namespace bundler;
422 
423  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3,
424  measurements_cam4, measurements_cam5;
425 
426  // 1. Project three landmarks into three cameras and triangulate
427  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
428  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
429  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
430  projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4);
431  projectToMultipleCameras(cam1, cam2, cam3, landmark5, measurements_cam5);
432 
433  KeyVector views {c1, c2, c3};
434 
435  SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2));
436  smartFactor1->add(measurements_cam1, views);
437 
438  SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2));
439  smartFactor2->add(measurements_cam2, views);
440 
441  SmartFactor::shared_ptr smartFactor3(new SmartFactor(unit2));
442  smartFactor3->add(measurements_cam3, views);
443 
444  SmartFactor::shared_ptr smartFactor4(new SmartFactor(unit2));
445  smartFactor4->add(measurements_cam4, views);
446 
447  SmartFactor::shared_ptr smartFactor5(new SmartFactor(unit2));
448  smartFactor5->add(measurements_cam5, views);
449 
450  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(9, 1e-6);
451 
452  NonlinearFactorGraph graph;
453  graph.push_back(smartFactor1);
454  graph.push_back(smartFactor2);
455  graph.push_back(smartFactor3);
456  graph.addPrior(c1, cam1, noisePrior);
457  graph.addPrior(c2, cam2, noisePrior);
458 
459  Values values;
460  values.insert(c1, cam1);
461  values.insert(c2, cam2);
462  // initialize third pose with some noise, we expect it to move back to original pose3
463  values.insert(c3, perturbCameraPose(cam3));
464  if (isDebugTest)
465  values.at<Camera>(c3).print("Smart: Pose3 before optimization: ");
466 
467  LevenbergMarquardtParams lmParams;
468  lmParams.relativeErrorTol = 1e-8;
469  lmParams.absoluteErrorTol = 0;
470  lmParams.maxIterations = 20;
471  if (isDebugTest)
472  lmParams.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
473  if (isDebugTest)
474  lmParams.verbosity = NonlinearOptimizerParams::ERROR;
475 
476  Values result;
477  LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
478  result = optimizer.optimize();
479 
480  if (isDebugTest)
481  result.at<Camera>(c3).print("Smart: Pose3 after optimization: ");
482  EXPECT(assert_equal(cam1, result.at<Camera>(c1), 1e-4));
483  EXPECT(assert_equal(cam2, result.at<Camera>(c2), 1e-4));
484  EXPECT(assert_equal(result.at<Camera>(c3).pose(), cam3.pose(), 1e-1));
485  EXPECT(
486  assert_equal(result.at<Camera>(c3).calibration(), cam3.calibration(), 1));
487  if (isDebugTest)
488  tictoc_print_();
489 }
490 
491 /* *************************************************************************/
492 TEST(SmartProjectionFactor, Cal3Bundler2 ) {
493 
494  using namespace bundler;
495 
496  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3,
497  measurements_cam4, measurements_cam5;
498 
499  // 1. Project three landmarks into three cameras and triangulate
500  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
501  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
502  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
503  projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4);
504  projectToMultipleCameras(cam1, cam2, cam3, landmark5, measurements_cam5);
505 
506  KeyVector views {c1, c2, c3};
507 
508  SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2));
509  smartFactor1->add(measurements_cam1, views);
510 
511  SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2));
512  smartFactor2->add(measurements_cam2, views);
513 
514  SmartFactor::shared_ptr smartFactor3(new SmartFactor(unit2));
515  smartFactor3->add(measurements_cam3, views);
516 
517  SmartFactor::shared_ptr smartFactor4(new SmartFactor(unit2));
518  smartFactor4->add(measurements_cam4, views);
519 
520  SmartFactor::shared_ptr smartFactor5(new SmartFactor(unit2));
521  smartFactor5->add(measurements_cam5, views);
522 
523  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(9, 1e-6);
524 
525  NonlinearFactorGraph graph;
526  graph.push_back(smartFactor1);
527  graph.push_back(smartFactor2);
528  graph.push_back(smartFactor3);
529  graph.addPrior(c1, cam1, noisePrior);
530  graph.addPrior(c2, cam2, noisePrior);
531 
532  Values values;
533  values.insert(c1, cam1);
534  values.insert(c2, cam2);
535  // initialize third pose with some noise, we expect it to move back to original pose3
536  values.insert(c3, perturbCameraPoseAndCalibration(cam3));
537  if (isDebugTest)
538  values.at<Camera>(c3).print("Smart: Pose3 before optimization: ");
539 
540  LevenbergMarquardtParams lmParams;
541  lmParams.relativeErrorTol = 1e-8;
542  lmParams.absoluteErrorTol = 0;
543  lmParams.maxIterations = 20;
544  if (isDebugTest)
545  lmParams.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
546  if (isDebugTest)
547  lmParams.verbosity = NonlinearOptimizerParams::ERROR;
548 
549  Values result;
550  LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
551  result = optimizer.optimize();
552 
553  if (isDebugTest)
554  result.at<Camera>(c3).print("Smart: Pose3 after optimization: ");
555  EXPECT(assert_equal(cam1, result.at<Camera>(c1), 1e-4));
556  EXPECT(assert_equal(cam2, result.at<Camera>(c2), 1e-4));
557  EXPECT(assert_equal(result.at<Camera>(c3).pose(), cam3.pose(), 1e-1));
558  EXPECT(
559  assert_equal(result.at<Camera>(c3).calibration(), cam3.calibration(), 2));
560  if (isDebugTest)
561  tictoc_print_();
562 }
563 
564 /* *************************************************************************/
565 TEST(SmartProjectionFactor, noiselessBundler ) {
566 
567  using namespace bundler;
568  Values values;
569  values.insert(c1, level_camera);
570  values.insert(c2, level_camera_right);
571 
572  SmartFactor::shared_ptr factor1(new SmartFactor(unit2));
573  factor1->add(level_uv, c1);
574  factor1->add(level_uv_right, c2);
575 
576  double actualError = factor1->error(values);
577 
578  double expectedError = 0.0;
579  DOUBLES_EQUAL(expectedError, actualError, 1e-3);
580 
581  Point3 oldPoint(0,0,0); // this takes the point stored in the factor (we are not interested in this)
582  if (factor1->point())
583  oldPoint = *(factor1->point());
584 
585  Point3 expectedPoint(0,0,0);
586  if (factor1->point(values))
587  expectedPoint = *(factor1->point(values));
588 
589  EXPECT(assert_equal(expectedPoint, landmark1, 1e-3));
590 }
591 
592 /* *************************************************************************/
593 TEST(SmartProjectionFactor, comparisonGeneralSfMFactor ) {
594 
595  using namespace bundler;
596  Values values;
597  values.insert(c1, level_camera);
598  values.insert(c2, level_camera_right);
599 
600  NonlinearFactorGraph smartGraph;
601  SmartFactor::shared_ptr factor1(new SmartFactor(unit2));
602  factor1->add(level_uv, c1);
603  factor1->add(level_uv_right, c2);
604  smartGraph.push_back(factor1);
605  double expectedError = factor1->error(values);
606  double expectedErrorGraph = smartGraph.error(values);
607  Point3 expectedPoint(0,0,0);
608  if (factor1->point())
609  expectedPoint = *(factor1->point());
610 
611  // COMMENTS:
612  // 1) triangulation introduces small errors, then for a fair comparison we use expectedPoint as
613  // value in the generalGrap
614  NonlinearFactorGraph generalGraph;
615  SFMFactor sfm1(level_uv, unit2, c1, l1);
616  SFMFactor sfm2(level_uv_right, unit2, c2, l1);
617  generalGraph.push_back(sfm1);
618  generalGraph.push_back(sfm2);
619  values.insert(l1, expectedPoint); // note: we get rid of possible errors in the triangulation
620  Vector e1 = sfm1.evaluateError(values.at<Camera>(c1), values.at<Point3>(l1));
621  Vector e2 = sfm2.evaluateError(values.at<Camera>(c2), values.at<Point3>(l1));
622  double actualError = 0.5 * (e1.squaredNorm() + e2.squaredNorm());
623  double actualErrorGraph = generalGraph.error(values);
624 
625  DOUBLES_EQUAL(expectedErrorGraph, actualErrorGraph, 1e-7);
626  DOUBLES_EQUAL(expectedErrorGraph, expectedError, 1e-7);
627  DOUBLES_EQUAL(actualErrorGraph, actualError, 1e-7);
628  DOUBLES_EQUAL(expectedError, actualError, 1e-7);
629 }
630 
631 /* *************************************************************************/
632 TEST(SmartProjectionFactor, comparisonGeneralSfMFactor1 ) {
633 
634  using namespace bundler;
635  Values values;
636  values.insert(c1, level_camera);
637  values.insert(c2, level_camera_right);
638 
639  NonlinearFactorGraph smartGraph;
640  SmartFactor::shared_ptr factor1(new SmartFactor(unit2));
641  factor1->add(level_uv, c1);
642  factor1->add(level_uv_right, c2);
643  smartGraph.push_back(factor1);
644  Matrix expectedHessian = smartGraph.linearize(values)->hessian().first;
645  Vector expectedInfoVector = smartGraph.linearize(values)->hessian().second;
646  Point3 expectedPoint(0,0,0);
647  if (factor1->point())
648  expectedPoint = *(factor1->point());
649 
650  // COMMENTS:
651  // 1) triangulation introduces small errors, then for a fair comparison we use expectedPoint as
652  // value in the generalGrap
653  NonlinearFactorGraph generalGraph;
654  SFMFactor sfm1(level_uv, unit2, c1, l1);
655  SFMFactor sfm2(level_uv_right, unit2, c2, l1);
656  generalGraph.push_back(sfm1);
657  generalGraph.push_back(sfm2);
658  values.insert(l1, expectedPoint); // note: we get rid of possible errors in the triangulation
659  Matrix actualFullHessian = generalGraph.linearize(values)->hessian().first;
660  Matrix actualFullInfoVector = generalGraph.linearize(values)->hessian().second;
661  Matrix actualHessian = actualFullHessian.block(0, 0, 18, 18)
662  - actualFullHessian.block(0, 18, 18, 3)
663  * (actualFullHessian.block(18, 18, 3, 3)).inverse()
664  * actualFullHessian.block(18, 0, 3, 18);
665  Vector actualInfoVector = actualFullInfoVector.block(0, 0, 18, 1)
666  - actualFullHessian.block(0, 18, 18, 3)
667  * (actualFullHessian.block(18, 18, 3, 3)).inverse()
668  * actualFullInfoVector.block(18, 0, 3, 1);
669 
670  EXPECT(assert_equal(expectedHessian, actualHessian, 1e-7));
671  EXPECT(assert_equal(expectedInfoVector, actualInfoVector, 1e-7));
672 }
673 
674 /* *************************************************************************/
675 // Have to think about how to compare multiplyHessianAdd in generalSfMFactor and smartFactors
676 //TEST(SmartProjectionFactor, comparisonGeneralSfMFactor2 ){
677 //
678 // Values values;
679 // values.insert(c1, level_camera);
680 // values.insert(c2, level_camera_right);
681 //
682 // NonlinearFactorGraph smartGraph;
683 // SmartFactor::shared_ptr factor1(new SmartFactor());
684 // factor1->add(level_uv, c1, unit2);
685 // factor1->add(level_uv_right, c2, unit2);
686 // smartGraph.push_back(factor1);
687 // GaussianFactorGraph::shared_ptr gfgSmart = smartGraph.linearize(values);
688 //
689 // Point3 expectedPoint(0,0,0);
690 // if(factor1->point())
691 // expectedPoint = *(factor1->point());
692 //
693 // // COMMENTS:
694 // // 1) triangulation introduces small errors, then for a fair comparison we use expectedPoint as
695 // // value in the generalGrap
696 // NonlinearFactorGraph generalGraph;
697 // SFMFactor sfm1(level_uv, unit2, c1, l1);
698 // SFMFactor sfm2(level_uv_right, unit2, c2, l1);
699 // generalGraph.push_back(sfm1);
700 // generalGraph.push_back(sfm2);
701 // values.insert(l1, expectedPoint); // note: we get rid of possible errors in the triangulation
702 // GaussianFactorGraph::shared_ptr gfg = generalGraph.linearize(values);
703 //
704 // double alpha = 1.0;
705 //
706 // VectorValues yExpected, yActual, ytmp;
707 // VectorValues xtmp = map_list_of
708 // (c1, (Vec(9) << 0,0,0,0,0,0,0,0,0))
709 // (c2, (Vec(9) << 0,0,0,0,0,0,0,0,0))
710 // (l1, (Vec(3) << 5.5, 0.5, 1.2));
711 // gfg ->multiplyHessianAdd(alpha, xtmp, ytmp);
712 //
713 // VectorValues x = map_list_of
714 // (c1, (Vec(9) << 1,2,3,4,5,6,7,8,9))
715 // (c2, (Vec(9) << 11,12,13,14,15,16,17,18,19))
716 // (l1, (Vec(3) << 5.5, 0.5, 1.2));
717 //
718 // gfgSmart->multiplyHessianAdd(alpha, ytmp + x, yActual);
719 // gfg ->multiplyHessianAdd(alpha, x, yExpected);
720 //
721 // EXPECT(assert_equal(yActual,yExpected, 1e-7));
722 //}
723 /* *************************************************************************/
724 TEST(SmartProjectionFactor, computeImplicitJacobian ) {
725 
726  using namespace bundler;
727  Values values;
728  values.insert(c1, level_camera);
729  values.insert(c2, level_camera_right);
730 
731  SmartFactor::shared_ptr factor1(new SmartFactor(unit2));
732  factor1->add(level_uv, c1);
733  factor1->add(level_uv_right, c2);
734  Matrix expectedE;
735  Vector expectedb;
736 
737  CameraSet<Camera> cameras;
738  cameras.push_back(level_camera);
739  cameras.push_back(level_camera_right);
740 
741  factor1->error(values); // this is important to have a triangulation of the point
742  Point3 point(0,0,0);
743  if (factor1->point())
744  point = *(factor1->point());
745  SmartFactor::FBlocks Fs;
746  factor1->computeJacobians(Fs, expectedE, expectedb, cameras, point);
747 
748  NonlinearFactorGraph generalGraph;
749  SFMFactor sfm1(level_uv, unit2, c1, l1);
750  SFMFactor sfm2(level_uv_right, unit2, c2, l1);
751  generalGraph.push_back(sfm1);
752  generalGraph.push_back(sfm2);
753  values.insert(l1, point); // note: we get rid of possible errors in the triangulation
754  Matrix actualFullHessian = generalGraph.linearize(values)->hessian().first;
755  Matrix actualVinv = (actualFullHessian.block(18, 18, 3, 3)).inverse();
756 
757  Matrix3 expectedVinv = factor1->PointCov(expectedE);
758  EXPECT(assert_equal(expectedVinv, actualVinv, 1e-7));
759 }
760 
761 /* *************************************************************************/
762 TEST(SmartProjectionFactor, implicitJacobianFactor ) {
763 
764  using namespace bundler;
765 
766  Values values;
767  values.insert(c1, level_camera);
768  values.insert(c2, level_camera_right);
769  double rankTol = 1;
770  bool useEPI = false;
771 
772  // Hessian version
773  SmartProjectionParams params;
774  params.setRankTolerance(rankTol);
775  params.setLinearizationMode(gtsam::HESSIAN);
776  params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY);
777  params.setEnableEPI(useEPI);
778 
779  SmartFactor::shared_ptr explicitFactor(
780  new SmartFactor(unit2, params));
781  explicitFactor->add(level_uv, c1);
782  explicitFactor->add(level_uv_right, c2);
783 
784  GaussianFactor::shared_ptr gaussianHessianFactor = explicitFactor->linearize(
785  values);
786  HessianFactor& hessianFactor =
787  dynamic_cast<HessianFactor&>(*gaussianHessianFactor);
788 
789  // Implicit Schur version
790  params.setLinearizationMode(gtsam::IMPLICIT_SCHUR);
791  SmartFactor::shared_ptr implicitFactor(
792  new SmartFactor(unit2, params));
793  implicitFactor->add(level_uv, c1);
794  implicitFactor->add(level_uv_right, c2);
795  GaussianFactor::shared_ptr gaussianImplicitSchurFactor =
796  implicitFactor->linearize(values);
797  CHECK(gaussianImplicitSchurFactor);
798  typedef RegularImplicitSchurFactor<Camera> Implicit9;
799  Implicit9& implicitSchurFactor =
800  dynamic_cast<Implicit9&>(*gaussianImplicitSchurFactor);
801 
802  VectorValues x{
803  {c1, (Vector(9) << 1, 2, 3, 4, 5, 6, 7, 8, 9).finished()},
804  {c2, (Vector(9) << 11, 12, 13, 14, 15, 16, 17, 18, 19).finished()}};
805 
806  VectorValues yExpected, yActual;
807  double alpha = 1.0;
808  hessianFactor.multiplyHessianAdd(alpha, x, yActual);
809  implicitSchurFactor.multiplyHessianAdd(alpha, x, yExpected);
810  EXPECT(assert_equal(yActual, yExpected, 1e-7));
811 }
812 
813 /* ************************************************************************* */
814 int main() {
815  TestResult tr;
816  return TestRegistry::runAllTests(tr);
817 }
818 /* ************************************************************************* */
819 
test_docs.d
d
Definition: test_docs.py:28
simple_graph::factor1
auto factor1
Definition: testJacobianFactor.cpp:187
gtsam::GeneralSFMFactor::evaluateError
Vector evaluateError(const CAMERA &camera, const LANDMARK &point, OptionalMatrixType H1, OptionalMatrixType H2) const override
Definition: GeneralSFMFactor.h:126
CHECK
#define CHECK(condition)
Definition: Test.h:108
simple_graph::factor2
auto factor2
Definition: testJacobianFactor.cpp:194
gtsam::NonlinearOptimizer::optimize
virtual const Values & optimize()
Definition: NonlinearOptimizer.h:98
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam_unstable.tests.test_ProjectionFactorRollingShutter.point
point
Definition: test_ProjectionFactorRollingShutter.py:25
gtsam::HessianFactor::multiplyHessianAdd
void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) const override
Definition: HessianFactor.cpp:385
gtsam::Values::at
const ValueType at(Key j) const
Definition: Values-inl.h:204
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
DOUBLES_EQUAL
#define DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:141
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
gtsam::FactorGraph::push_back
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:190
vanilla::level_camera
static const Camera level_camera(level_pose, K2)
gtsam::SmartProjectionParams::setEnableEPI
void setEnableEPI(bool enableEPI)
Definition: SmartFactorParams.h:109
gtsam::CameraSet::reprojectionError
Vector reprojectionError(const POINT &point, const ZVector &measured, FBlocks *Fs=nullptr, Matrix *E=nullptr) const
Calculate vector [project2(point)-z] of re-projection errors.
Definition: CameraSet.h:146
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
M_PI
#define M_PI
Definition: main.h:106
gtsam::SmartFactorBase< PinholePose< CALIBRATION > >::FBlocks
std::vector< MatrixZD, Eigen::aligned_allocator< MatrixZD > > FBlocks
Definition: SmartFactorBase.h:64
vanilla::cam1
static const Camera cam1(level_pose, K2)
l2
gtsam::Key l2
Definition: testLinearContainerFactor.cpp:24
leaf::values
leaf::MyValues values
initial
Values initial
Definition: OdometryOptimize.cpp:2
gtsam::NonlinearOptimizerParams::verbosity
Verbosity verbosity
The printing verbosity during optimization (default SILENT)
Definition: NonlinearOptimizerParams.h:46
perturbCameraPose
CAMERA perturbCameraPose(const CAMERA &camera)
Definition: smartFactorScenarios.h:153
Eigen::internal::print
EIGEN_STRONG_INLINE Packet4f print(const Packet4f &a)
Definition: NEON/PacketMath.h:3115
vanilla::level_camera_right
static const Camera level_camera_right(pose_right, K2)
gtsam::PinholeCamera::calibration
const Calibration & calibration() const override
return calibration
Definition: PinholeCamera.h:178
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
gtsam::CameraSet
A set of cameras, all with their own calibration.
Definition: CameraSet.h:36
landmark1
Point2 landmark1(5.0, 1.5)
gtsam::NonlinearOptimizerParams::absoluteErrorTol
double absoluteErrorTol
The maximum absolute error decrease to stop iterating (default 1e-5)
Definition: NonlinearOptimizerParams.h:44
EXPECT_DOUBLES_EQUAL
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:161
inverse
EIGEN_DEVICE_FUNC const InverseReturnType inverse() const
Definition: ArrayCwiseUnaryOps.h:411
vanilla::params
static const SmartProjectionParams params
Definition: smartFactorScenarios.h:69
gtsam::NonlinearFactorGraph::linearize
std::shared_ptr< GaussianFactorGraph > linearize(const Values &linearizationPoint) const
Linearize a nonlinear factor graph.
Definition: NonlinearFactorGraph.cpp:239
gtsam::NonlinearFactorGraph::addPrior
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
Definition: NonlinearFactorGraph.h:199
gtsam::PinholeCamera::pose
const Pose3 & pose() const
return pose
Definition: PinholeCamera.h:164
l3
Point3 l3(2, 2, 0)
gtsam::NonlinearOptimizerParams::relativeErrorTol
double relativeErrorTol
The maximum relative error decrease to stop iterating (default 1e-5)
Definition: NonlinearOptimizerParams.h:43
gtsam::SmartProjectionParams::setRankTolerance
void setRankTolerance(double rankTol)
Definition: SmartFactorParams.h:106
SmartProjectionFactor.h
Smart factor on cameras (pose + calibration)
vanilla::cam3
static const Camera cam3(pose_above, K2)
rankTol
static const double rankTol
Definition: testSmartProjectionRigFactor.cpp:35
gtsam::PinholeBaseK::project
Point2 project(const Point3 &pw, OptionalJacobian< 2, 6 > Dpose={}, OptionalJacobian< 2, 3 > Dpoint={}, OptionalJacobian< 2, DimK > Dcal={}) const
project a 3D point from world coordinates into the image
Definition: PinholePose.h:112
landmark2
Point2 landmark2(7.0, 1.5)
vanilla::K2
static const Cal3_S2 K2(1500, 1200, 0, w, h)
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
result
Values result
Definition: OdometryOptimize.cpp:8
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
level_pose
Pose3 level_pose
Definition: testInvDepthCamera3.cpp:21
measurement1
static Point2 measurement1(323.0, 240.0)
perturbCameraPoseAndCalibration
PinholeCamera< CALIBRATION > perturbCameraPoseAndCalibration(const PinholeCamera< CALIBRATION > &camera)
Definition: testSmartProjectionFactor.cpp:37
projectToMultipleCameras
void projectToMultipleCameras(const CAMERA &cam1, const CAMERA &cam2, const CAMERA &cam3, Point3 landmark, typename CAMERA::MeasurementVector &measurements_cam)
Definition: smartFactorScenarios.h:162
lmParams
LevenbergMarquardtParams lmParams
Definition: testSmartProjectionRigFactor.cpp:55
gtsam::SfmTrack2d::measurements
std::vector< SfmMeasurement > measurements
The 2D image projections (id,(u,v))
Definition: SfmTrack.h:44
alpha
RealScalar alpha
Definition: level1_cplx_impl.h:147
LevenbergMarquardtOptimizer.h
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
gtsam::tictoc_print_
void tictoc_print_()
Definition: timing.h:268
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::NonlinearFactorGraph::error
double error(const Values &values) const
Definition: NonlinearFactorGraph.cpp:170
gtsam::LieGroup::compose
Class compose(const Class &g) const
Definition: Lie.h:48
vanilla::SmartFactor
SmartProjectionFactor< Camera > SmartFactor
Definition: smartFactorScenarios.h:58
l1
gtsam::Key l1
Definition: testLinearContainerFactor.cpp:24
gtsam::SharedDiagonal
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:743
gtsam::Cal3Bundler
Calibration used by Bundler.
Definition: Cal3Bundler.h:32
gtsam::SmartProjectionParams::setDegeneracyMode
void setDegeneracyMode(DegeneracyMode degMode)
Definition: SmartFactorParams.h:100
gtsam::SmartFactorBase::add
void add(const Z &measured, const Key &key)
Definition: SmartFactorBase.h:127
gtsam::Point2Vector
std::vector< Point2, Eigen::aligned_allocator< Point2 > > Point2Vector
Definition: Point2.h:49
vanilla::cam2
static const Camera cam2(pose_right, K2)
vanilla::level_uv_right
static const Point2 level_uv_right
Definition: smartFactorScenarios.h:64
gtsam::HessianFactor
A Gaussian factor using the canonical parameters (information form)
Definition: HessianFactor.h:100
gtsam::Values::insert
void insert(Key j, const Value &val)
Definition: Values.cpp:155
gtsam::SmartProjectionParams::setLinearizationMode
void setLinearizationMode(LinearizationMode linMode)
Definition: SmartFactorParams.h:97
TEST
TEST(SmartProjectionFactor, perturbCameraPose)
Definition: testSmartProjectionFactor.cpp:51
point1
const Point3 point1(3.0, 4.0, 2.0)
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
Camera
Definition: camera.h:36
gtsam::LevenbergMarquardtParams::verbosityLM
VerbosityLM verbosityLM
The verbosity level for Levenberg-Marquardt (default: SILENT), see also NonlinearOptimizerParams::ver...
Definition: LevenbergMarquardtParams.h:53
camera
static const CalibratedCamera camera(kDefaultPose)
gtsam::SmartProjectionPoseFactor::shared_ptr
std::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
Definition: SmartProjectionPoseFactor.h:59
x
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
Definition: gnuplot_common_settings.hh:12
ceres::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: gtsam/3rdparty/ceres/eigen.h:38
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
vanilla::level_uv
static const Point2 level_uv
Definition: smartFactorScenarios.h:63
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
gtsam::NonlinearOptimizerParams::maxIterations
size_t maxIterations
The maximum iterations to stop iterating (default 100)
Definition: NonlinearOptimizerParams.h:42

gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:39:33