testSmartProjectionRigFactor.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 
27 
28 #include <iostream>
29 
30 #include "smartFactorScenarios.h"
31 #define DISABLE_TIMING
32 
33 using namespace std::placeholders;
34 
35 static const double rankTol = 1.0;
36 // Create a noise model for the pixel error
37 static const double sigma = 0.1;
38 static SharedIsotropic model(noiseModel::Isotropic::Sigma(2, sigma));
39 
40 // Convenience for named keys
43 
44 // tests data
45 static Symbol x1('X', 1);
46 static Symbol x2('X', 2);
47 static Symbol x3('X', 3);
48 
49 Key cameraId1 = 0; // first camera
52 
53 static Point2 measurement1(323.0, 240.0);
54 
56 
57 /* ************************************************************************* */
58 // default Cal3_S2 poses with rolling shutter effect
59 namespace vanillaRig {
60 using namespace vanillaPose;
63  gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors
64 } // namespace vanillaRig
65 
66 /* ************************************************************************* */
68  using namespace vanillaRig;
69  std::shared_ptr<Cameras> cameraRig(new Cameras());
70  cameraRig->push_back(Camera(Pose3(), sharedK));
72  new SmartRigFactor(model, cameraRig, params));
73 }
74 
75 /* ************************************************************************* */
77  using namespace vanillaRig;
78  std::shared_ptr<Cameras> cameraRig(new Cameras());
79  SmartProjectionParams params2(
81  gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors
82  params2.setRankTolerance(rankTol);
83  SmartRigFactor factor1(model, cameraRig, params2);
84 }
85 
86 /* ************************************************************************* */
88  using namespace vanillaRig;
89  std::shared_ptr<Cameras> cameraRig(new Cameras());
90  cameraRig->push_back(Camera(Pose3(), sharedK));
92  new SmartRigFactor(model, cameraRig, params));
94 }
95 
96 /* ************************************************************************* */
98  using namespace vanillaRig;
99  std::shared_ptr<Cameras> cameraRig(new Cameras());
100  cameraRig->push_back(Camera(Pose3(), sharedK));
101  SmartProjectionParams params2(
103  gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors
104  params2.setRankTolerance(rankTol);
105  SmartRigFactor factor1(model, cameraRig, params2);
107 }
108 
109 /* ************************************************************************* */
111  using namespace vanillaRig;
112  std::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
113  cameraRig->push_back(Camera(Pose3(), sharedK));
114 
116  new SmartRigFactor(model, cameraRig, params));
118 
120  new SmartRigFactor(model, cameraRig, params));
122 
124 
126  new SmartRigFactor(model, cameraRig, params));
127  factor3->add(measurement1, x1); // now use default camera ID
128 
130 }
131 
132 /* *************************************************************************/
134  using namespace vanillaRig;
135 
136  // Project two landmarks into two cameras
139 
140  std::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
141  cameraRig->push_back(Camera(Pose3(), sharedK));
142 
143  SmartRigFactor factor(model, cameraRig, params);
144  factor.add(level_uv, x1); // both taken from the same camera
145  factor.add(level_uv_right, x2);
146 
147  Values values; // it's a pose factor, hence these are poses
148  values.insert(x1, cam1.pose());
149  values.insert(x2, cam2.pose());
150 
151  double actualError = factor.error(values);
152  double expectedError = 0.0;
153  EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7);
154 
156  double actualError2 = factor.totalReprojectionError(cameras);
157  EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7);
158 
159  // Calculate expected derivative for point (easiest to check)
160  std::function<Vector(Point3)> f = //
161  std::bind(&SmartRigFactor::whitenedError<Point3>, factor, cameras,
162  std::placeholders::_1);
163 
164  // Calculate using computeEP
165  Matrix actualE;
166  factor.triangulateAndComputeE(actualE, values);
167 
168  // get point
169  auto point = factor.point();
170  CHECK(point);
171 
172  // calculate numerical derivative with triangulated point
173  Matrix expectedE = sigma * numericalDerivative11<Vector, Point3>(f, *point);
174  EXPECT(assert_equal(expectedE, actualE, 1e-7));
175 
176  // Calculate using reprojectionError
178  Matrix E;
179  Vector actualErrors = factor.unwhitenedError(cameras, *point, F, E);
180  EXPECT(assert_equal(expectedE, E, 1e-7));
181 
182  EXPECT(assert_equal(Z_4x1, actualErrors, 1e-7));
183 
184  // Calculate using computeJacobians
185  Vector b;
187  factor.computeJacobians(Fs, E, b, cameras, *point);
188  double actualError3 = b.squaredNorm();
189  EXPECT(assert_equal(expectedE, E, 1e-7));
190  EXPECT_DOUBLES_EQUAL(expectedError, actualError3, 1e-6);
191 }
192 
193 /* *************************************************************************/
195  using namespace vanillaRig;
196 
197  std::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
198  cameraRig->push_back(Camera(Pose3(), sharedK));
199 
200  // Project two landmarks into two cameras
201  Point2 pixelError(0.2, 0.2);
202  Point2 level_uv = level_camera.project(landmark1) + pixelError;
204 
205  Values values;
206  values.insert(x1, cam1.pose());
207  Pose3 noise_pose =
208  Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.5, 0.1, 0.3));
209  values.insert(x2, pose_right.compose(noise_pose));
210 
212  new SmartRigFactor(model, cameraRig, params));
213  factor->add(level_uv, x1, cameraId1);
215 
216  double actualError1 = factor->error(values);
217 
218  // create other factor by passing multiple measurements
220  new SmartRigFactor(model, cameraRig, params));
221 
223  measurements.push_back(level_uv);
224  measurements.push_back(level_uv_right);
225 
226  KeyVector views{x1, x2};
227  FastVector<size_t> cameraIds{0, 0};
228 
229  factor2->add(measurements, views, cameraIds);
230  double actualError2 = factor2->error(values);
231  DOUBLES_EQUAL(actualError1, actualError2, 1e-7);
232 }
233 
234 /* *************************************************************************/
235 TEST(SmartProjectionRigFactor, smartFactorWithSensorBodyTransform) {
236  using namespace vanillaRig;
237 
238  // create arbitrary body_T_sensor (transforms from sensor to body)
239  Pose3 body_T_sensor =
240  Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(1, 1, 1));
241  std::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
242  cameraRig->push_back(Camera(body_T_sensor, sharedK));
243 
244  // These are the poses we want to estimate, from camera measurements
245  const Pose3 sensor_T_body = body_T_sensor.inverse();
246  Pose3 wTb1 = cam1.pose() * sensor_T_body;
247  Pose3 wTb2 = cam2.pose() * sensor_T_body;
248  Pose3 wTb3 = cam3.pose() * sensor_T_body;
249 
250  // three landmarks ~5 meters infront of camera
251  Point3 landmark1(5, 0.5, 1.2), landmark2(5, -0.5, 1.2), landmark3(5, 0, 3.0);
252 
253  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
254 
255  // Project three landmarks into three cameras
256  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
257  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
258  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
259 
260  // Create smart factors
261  KeyVector views{x1, x2, x3};
262  FastVector<size_t> cameraIds{0, 0, 0};
263 
267  params.setEnableEPI(false);
268 
269  SmartRigFactor smartFactor1(model, cameraRig, params);
270  smartFactor1.add(
271  measurements_cam1,
272  views); // use default CameraIds since we have a single camera
273 
274  SmartRigFactor smartFactor2(model, cameraRig, params);
275  smartFactor2.add(measurements_cam2, views);
276 
277  SmartRigFactor smartFactor3(model, cameraRig, params);
278  smartFactor3.add(measurements_cam3, views);
279 
280  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
281 
282  // Put all factors in factor graph, adding priors
284  graph.push_back(smartFactor1);
285  graph.push_back(smartFactor2);
286  graph.push_back(smartFactor3);
287  graph.addPrior(x1, wTb1, noisePrior);
288  graph.addPrior(x2, wTb2, noisePrior);
289 
290  // Check errors at ground truth poses
291  Values gtValues;
292  gtValues.insert(x1, wTb1);
293  gtValues.insert(x2, wTb2);
294  gtValues.insert(x3, wTb3);
295  double actualError = graph.error(gtValues);
296  double expectedError = 0.0;
297  DOUBLES_EQUAL(expectedError, actualError, 1e-7)
298 
299  Pose3 noise_pose =
300  Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1));
301  Values values;
302  values.insert(x1, wTb1);
303  values.insert(x2, wTb2);
304  // initialize third pose with some noise, we expect it to move back to
305  // original pose3
306  values.insert(x3, wTb3 * noise_pose);
307 
309  Values result;
311  result = optimizer.optimize();
312  EXPECT(assert_equal(wTb3, result.at<Pose3>(x3)));
313 }
314 
315 /* *************************************************************************/
316 TEST(SmartProjectionRigFactor, smartFactorWithMultipleCameras) {
317  using namespace vanillaRig;
318 
319  // create arbitrary body_T_sensor (transforms from sensor to body)
320  Pose3 body_T_sensor1 =
321  Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(1, 1, 1));
322  Pose3 body_T_sensor2 =
323  Pose3(Rot3::Ypr(-M_PI / 5, 0., -M_PI / 2), Point3(0, 0, 1));
324  Pose3 body_T_sensor3 = Pose3();
325 
326  std::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
327  cameraRig->push_back(Camera(body_T_sensor1, sharedK));
328  cameraRig->push_back(Camera(body_T_sensor2, sharedK));
329  cameraRig->push_back(Camera(body_T_sensor3, sharedK));
330 
331  // These are the poses we want to estimate, from camera measurements
332  const Pose3 sensor_T_body1 = body_T_sensor1.inverse();
333  const Pose3 sensor_T_body2 = body_T_sensor2.inverse();
334  const Pose3 sensor_T_body3 = body_T_sensor3.inverse();
335  Pose3 wTb1 = cam1.pose() * sensor_T_body1;
336  Pose3 wTb2 = cam2.pose() * sensor_T_body2;
337  Pose3 wTb3 = cam3.pose() * sensor_T_body3;
338 
339  // three landmarks ~5 meters infront of camera
340  Point3 landmark1(5, 0.5, 1.2), landmark2(5, -0.5, 1.2), landmark3(5, 0, 3.0);
341 
342  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
343 
344  // Project three landmarks into three cameras
345  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
346  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
347  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
348 
349  // Create smart factors
350  KeyVector views{x1, x2, x3};
351  FastVector<size_t> cameraIds{0, 1, 2};
352 
356  params.setEnableEPI(false);
357 
358  SmartRigFactor smartFactor1(model, cameraRig, params);
359  smartFactor1.add(measurements_cam1, views, cameraIds);
360 
361  SmartRigFactor smartFactor2(model, cameraRig, params);
362  smartFactor2.add(measurements_cam2, views, cameraIds);
363 
364  SmartRigFactor smartFactor3(model, cameraRig, params);
365  smartFactor3.add(measurements_cam3, views, cameraIds);
366 
367  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
368 
369  // Put all factors in factor graph, adding priors
371  graph.push_back(smartFactor1);
372  graph.push_back(smartFactor2);
373  graph.push_back(smartFactor3);
374  graph.addPrior(x1, wTb1, noisePrior);
375  graph.addPrior(x2, wTb2, noisePrior);
376 
377  // Check errors at ground truth poses
378  Values gtValues;
379  gtValues.insert(x1, wTb1);
380  gtValues.insert(x2, wTb2);
381  gtValues.insert(x3, wTb3);
382  double actualError = graph.error(gtValues);
383  double expectedError = 0.0;
384  DOUBLES_EQUAL(expectedError, actualError, 1e-7)
385 
386  Pose3 noise_pose =
387  Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1));
388  Values values;
389  values.insert(x1, wTb1);
390  values.insert(x2, wTb2);
391  // initialize third pose with some noise, we expect it to move back to
392  // original pose3
393  values.insert(x3, wTb3 * noise_pose);
394 
396  Values result;
398  result = optimizer.optimize();
399  EXPECT(assert_equal(wTb3, result.at<Pose3>(x3)));
400 }
401 
402 /* *************************************************************************/
403 TEST(SmartProjectionRigFactor, 3poses_smart_projection_factor) {
404  using namespace vanillaPose2;
405  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
406 
407  std::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
408  cameraRig->push_back(Camera(Pose3(), sharedK2));
409 
410  // Project three landmarks into three cameras
411  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
412  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
413  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
414 
415  KeyVector views{x1, x2, x3};
416  FastVector<size_t> cameraIds{
417  0, 0, 0}; // 3 measurements from the same camera in the rig
418 
421  gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors
422 
423  SmartRigFactor::shared_ptr smartFactor1(
424  new SmartRigFactor(model, cameraRig, params));
425  smartFactor1->add(measurements_cam1, views, cameraIds);
426 
427  SmartRigFactor::shared_ptr smartFactor2(
428  new SmartRigFactor(model, cameraRig, params));
429  smartFactor2->add(measurements_cam2, views, cameraIds);
430 
431  SmartRigFactor::shared_ptr smartFactor3(
432  new SmartRigFactor(model, cameraRig, params));
433  smartFactor3->add(measurements_cam3, views, cameraIds);
434 
435  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
436 
438  graph.push_back(smartFactor1);
439  graph.push_back(smartFactor2);
440  graph.push_back(smartFactor3);
441  graph.addPrior(x1, cam1.pose(), noisePrior);
442  graph.addPrior(x2, cam2.pose(), noisePrior);
443 
444  Values groundTruth;
445  groundTruth.insert(x1, cam1.pose());
446  groundTruth.insert(x2, cam2.pose());
447  groundTruth.insert(x3, cam3.pose());
448  DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9);
449 
450  // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10),
451  // Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
452  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
453  Point3(0.1, 0.1, 0.1)); // smaller noise
454  Values values;
455  values.insert(x1, cam1.pose());
456  values.insert(x2, cam2.pose());
457  // initialize third pose with some noise, we expect it to move back to
458  // original pose_above
459  values.insert(x3, pose_above * noise_pose);
461  Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598,
462  -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598),
463  Point3(0.1, -0.1, 1.9)),
464  values.at<Pose3>(x3)));
465 
466  Values result;
468  result = optimizer.optimize();
469  EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-6));
470 }
471 
472 /* *************************************************************************/
474  using namespace vanillaRig;
475 
476  // Default cameras for simple derivatives
477  Rot3 R;
478  static Cal3_S2::shared_ptr sharedK(new Cal3_S2(100, 100, 0, 0, 0));
479  Camera cam1(Pose3(R, Point3(0, 0, 0)), sharedK),
480  cam2(Pose3(R, Point3(1, 0, 0)), sharedK);
481 
482  // one landmarks 1m in front of camera
483  Point3 landmark1(0, 0, 10);
484 
485  Point2Vector measurements_cam1;
486 
487  // Project 2 landmarks into 2 cameras
488  measurements_cam1.push_back(cam1.project(landmark1));
489  measurements_cam1.push_back(cam2.project(landmark1));
490 
491  // Create smart factors
492  KeyVector views{x1, x2};
493  FastVector<size_t> cameraIds{0, 0};
494 
495  std::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
496  cameraRig->push_back(Camera(Pose3(), sharedK));
497 
498  SmartRigFactor::shared_ptr smartFactor1 = std::make_shared<SmartRigFactor>(
499  model, cameraRig, params);
500  smartFactor1->add(measurements_cam1,
501  views); // we have a single camera so use default cameraIds
502 
504  cameras.push_back(cam1);
505  cameras.push_back(cam2);
506 
507  // Make sure triangulation works
508  CHECK(smartFactor1->triangulateSafe(cameras));
509  CHECK(!smartFactor1->isDegenerate());
510  CHECK(!smartFactor1->isPointBehindCamera());
511  auto p = smartFactor1->point();
512  CHECK(p);
514 
515  VectorValues zeroDelta;
516  Vector6 delta;
517  delta.setZero();
518  zeroDelta.insert(x1, delta);
519  zeroDelta.insert(x2, delta);
520 
521  VectorValues perturbedDelta;
522  delta.setOnes();
523  perturbedDelta.insert(x1, delta);
524  perturbedDelta.insert(x2, delta);
525  double expectedError = 2500;
526 
527  // After eliminating the point, A1 and A2 contain 2-rank information on
528  // cameras:
529  Matrix16 A1, A2;
530  A1 << -10, 0, 0, 0, 1, 0;
531  A2 << 10, 0, 1, 0, -1, 0;
532  A1 *= 10. / sigma;
533  A2 *= 10. / sigma;
534  Matrix expectedInformation; // filled below
535  {
536  // createHessianFactor
537  Matrix66 G11 = 0.5 * A1.transpose() * A1;
538  Matrix66 G12 = 0.5 * A1.transpose() * A2;
539  Matrix66 G22 = 0.5 * A2.transpose() * A2;
540 
541  Vector6 g1;
542  g1.setZero();
543  Vector6 g2;
544  g2.setZero();
545 
546  double f = 0;
547 
548  RegularHessianFactor<6> expected(x1, x2, G11, G12, g1, G22, g2, f);
549  expectedInformation = expected.information();
550 
551  Values values;
552  values.insert(x1, cam1.pose());
553  values.insert(x2, cam2.pose());
554 
555  std::shared_ptr<RegularHessianFactor<6>> actual =
556  smartFactor1->createHessianFactor(values, 0.0);
557  EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6));
558  EXPECT(assert_equal(expected, *actual, 1e-6));
559  EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6);
560  EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6);
561  }
562 }
563 
564 /* *************************************************************************/
565 TEST(SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor) {
566  using namespace vanillaRig;
567 
568  KeyVector views{x1, x2, x3};
569 
570  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
571 
572  // Project three landmarks into three cameras
573  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
574  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
575  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
576 
577  // create smart factor
578  std::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
579  cameraRig->push_back(Camera(Pose3(), sharedK));
580  FastVector<size_t> cameraIds{0, 0, 0};
581  SmartRigFactor::shared_ptr smartFactor1(
582  new SmartRigFactor(model, cameraRig, params));
583  smartFactor1->add(measurements_cam1, views, cameraIds);
584 
585  SmartRigFactor::shared_ptr smartFactor2(
586  new SmartRigFactor(model, cameraRig, params));
587  smartFactor2->add(measurements_cam2, views, cameraIds);
588 
589  SmartRigFactor::shared_ptr smartFactor3(
590  new SmartRigFactor(model, cameraRig, params));
591  smartFactor3->add(measurements_cam3, views, cameraIds);
592 
593  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
594 
596  graph.push_back(smartFactor1);
597  graph.push_back(smartFactor2);
598  graph.push_back(smartFactor3);
599  graph.addPrior(x1, cam1.pose(), noisePrior);
600  graph.addPrior(x2, cam2.pose(), noisePrior);
601 
602  // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10),
603  // Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
604  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
605  Point3(0.1, 0.1, 0.1)); // smaller noise
606  Values values;
607  values.insert(x1, cam1.pose());
608  values.insert(x2, cam2.pose());
609  // initialize third pose with some noise, we expect it to move back to
610  // original pose_above
611  values.insert(x3, pose_above * noise_pose);
612  EXPECT(assert_equal(Pose3(Rot3(1.11022302e-16, -0.0314107591, 0.99950656,
613  -0.99950656, -0.0313952598, -0.000986635786,
614  0.0314107591, -0.999013364, -0.0313952598),
615  Point3(0.1, -0.1, 1.9)),
616  values.at<Pose3>(x3)));
617 
618  Values result;
620  result = optimizer.optimize();
621  EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-7));
622 }
623 
624 /* *************************************************************************/
625 TEST(SmartProjectionRigFactor, landmarkDistance) {
626  using namespace vanillaRig;
627 
628  double excludeLandmarksFutherThanDist = 2;
629 
630  KeyVector views{x1, x2, x3};
631 
632  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
633 
634  // Project three landmarks into three cameras
635  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
636  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
637  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
638 
643  params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist);
644  params.setEnableEPI(false);
645 
646  std::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
647  cameraRig->push_back(Camera(Pose3(), sharedK));
648  FastVector<size_t> cameraIds{0, 0, 0};
649 
650  SmartRigFactor::shared_ptr smartFactor1(
651  new SmartRigFactor(model, cameraRig, params));
652  smartFactor1->add(measurements_cam1, views, cameraIds);
653 
654  SmartRigFactor::shared_ptr smartFactor2(
655  new SmartRigFactor(model, cameraRig, params));
656  smartFactor2->add(measurements_cam2, views, cameraIds);
657 
658  SmartRigFactor::shared_ptr smartFactor3(
659  new SmartRigFactor(model, cameraRig, params));
660  smartFactor3->add(measurements_cam3, views, cameraIds);
661 
662  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
663 
665  graph.push_back(smartFactor1);
666  graph.push_back(smartFactor2);
667  graph.push_back(smartFactor3);
668  graph.addPrior(x1, cam1.pose(), noisePrior);
669  graph.addPrior(x2, cam2.pose(), noisePrior);
670 
671  // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10),
672  // Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
673  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
674  Point3(0.1, 0.1, 0.1)); // smaller noise
675  Values values;
676  values.insert(x1, cam1.pose());
677  values.insert(x2, cam2.pose());
678  values.insert(x3, pose_above * noise_pose);
679 
680  // All factors are disabled and pose should remain where it is
681  Values result;
683  result = optimizer.optimize();
685 }
686 
687 /* *************************************************************************/
688 TEST(SmartProjectionRigFactor, dynamicOutlierRejection) {
689  using namespace vanillaRig;
690 
691  double excludeLandmarksFutherThanDist = 1e10;
692  double dynamicOutlierRejectionThreshold =
693  1; // max 1 pixel of average reprojection error
694 
695  KeyVector views{x1, x2, x3};
696 
697  // add fourth landmark
698  Point3 landmark4(5, -0.5, 1);
699 
700  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3,
701  measurements_cam4;
702 
703  // Project 4 landmarks into three cameras
704  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
705  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
706  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
707  projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4);
708  measurements_cam4.at(0) =
709  measurements_cam4.at(0) + Point2(10, 10); // add outlier
710 
714  params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist);
715  params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold);
716 
717  std::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
718  cameraRig->push_back(Camera(Pose3(), sharedK));
719  FastVector<size_t> cameraIds{0, 0, 0};
720 
721  SmartRigFactor::shared_ptr smartFactor1(
722  new SmartRigFactor(model, cameraRig, params));
723  smartFactor1->add(measurements_cam1, views, cameraIds);
724 
725  SmartRigFactor::shared_ptr smartFactor2(
726  new SmartRigFactor(model, cameraRig, params));
727  smartFactor2->add(measurements_cam2, views, cameraIds);
728 
729  SmartRigFactor::shared_ptr smartFactor3(
730  new SmartRigFactor(model, cameraRig, params));
731  smartFactor3->add(measurements_cam3, views, cameraIds);
732 
733  SmartRigFactor::shared_ptr smartFactor4(
734  new SmartRigFactor(model, cameraRig, params));
735  smartFactor4->add(measurements_cam4, views, cameraIds);
736 
737  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
738 
740  graph.push_back(smartFactor1);
741  graph.push_back(smartFactor2);
742  graph.push_back(smartFactor3);
743  graph.push_back(smartFactor4);
744  graph.addPrior(x1, cam1.pose(), noisePrior);
745  graph.addPrior(x2, cam2.pose(), noisePrior);
746 
747  Values values;
748  values.insert(x1, cam1.pose());
749  values.insert(x2, cam2.pose());
750  values.insert(x3, cam3.pose());
751 
752  // All factors are disabled and pose should remain where it is
753  Values result;
755  result = optimizer.optimize();
757 }
758 
759 /* *************************************************************************/
761  KeyVector views{x1, x2, x3};
762 
763  using namespace vanillaRig;
764 
765  // Two slightly different cameras
766  Pose3 pose2 =
767  level_pose * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
768  Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
771 
772  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
773 
774  // Project three landmarks into three cameras
775  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
776  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
777  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
778 
782 
783  std::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
784  cameraRig->push_back(Camera(Pose3(), sharedK));
785  FastVector<size_t> cameraIds{0, 0, 0};
786 
787  SmartRigFactor::shared_ptr smartFactor1(
788  new SmartRigFactor(model, cameraRig, params)); // HESSIAN, by default
789  smartFactor1->add(measurements_cam1, views, cameraIds);
790 
791  SmartRigFactor::shared_ptr smartFactor2(
792  new SmartRigFactor(model, cameraRig, params)); // HESSIAN, by default
793  smartFactor2->add(measurements_cam2, views, cameraIds);
794 
795  SmartRigFactor::shared_ptr smartFactor3(
796  new SmartRigFactor(model, cameraRig, params)); // HESSIAN, by default
797  smartFactor3->add(measurements_cam3, views, cameraIds);
798 
800  graph.push_back(smartFactor1);
801  graph.push_back(smartFactor2);
802  graph.push_back(smartFactor3);
803 
804  // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10),
805  // Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
806  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
807  Point3(0.1, 0.1, 0.1)); // smaller noise
808  Values values;
809  values.insert(x1, cam1.pose());
810  values.insert(x2, cam2.pose());
811  // initialize third pose with some noise, we expect it to move back to
812  // original pose_above
813  values.insert(x3, pose3 * noise_pose);
814  EXPECT(assert_equal(Pose3(Rot3(0.00563056869, -0.130848107, 0.991386438,
815  -0.991390265, -0.130426831, -0.0115837907,
816  0.130819108, -0.98278564, -0.130455917),
817  Point3(0.0897734171, -0.110201006, 0.901022872)),
818  values.at<Pose3>(x3)));
819 
820  std::shared_ptr<GaussianFactor> factor1 = smartFactor1->linearize(values);
821  std::shared_ptr<GaussianFactor> factor2 = smartFactor2->linearize(values);
822  std::shared_ptr<GaussianFactor> factor3 = smartFactor3->linearize(values);
823 
824  Matrix CumulativeInformation =
825  factor1->information() + factor2->information() + factor3->information();
826 
827  std::shared_ptr<GaussianFactorGraph> GaussianGraph =
829  Matrix GraphInformation = GaussianGraph->hessian().first;
830 
831  // Check Hessian
832  EXPECT(assert_equal(GraphInformation, CumulativeInformation, 1e-6));
833 
834  Matrix AugInformationMatrix = factor1->augmentedInformation() +
835  factor2->augmentedInformation() +
836  factor3->augmentedInformation();
837 
838  // Check Information vector
839  Vector InfoVector = AugInformationMatrix.block(
840  0, 18, 18, 1); // 18x18 Hessian + information vector
841 
842  // Check Hessian
843  EXPECT(assert_equal(InfoVector, GaussianGraph->hessian().second, 1e-6));
844 }
845 
846 /* *************************************************************************/
848  using namespace vanillaPose2;
849 
850  KeyVector views{x1, x2};
851 
852  // Project three landmarks into 2 cameras
853  Point2 cam1_uv1 = cam1.project(landmark1);
854  Point2 cam2_uv1 = cam2.project(landmark1);
855  Point2Vector measurements_cam1;
856  measurements_cam1.push_back(cam1_uv1);
857  measurements_cam1.push_back(cam2_uv1);
858 
859  std::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
860  cameraRig->push_back(Camera(Pose3(), sharedK2));
861  FastVector<size_t> cameraIds{0, 0};
862 
865  gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors
866 
867  SmartRigFactor::shared_ptr smartFactor1(
868  new SmartRigFactor(model, cameraRig, params));
869  smartFactor1->add(measurements_cam1, views, cameraIds);
870 
871  Pose3 noise_pose =
872  Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.5, 0.1, 0.3));
873  Values values;
874  values.insert(x1, cam1.pose());
875  values.insert(x2, cam2.pose());
876 
877  std::shared_ptr<GaussianFactor> factor = smartFactor1->linearize(values);
878 
879  // compute triangulation from linearization point
880  // compute reprojection errors (sum squared)
881  // compare with factor.info(): the bottom right element is the squared sum of
882  // the reprojection errors (normalized by the covariance) check that it is
883  // correctly scaled when using noiseProjection = [1/4 0; 0 1/4]
884 }
885 
886 /* ************************************************************************* */
887 TEST(SmartProjectionRigFactor, ConstructorWithCal3Bundler) {
888  using namespace bundlerPose;
889  std::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
890  cameraRig->push_back(Camera(Pose3(), sharedBundlerK));
891 
894  SmartRigFactor factor(model, cameraRig, params);
896 }
897 
898 /* *************************************************************************/
900  using namespace bundlerPose;
903  gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors
904 
905  // three landmarks ~5 meters in front of camera
906  Point3 landmark3(3, 0, 3.0);
907 
908  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
909 
910  // Project three landmarks into three cameras
911  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
912  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
913  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
914 
915  KeyVector views{x1, x2, x3};
916 
917  std::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
918  cameraRig->push_back(Camera(Pose3(), sharedBundlerK));
919  FastVector<size_t> cameraIds{0, 0, 0};
920 
921  SmartRigFactor::shared_ptr smartFactor1(
922  new SmartRigFactor(model, cameraRig, params));
923  smartFactor1->add(measurements_cam1, views, cameraIds);
924 
925  SmartRigFactor::shared_ptr smartFactor2(
926  new SmartRigFactor(model, cameraRig, params));
927  smartFactor2->add(measurements_cam2, views, cameraIds);
928 
929  SmartRigFactor::shared_ptr smartFactor3(
930  new SmartRigFactor(model, cameraRig, params));
931  smartFactor3->add(measurements_cam3, views, cameraIds);
932 
933  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
934 
936  graph.push_back(smartFactor1);
937  graph.push_back(smartFactor2);
938  graph.push_back(smartFactor3);
939  graph.addPrior(x1, cam1.pose(), noisePrior);
940  graph.addPrior(x2, cam2.pose(), noisePrior);
941 
942  // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10),
943  // Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
944  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
945  Point3(0.1, 0.1, 0.1)); // smaller noise
946  Values values;
947  values.insert(x1, cam1.pose());
948  values.insert(x2, cam2.pose());
949  // initialize third pose with some noise, we expect it to move back to
950  // original pose_above
951  values.insert(x3, pose_above * noise_pose);
953  Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598,
954  -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598),
955  Point3(0.1, -0.1, 1.9)),
956  values.at<Pose3>(x3)));
957 
958  Values result;
960  result = optimizer.optimize();
962 }
963 
966 static Symbol l0('L', 0);
967 /* *************************************************************************/
969  hessianComparedToProjFactors_measurementsFromSamePose) {
970  // in this test we make sure the fact works even if we have multiple pixel
971  // measurements of the same landmark at a single pose, a setup that occurs in
972  // multi-camera systems
973 
974  using namespace vanillaRig;
975  Point2Vector measurements_lmk1;
976 
977  // Project three landmarks into three cameras
978  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1);
979 
980  // create redundant measurements:
981  Camera::MeasurementVector measurements_lmk1_redundant = measurements_lmk1;
982  measurements_lmk1_redundant.push_back(
983  measurements_lmk1.at(0)); // we readd the first measurement
984 
985  // create inputs
986  KeyVector keys{x1, x2, x3, x1};
987 
988  std::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
989  cameraRig->push_back(Camera(Pose3(), sharedK));
990  FastVector<size_t> cameraIds{0, 0, 0, 0};
991 
992  SmartRigFactor::shared_ptr smartFactor1(
993  new SmartRigFactor(model, cameraRig, params));
994  smartFactor1->add(measurements_lmk1_redundant, keys, cameraIds);
995 
996  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
997  Point3(0.1, 0.1, 0.1)); // smaller noise
998  Values values;
1000  values.insert(x2, pose_right);
1001  // initialize third pose with some noise to get a nontrivial linearization
1002  // point
1003  values.insert(x3, pose_above * noise_pose);
1004  EXPECT( // check that the pose is actually noisy
1005  assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656,
1006  -0.0313952598, -0.000986635786, 0.0314107591,
1007  -0.999013364, -0.0313952598),
1008  Point3(0.1, -0.1, 1.9)),
1009  values.at<Pose3>(x3)));
1010 
1011  // linearization point for the poses
1013  Pose3 pose2 = pose_right;
1014  Pose3 pose3 = pose_above * noise_pose;
1015 
1016  // ==== check Hessian of smartFactor1 =====
1017  // -- compute actual Hessian
1018  std::shared_ptr<GaussianFactor> linearfactor1 =
1019  smartFactor1->linearize(values);
1020  Matrix actualHessian = linearfactor1->information();
1021 
1022  // -- compute expected Hessian from manual Schur complement from Jacobians
1023  // linearization point for the 3D point
1024  smartFactor1->triangulateSafe(smartFactor1->cameras(values));
1025  TriangulationResult point = smartFactor1->point();
1026  EXPECT(point.valid()); // check triangulated point is valid
1027 
1028  // Use standard ProjectionFactor factor to calculate the Jacobians
1029  Matrix F = Matrix::Zero(2 * 4, 6 * 3);
1030  Matrix E = Matrix::Zero(2 * 4, 3);
1031  Vector b = Vector::Zero(2 * 4);
1032 
1033  // create projection factors rolling shutter
1034  TestProjectionFactor factor11(measurements_lmk1_redundant[0], model, x1, l0,
1035  sharedK);
1036  Matrix HPoseActual, HEActual;
1037  // note: b is minus the reprojection error, cf the smart factor jacobian
1038  // computation
1039  b.segment<2>(0) =
1040  -factor11.evaluateError(pose1, *point, HPoseActual, HEActual);
1041  F.block<2, 6>(0, 0) = HPoseActual;
1042  E.block<2, 3>(0, 0) = HEActual;
1043 
1044  TestProjectionFactor factor12(measurements_lmk1_redundant[1], model, x2, l0,
1045  sharedK);
1046  b.segment<2>(2) =
1047  -factor12.evaluateError(pose2, *point, HPoseActual, HEActual);
1048  F.block<2, 6>(2, 6) = HPoseActual;
1049  E.block<2, 3>(2, 0) = HEActual;
1050 
1051  TestProjectionFactor factor13(measurements_lmk1_redundant[2], model, x3, l0,
1052  sharedK);
1053  b.segment<2>(4) =
1054  -factor13.evaluateError(pose3, *point, HPoseActual, HEActual);
1055  F.block<2, 6>(4, 12) = HPoseActual;
1056  E.block<2, 3>(4, 0) = HEActual;
1057 
1058  TestProjectionFactor factor14(measurements_lmk1_redundant[3], model, x1, l0,
1059  sharedK);
1060  b.segment<2>(6) =
1061  -factor11.evaluateError(pose1, *point, HPoseActual, HEActual);
1062  F.block<2, 6>(6, 0) = HPoseActual;
1063  E.block<2, 3>(6, 0) = HEActual;
1064 
1065  // whiten
1066  F = (1 / sigma) * F;
1067  E = (1 / sigma) * E;
1068  b = (1 / sigma) * b;
1069  //* G = F' * F - F' * E * P * E' * F
1070  Matrix P = (E.transpose() * E).inverse();
1071  Matrix expectedHessian =
1072  F.transpose() * F - (F.transpose() * E * P * E.transpose() * F);
1073  EXPECT(assert_equal(expectedHessian, actualHessian, 1e-6));
1074 
1075  // ==== check Information vector of smartFactor1 =====
1076  GaussianFactorGraph gfg;
1077  gfg.add(linearfactor1);
1078  Matrix actualHessian_v2 = gfg.hessian().first;
1079  EXPECT(assert_equal(actualHessian_v2, actualHessian,
1080  1e-6)); // sanity check on hessian
1081 
1082  // -- compute actual information vector
1083  Vector actualInfoVector = gfg.hessian().second;
1084 
1085  // -- compute expected information vector from manual Schur complement from
1086  // Jacobians
1087  //* g = F' * (b - E * P * E' * b)
1088  Vector expectedInfoVector = F.transpose() * (b - E * P * E.transpose() * b);
1089  EXPECT(assert_equal(expectedInfoVector, actualInfoVector, 1e-6));
1090 
1091  // ==== check error of smartFactor1 (again) =====
1092  NonlinearFactorGraph nfg_projFactors;
1093  nfg_projFactors.add(factor11);
1094  nfg_projFactors.add(factor12);
1095  nfg_projFactors.add(factor13);
1096  nfg_projFactors.add(factor14);
1097  values.insert(l0, *point);
1098 
1099  double actualError = smartFactor1->error(values);
1100  double expectedError = nfg_projFactors.error(values);
1101  EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7);
1102 }
1103 
1104 /* *************************************************************************/
1105 TEST(SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose) {
1106  using namespace vanillaRig;
1107  Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3;
1108 
1109  // Project three landmarks into three cameras
1110  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1);
1111  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2);
1112  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3);
1113 
1114  // create inputs
1115  KeyVector keys{x1, x2, x3};
1116  std::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
1117  cameraRig->push_back(Camera(Pose3(), sharedK));
1118  FastVector<size_t> cameraIds{0, 0, 0};
1119  FastVector<size_t> cameraIdsRedundant{0, 0, 0, 0};
1120 
1121  // For first factor, we create redundant measurement (taken by the same keys
1122  // as factor 1, to make sure the redundancy in the keys does not create
1123  // problems)
1124  Camera::MeasurementVector& measurements_lmk1_redundant = measurements_lmk1;
1125  measurements_lmk1_redundant.push_back(
1126  measurements_lmk1.at(0)); // we readd the first measurement
1127  KeyVector keys_redundant = keys;
1128  keys_redundant.push_back(keys.at(0)); // we readd the first key
1129 
1130  SmartRigFactor::shared_ptr smartFactor1(
1131  new SmartRigFactor(model, cameraRig, params));
1132  smartFactor1->add(measurements_lmk1_redundant, keys_redundant,
1133  cameraIdsRedundant);
1134 
1135  SmartRigFactor::shared_ptr smartFactor2(
1136  new SmartRigFactor(model, cameraRig, params));
1137  smartFactor2->add(measurements_lmk2, keys, cameraIds);
1138 
1139  SmartRigFactor::shared_ptr smartFactor3(
1140  new SmartRigFactor(model, cameraRig, params));
1141  smartFactor3->add(measurements_lmk3, keys, cameraIds);
1142 
1143  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
1144 
1146  graph.push_back(smartFactor1);
1147  graph.push_back(smartFactor2);
1148  graph.push_back(smartFactor3);
1149  graph.addPrior(x1, level_pose, noisePrior);
1150  graph.addPrior(x2, pose_right, noisePrior);
1151 
1152  Values groundTruth;
1153  groundTruth.insert(x1, level_pose);
1154  groundTruth.insert(x2, pose_right);
1155  groundTruth.insert(x3, pose_above);
1156  DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9);
1157 
1158  // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10),
1159  // Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
1160  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
1161  Point3(0.1, 0.1, 0.1)); // smaller noise
1162  Values values;
1164  values.insert(x2, pose_right);
1165  // initialize third pose with some noise, we expect it to move back to
1166  // original pose_above
1167  values.insert(x3, pose_above * noise_pose);
1168  EXPECT( // check that the pose is actually noisy
1169  assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656,
1170  -0.0313952598, -0.000986635786, 0.0314107591,
1171  -0.999013364, -0.0313952598),
1172  Point3(0.1, -0.1, 1.9)),
1173  values.at<Pose3>(x3)));
1174 
1175  Values result;
1177  result = optimizer.optimize();
1178  EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-5));
1179 }
1180 
1181 #ifndef DISABLE_TIMING
1182 #include <gtsam/base/timing.h>
1183 // this factor is slightly slower (but comparable) to original
1184 // SmartProjectionPoseFactor
1185 //-Total: 0 CPU (0 times, 0 wall, 0.17 children, min: 0 max: 0)
1186 //| -SmartRigFactor LINEARIZE: 0.05 CPU (10000 times, 0.057952 wall, 0.05
1187 // children, min: 0 max: 0) | -SmartPoseFactor LINEARIZE: 0.05 CPU (10000
1188 // times, 0.069647 wall, 0.05 children, min: 0 max: 0)
1189 /* *************************************************************************/
1190 TEST(SmartProjectionRigFactor, timing) {
1191  using namespace vanillaRig;
1192 
1193  // Default cameras for simple derivatives
1194  static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0));
1195 
1196  Rot3 R = Rot3::Identity();
1197  Pose3 pose1 = Pose3(R, Point3(0, 0, 0));
1198  Pose3 pose2 = Pose3(R, Point3(1, 0, 0));
1199  Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple);
1200  Pose3 body_P_sensorId = Pose3();
1201 
1202  std::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
1203  cameraRig->push_back(Camera(body_P_sensorId, sharedKSimple));
1204 
1205  // one landmarks 1m in front of camera
1206  Point3 landmark1(0, 0, 10);
1207 
1208  Point2Vector measurements_lmk1;
1209 
1210  // Project 2 landmarks into 2 cameras
1211  measurements_lmk1.push_back(cam1.project(landmark1));
1212  measurements_lmk1.push_back(cam2.project(landmark1));
1213 
1214  size_t nrTests = 10000;
1215 
1216  for (size_t i = 0; i < nrTests; i++) {
1217  SmartRigFactor::shared_ptr smartRigFactor(
1218  new SmartRigFactor(model, cameraRig, params));
1219  smartRigFactor->add(measurements_lmk1[0], x1, cameraId1);
1220  smartRigFactor->add(measurements_lmk1[1], x1, cameraId1);
1221 
1222  Values values;
1223  values.insert(x1, pose1);
1224  values.insert(x2, pose2);
1225  gttic_(SmartRigFactor_LINEARIZE);
1226  smartRigFactor->linearize(values);
1227  gttoc_(SmartRigFactor_LINEARIZE);
1228  }
1229 
1230  for (size_t i = 0; i < nrTests; i++) {
1231  SmartFactor::shared_ptr smartFactor(
1232  new SmartFactor(model, sharedKSimple, params));
1233  smartFactor->add(measurements_lmk1[0], x1);
1234  smartFactor->add(measurements_lmk1[1], x2);
1235 
1236  Values values;
1237  values.insert(x1, pose1);
1238  values.insert(x2, pose2);
1239  gttic_(SmartPoseFactor_LINEARIZE);
1240  smartFactor->linearize(values);
1241  gttoc_(SmartPoseFactor_LINEARIZE);
1242  }
1243  tictoc_print_();
1244 }
1245 #endif
1246 
1247 /* *************************************************************************/
1248 TEST(SmartProjectionFactorP, optimization_3poses_sphericalCamera) {
1249  using namespace sphericalCamera;
1250  Camera::MeasurementVector measurements_lmk1, measurements_lmk2,
1251  measurements_lmk3;
1252 
1253  // Project three landmarks into three cameras
1254  projectToMultipleCameras<Camera>(cam1, cam2, cam3, landmark1,
1255  measurements_lmk1);
1256  projectToMultipleCameras<Camera>(cam1, cam2, cam3, landmark2,
1257  measurements_lmk2);
1258  projectToMultipleCameras<Camera>(cam1, cam2, cam3, landmark3,
1259  measurements_lmk3);
1260 
1261  // create inputs
1262  KeyVector keys;
1263  keys.push_back(x1);
1264  keys.push_back(x2);
1265  keys.push_back(x3);
1266 
1267  std::shared_ptr<Cameras> cameraRig(new Cameras());
1268  cameraRig->push_back(Camera(Pose3(), emptyK));
1269 
1272  gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors
1273  params.setRankTolerance(0.1);
1274 
1275  SmartFactorP::shared_ptr smartFactor1(
1276  new SmartFactorP(model, cameraRig, params));
1277  smartFactor1->add(measurements_lmk1, keys);
1278 
1279  SmartFactorP::shared_ptr smartFactor2(
1280  new SmartFactorP(model, cameraRig, params));
1281  smartFactor2->add(measurements_lmk2, keys);
1282 
1283  SmartFactorP::shared_ptr smartFactor3(
1284  new SmartFactorP(model, cameraRig, params));
1285  smartFactor3->add(measurements_lmk3, keys);
1286 
1287  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
1288 
1290  graph.push_back(smartFactor1);
1291  graph.push_back(smartFactor2);
1292  graph.push_back(smartFactor3);
1293  graph.addPrior(x1, level_pose, noisePrior);
1294  graph.addPrior(x2, pose_right, noisePrior);
1295 
1296  Values groundTruth;
1297  groundTruth.insert(x1, level_pose);
1298  groundTruth.insert(x2, pose_right);
1299  groundTruth.insert(x3, pose_above);
1300  DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9);
1301 
1302  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 100),
1303  Point3(0.2, 0.2, 0.2)); // note: larger noise!
1304 
1305  Values values;
1307  values.insert(x2, pose_right);
1308  // initialize third pose with some noise, we expect it to move back to
1309  // original pose_above
1310  values.insert(x3, pose_above * noise_pose);
1311 
1312  DOUBLES_EQUAL(0.94148963675515274, graph.error(values), 1e-9);
1313 
1314  Values result;
1316  result = optimizer.optimize();
1317 
1318  EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-5));
1319 }
1320 
1321 #ifndef DISABLE_TIMING
1322 #include <gtsam/base/timing.h>
1323 // using spherical camera is slightly slower (but comparable) to
1324 // PinholePose<Cal3_S2>
1325 //| -SmartFactorP spherical LINEARIZE: 0.01 CPU (1000 times, 0.008178 wall,
1326 // 0.01 children, min: 0 max: 0) | -SmartFactorP pinhole LINEARIZE: 0.01 CPU
1327 //(1000 times, 0.005717 wall, 0.01 children, min: 0 max: 0)
1328 /* *************************************************************************/
1329 TEST(SmartProjectionFactorP, timing_sphericalCamera) {
1330  // create common data
1331  Rot3 R = Rot3::Identity();
1332  Pose3 pose1 = Pose3(R, Point3(0, 0, 0));
1333  Pose3 pose2 = Pose3(R, Point3(1, 0, 0));
1334  Pose3 body_P_sensorId = Pose3();
1335  Point3 landmark1(0, 0, 10);
1336 
1337  // create spherical data
1339  SphericalCamera cam1_sphere(pose1, emptyK), cam2_sphere(pose2, emptyK);
1340  // Project 2 landmarks into 2 cameras
1341  std::vector<Unit3> measurements_lmk1_sphere;
1342  measurements_lmk1_sphere.push_back(cam1_sphere.project(landmark1));
1343  measurements_lmk1_sphere.push_back(cam2_sphere.project(landmark1));
1344 
1345  // create Cal3_S2 data
1346  static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0));
1347  PinholePose<Cal3_S2> cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple);
1348  // Project 2 landmarks into 2 cameras
1349  std::vector<Point2> measurements_lmk1;
1350  measurements_lmk1.push_back(cam1.project(landmark1));
1351  measurements_lmk1.push_back(cam2.project(landmark1));
1352 
1355  gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors
1356 
1357  size_t nrTests = 1000;
1358 
1359  for (size_t i = 0; i < nrTests; i++) {
1360  std::shared_ptr<CameraSet<SphericalCamera>> cameraRig(
1361  new CameraSet<SphericalCamera>()); // single camera in the rig
1362  cameraRig->push_back(SphericalCamera(body_P_sensorId, emptyK));
1363 
1366  params));
1367  smartFactorP->add(measurements_lmk1_sphere[0], x1);
1368  smartFactorP->add(measurements_lmk1_sphere[1], x1);
1369 
1370  Values values;
1371  values.insert(x1, pose1);
1372  values.insert(x2, pose2);
1373  gttic_(SmartFactorP_spherical_LINEARIZE);
1374  smartFactorP->linearize(values);
1375  gttoc_(SmartFactorP_spherical_LINEARIZE);
1376  }
1377 
1378  for (size_t i = 0; i < nrTests; i++) {
1379  std::shared_ptr<CameraSet<PinholePose<Cal3_S2>>> cameraRig(
1380  new CameraSet<PinholePose<Cal3_S2>>()); // single camera in the rig
1381  cameraRig->push_back(PinholePose<Cal3_S2>(body_P_sensorId, sharedKSimple));
1382 
1383  SmartProjectionRigFactor<PinholePose<Cal3_S2>>::shared_ptr smartFactorP2(
1385  params));
1386  smartFactorP2->add(measurements_lmk1[0], x1);
1387  smartFactorP2->add(measurements_lmk1[1], x1);
1388 
1389  Values values;
1390  values.insert(x1, pose1);
1391  values.insert(x2, pose2);
1392  gttic_(SmartFactorP_pinhole_LINEARIZE);
1393  smartFactorP2->linearize(values);
1394  gttoc_(SmartFactorP_pinhole_LINEARIZE);
1395  }
1396  tictoc_print_();
1397 }
1398 #endif
1399 
1400 /* *************************************************************************/
1401 TEST(SmartProjectionFactorP, 2poses_rankTol) {
1402  Pose3 poseA = Pose3(
1403  Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2),
1404  Point3(0.0, 0.0, 0.0)); // with z pointing along x axis of global frame
1405  Pose3 poseB = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2),
1406  Point3(0.0, -0.1, 0.0)); // 10cm to the right of poseA
1407  Point3 landmarkL = Point3(5.0, 0.0, 0.0); // 5m in front of poseA
1408 
1409  // triangulate from a stereo with 10cm baseline, assuming standard calibration
1410  { // default rankTol = 1 gives a valid point (compare with calibrated and
1411  // spherical cameras below)
1412  using namespace vanillaPose; // pinhole with Cal3_S2 calibration
1413 
1414  Camera cam1(poseA, sharedK);
1415  Camera cam2(poseB, sharedK);
1416 
1419  gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors
1421 
1422  std::shared_ptr<CameraSet<PinholePose<Cal3_S2>>> cameraRig(
1423  new CameraSet<PinholePose<Cal3_S2>>()); // single camera in the rig
1424  cameraRig->push_back(PinholePose<Cal3_S2>(Pose3(), sharedK));
1425 
1426  SmartRigFactor::shared_ptr smartFactor1(
1427  new SmartRigFactor(model, cameraRig, params));
1428  smartFactor1->add(cam1.project(landmarkL), x1);
1429  smartFactor1->add(cam2.project(landmarkL), x2);
1430 
1432  graph.push_back(smartFactor1);
1433 
1434  Values groundTruth;
1435  groundTruth.insert(x1, poseA);
1436  groundTruth.insert(x2, poseB);
1437  DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9);
1438 
1439  // get point
1440  TriangulationResult point = smartFactor1->point();
1441  EXPECT(point.valid()); // valid triangulation
1442  EXPECT(assert_equal(landmarkL, *point, 1e-7));
1443  }
1444  // triangulate from a stereo with 10cm baseline, assuming canonical
1445  // calibration
1446  { // default rankTol = 1 or 0.1 gives a degenerate point, which is
1447  // undesirable for a point 5m away and 10cm baseline
1448  using namespace vanillaPose; // pinhole with Cal3_S2 calibration
1449  static Cal3_S2::shared_ptr canonicalK(
1450  new Cal3_S2(1.0, 1.0, 0.0, 0.0, 0.0)); // canonical camera
1451 
1452  Camera cam1(poseA, canonicalK);
1453  Camera cam2(poseB, canonicalK);
1454 
1457  gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors
1458  params.setRankTolerance(0.1);
1459 
1460  std::shared_ptr<CameraSet<PinholePose<Cal3_S2>>> cameraRig(
1461  new CameraSet<PinholePose<Cal3_S2>>()); // single camera in the rig
1462  cameraRig->push_back(PinholePose<Cal3_S2>(Pose3(), canonicalK));
1463 
1464  SmartRigFactor::shared_ptr smartFactor1(
1465  new SmartRigFactor(model, cameraRig, params));
1466  smartFactor1->add(cam1.project(landmarkL), x1);
1467  smartFactor1->add(cam2.project(landmarkL), x2);
1468 
1470  graph.push_back(smartFactor1);
1471 
1472  Values groundTruth;
1473  groundTruth.insert(x1, poseA);
1474  groundTruth.insert(x2, poseB);
1475  DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9);
1476 
1477  // get point
1478  TriangulationResult point = smartFactor1->point();
1479  EXPECT(point.degenerate()); // valid triangulation
1480  }
1481  // triangulate from a stereo with 10cm baseline, assuming canonical
1482  // calibration
1483  { // smaller rankTol = 0.01 gives a valid point (compare with calibrated and
1484  // spherical cameras below)
1485  using namespace vanillaPose; // pinhole with Cal3_S2 calibration
1486  static Cal3_S2::shared_ptr canonicalK(
1487  new Cal3_S2(1.0, 1.0, 0.0, 0.0, 0.0)); // canonical camera
1488 
1489  Camera cam1(poseA, canonicalK);
1490  Camera cam2(poseB, canonicalK);
1491 
1494  gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors
1495  params.setRankTolerance(0.01);
1496 
1497  std::shared_ptr<CameraSet<PinholePose<Cal3_S2>>> cameraRig(
1498  new CameraSet<PinholePose<Cal3_S2>>()); // single camera in the rig
1499  cameraRig->push_back(PinholePose<Cal3_S2>(Pose3(), canonicalK));
1500 
1501  SmartRigFactor::shared_ptr smartFactor1(
1502  new SmartRigFactor(model, cameraRig, params));
1503  smartFactor1->add(cam1.project(landmarkL), x1);
1504  smartFactor1->add(cam2.project(landmarkL), x2);
1505 
1507  graph.push_back(smartFactor1);
1508 
1509  Values groundTruth;
1510  groundTruth.insert(x1, poseA);
1511  groundTruth.insert(x2, poseB);
1512  DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9);
1513 
1514  // get point
1515  TriangulationResult point = smartFactor1->point();
1516  EXPECT(point.valid()); // valid triangulation
1517  EXPECT(assert_equal(landmarkL, *point, 1e-7));
1518  }
1519 }
1520 
1521 /* *************************************************************************/
1522 TEST(SmartProjectionFactorP, 2poses_sphericalCamera_rankTol) {
1523  typedef SphericalCamera Camera;
1526  Pose3 poseA = Pose3(
1527  Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2),
1528  Point3(0.0, 0.0, 0.0)); // with z pointing along x axis of global frame
1529  Pose3 poseB = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2),
1530  Point3(0.0, -0.1, 0.0)); // 10cm to the right of poseA
1531  Point3 landmarkL = Point3(5.0, 0.0, 0.0); // 5m in front of poseA
1532 
1533  Camera cam1(poseA);
1534  Camera cam2(poseB);
1535 
1536  std::shared_ptr<CameraSet<SphericalCamera>> cameraRig(
1537  new CameraSet<SphericalCamera>()); // single camera in the rig
1538  cameraRig->push_back(SphericalCamera(Pose3(), emptyK));
1539 
1540  // TRIANGULATION TEST WITH DEFAULT RANK TOL
1541  { // rankTol = 1 or 0.1 gives a degenerate point, which is undesirable for a
1542  // point 5m away and 10cm baseline
1545  gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors
1546  params.setRankTolerance(0.1);
1547 
1548  SmartRigFactor::shared_ptr smartFactor1(
1549  new SmartRigFactor(model, cameraRig, params));
1550  smartFactor1->add(cam1.project(landmarkL), x1);
1551  smartFactor1->add(cam2.project(landmarkL), x2);
1552 
1554  graph.push_back(smartFactor1);
1555 
1556  Values groundTruth;
1557  groundTruth.insert(x1, poseA);
1558  groundTruth.insert(x2, poseB);
1559  DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9);
1560 
1561  // get point
1562  TriangulationResult point = smartFactor1->point();
1563  EXPECT(point.degenerate()); // not enough parallax
1564  }
1565  // SAME TEST WITH SMALLER RANK TOL
1566  { // rankTol = 0.01 gives a valid point
1567  // By playing with this test, we can show we can triangulate also with a
1568  // baseline of 5cm (even for points far away, >100m), but the test fails
1569  // when the baseline becomes 1cm. This suggests using rankTol = 0.01 and
1570  // setting a reasonable max landmark distance to obtain best results.
1573  gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors
1574  params.setRankTolerance(0.01);
1575 
1576  SmartRigFactor::shared_ptr smartFactor1(
1577  new SmartRigFactor(model, cameraRig, params));
1578  smartFactor1->add(cam1.project(landmarkL), x1);
1579  smartFactor1->add(cam2.project(landmarkL), x2);
1580 
1582  graph.push_back(smartFactor1);
1583 
1584  Values groundTruth;
1585  groundTruth.insert(x1, poseA);
1586  groundTruth.insert(x2, poseB);
1587  DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9);
1588 
1589  // get point
1590  TriangulationResult point = smartFactor1->point();
1591  EXPECT(point.valid()); // valid triangulation
1592  EXPECT(assert_equal(landmarkL, *point, 1e-7));
1593  }
1594 }
1595 
1596 /* ************************************************************************* */
1597 int main() {
1598  TestResult tr;
1599  return TestRegistry::runAllTests(tr);
1600 }
1601 /* ************************************************************************* */
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
timing.h
Timing utilities.
x3
static Symbol x3('X', 3)
vanillaRig
Definition: testSmartProjectionRigFactor.cpp:59
cam1
static PinholeCamera< Cal3_S2 > cam1(pose3, cal1)
PoseTranslationPrior.h
Implements a prior on the translation component of a pose.
gtsam::NonlinearOptimizer::optimize
virtual const Values & optimize()
Definition: NonlinearOptimizer.h:98
sigma
static const double sigma
Definition: testSmartProjectionRigFactor.cpp:37
gtsam::StereoCamera::pose
const Pose3 & pose() const
pose
Definition: StereoCamera.h:127
ProjectionFactor.h
Reprojection of a LANDMARK to a 2D point.
cameraId1
Key cameraId1
Definition: testSmartProjectionRigFactor.cpp:49
vanillaPoseRS::cam3
Camera cam3(interp_pose3, sharedK)
inverse
const EIGEN_DEVICE_FUNC InverseReturnType inverse() const
Definition: ArrayCwiseUnaryOps.h:411
fixture::cameras
std::array< PinholeCamera< Cal3_S2 >, 3 > cameras
Definition: testTransferFactor.cpp:59
vanillaPose::SmartRigFactor
SmartProjectionRigFactor< Camera > SmartRigFactor
Definition: smartFactorScenarios.h:78
simple_graph::factor2
auto factor2
Definition: testJacobianFactor.cpp:203
vanillaPose
Definition: smartFactorScenarios.h:74
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
level_pose
Pose3 level_pose
Definition: testInvDepthCamera3.cpp:21
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
gtsam::HESSIAN
@ HESSIAN
Definition: SmartFactorParams.h:31
TestHarness.h
gtsam::EmptyCal
Definition: SphericalCamera.h:42
keys
const KeyVector keys
Definition: testRegularImplicitSchurFactor.cpp:40
g1
Pose3 g1(Rot3(), Point3(100.0, 0.0, 300.0))
Camera
Definition: camera.h:36
gtsam::RegularHessianFactor
Definition: RegularHessianFactor.h:28
gtsam::VectorValues::insert
iterator insert(const std::pair< Key, Vector > &key_value)
Definition: VectorValues.cpp:90
gtsam::SmartProjectionParams
Definition: SmartFactorParams.h:42
b
Scalar * b
Definition: benchVecAdd.cpp:17
gtsam::SmartProjectionParams::setLandmarkDistanceThreshold
void setLandmarkDistanceThreshold(double landmarkDistanceThreshold)
Definition: SmartFactorParams.h:112
gtsam::FastVector
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
Definition: FastVector.h:34
LevenbergMarquardtOptimizer.h
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
gtsam::GaussianFactorGraph::hessian
std::pair< Matrix, Vector > hessian(const Ordering &ordering) const
Definition: GaussianFactorGraph.cpp:264
measurement1
static Point2 measurement1(323.0, 240.0)
pose3
static Pose3 pose3(rt3, pt3)
l0
static Symbol l0('L', 0)
gtsam::SphericalCamera
Definition: SphericalCamera.h:74
vanillaPose2::sharedK2
static const Cal3_S2::shared_ptr sharedK2(new Cal3_S2(1500, 1200, 0, 640, 480))
x1
static Symbol x1('X', 1)
vanilla::level_camera
static const Camera level_camera(level_pose, K2)
lmParams
LevenbergMarquardtParams lmParams
Definition: testSmartProjectionRigFactor.cpp:55
gtsam::DecisionTreeFactor::error
double error(const DiscreteValues &values) const override
Calculate error for DiscreteValues x, is -log(probability).
Definition: DecisionTreeFactor.cpp:56
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
vanillaPose::sharedK
static const Cal3_S2::shared_ptr sharedK(new Cal3_S2(fov, w, h))
different_sigmas::values
HybridValues values
Definition: testHybridBayesNet.cpp:245
gtsam::NonlinearFactorGraph::linearize
std::shared_ptr< GaussianFactorGraph > linearize(const Values &linearizationPoint) const
Linearize a nonlinear factor graph.
Definition: NonlinearFactorGraph.cpp:239
simple::pose2
static Pose3 pose2
Definition: testInitializePose3.cpp:58
X
#define X
Definition: icosphere.cpp:20
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:39
rankTol
static const double rankTol
Definition: testSmartProjectionRigFactor.cpp:35
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:92
result
Values result
Definition: OdometryOptimize.cpp:8
gtsam::CameraSet
A set of cameras, all with their own calibration.
Definition: CameraSet.h:37
TEST
TEST(SmartProjectionRigFactor, Constructor)
Definition: testSmartProjectionRigFactor.cpp:67
sphericalCamera::SmartFactorP
SmartProjectionRigFactor< Camera > SmartFactorP
Definition: smartFactorScenarios.h:142
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)
pruning_fixture::factor
DecisionTreeFactor factor(D &C &B &A, "0.0 0.0 0.0 0.60658897 0.61241912 0.61241969 0.61247685 0.61247742 0.0 " "0.0 0.0 0.99995287 1.0 1.0 1.0 1.0")
vanilla::level_camera_right
static const Camera level_camera_right(pose_right, K2)
gtsam::GenericProjectionFactor::evaluateError
Vector evaluateError(const Pose3 &pose, const Point3 &point, OptionalMatrixType H1, OptionalMatrixType H2) const override
Evaluate error h(x)-z and optionally derivatives.
Definition: ProjectionFactor.h:138
gtsam::ZERO_ON_DEGENERACY
@ ZERO_ON_DEGENERACY
Definition: SmartFactorParams.h:36
sphericalCamera::emptyK
static const EmptyCal::shared_ptr emptyK(new EmptyCal())
gtsam::GaussianFactorGraph
Definition: GaussianFactorGraph.h:73
x2
static Symbol x2('X', 2)
gtsam_unstable.tests.test_ProjectionFactorRollingShutter.point
point
Definition: test_ProjectionFactorRollingShutter.py:25
gtsam::NonlinearFactorGraph::error
double error(const Values &values) const
Definition: NonlinearFactorGraph.cpp:170
gtsam::SmartProjectionParams::setDynamicOutlierRejectionThreshold
void setDynamicOutlierRejectionThreshold(double dynOutRejectionThreshold)
Definition: SmartFactorParams.h:115
gtsam::Values::at
const ValueType at(Key j) const
Definition: Values-inl.h:261
gtsam::SmartProjectionParams::setDegeneracyMode
void setDegeneracyMode(DegeneracyMode degMode)
Definition: SmartFactorParams.h:100
gtsam::SmartProjectionRigFactor::shared_ptr
std::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
Definition: SmartProjectionRigFactor.h:81
landmark1
Point2 landmark1(5.0, 1.5)
gtsam::SmartProjectionParams::setRankTolerance
void setRankTolerance(double rankTol)
Definition: SmartFactorParams.h:106
numericalDerivative.h
Some functions to compute numerical derivatives.
gtsam.examples.PlanarManipulatorExample.delta
def delta(g0, g1)
Definition: PlanarManipulatorExample.py:45
gtsam::VectorValues
Definition: VectorValues.h:74
gttoc_
#define gttoc_(label)
Definition: timing.h:250
gttic_
#define gttic_(label)
Definition: timing.h:245
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
gtsam::Pose3
Definition: Pose3.h:37
DOUBLES_EQUAL
#define DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:141
g2
Pose3 g2(g1.expmap(h *V1_g1))
gtsam::Pose3::inverse
Pose3 inverse() const
inverse transformation with derivatives
Definition: Pose3.cpp:47
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
gtsam::SharedDiagonal
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:764
gtsam::GenericProjectionFactor
Definition: ProjectionFactor.h:40
A2
static const double A2[]
Definition: expn.h:7
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
gtsam::NonlinearFactorGraph::addPrior
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
Definition: NonlinearFactorGraph.h:199
gtsam::tictoc_print_
void tictoc_print_()
Definition: timing.h:268
gtsam::Cal3Bundler
Calibration used by Bundler.
Definition: Cal3Bundler.h:32
bundlerPose
Definition: smartFactorScenarios.h:122
gtsam::LevenbergMarquardtOptimizer
Definition: LevenbergMarquardtOptimizer.h:35
L
MatrixXd L
Definition: LLT_example.cpp:6
gtsam::HybridValues::insert
void insert(Key j, const Vector &value)
Definition: HybridValues.cpp:85
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
vanillaPose::Cameras
CameraSet< Camera > Cameras
Definition: smartFactorScenarios.h:76
gtsam::Cal3_S2::shared_ptr
std::shared_ptr< Cal3_S2 > shared_ptr
Definition: Cal3_S2.h:39
SmartFactor
SmartProjectionPoseFactor< Cal3_S2 > SmartFactor
Definition: ISAM2Example_SmartFactor.cpp:22
gtsam::symbol_shorthand::F
Key F(std::uint64_t j)
Definition: inference/Symbol.h:153
cameraId3
Key cameraId3
Definition: testSmartProjectionRigFactor.cpp:51
EXPECT_DOUBLES_EQUAL
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:161
serializationTestHelpers.h
TestResult
Definition: TestResult.h:26
E
DiscreteKey E(5, 2)
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:218
gtsam::SmartFactorBase::FBlocks
std::vector< MatrixZD, Eigen::aligned_allocator< MatrixZD > > FBlocks
Definition: SmartFactorBase.h:64
simple_graph::factor3
auto factor3
Definition: testJacobianFactor.cpp:210
cam2
static StereoCamera cam2(pose3, cal4ptr)
gtsam::HybridValues::at
Vector & at(Key j)
Definition: HybridValues.cpp:129
vanillaPose2
Definition: smartFactorScenarios.h:89
sphericalCamera
Definition: smartFactorScenarios.h:139
pose1
static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5))
vanilla::level_uv_right
static const Point2 level_uv_right
Definition: smartFactorScenarios.h:64
estimation_fixture::measurements
std::vector< double > measurements
Definition: testHybridEstimation.cpp:51
gtsam::FactorGraph::push_back
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:147
gtsam::SmartProjectionRigFactor::add
void add(const MEASUREMENT &measured, const Key &poseKey, const size_t &cameraId=0)
Definition: SmartProjectionRigFactor.h:120
gtsam::Values
Definition: Values.h:65
CHECK
#define CHECK(condition)
Definition: Test.h:108
cameraId2
Key cameraId2
Definition: testSmartProjectionRigFactor.cpp:50
p
float * p
Definition: Tutorial_Map_using.cpp:9
A1
static const double A1[]
Definition: expn.h:6
gtsam::Values::insert
void insert(Key j, const Value &val)
Definition: Values.cpp:156
bundlerPose::sharedBundlerK
static const std::shared_ptr< Cal3Bundler > sharedBundlerK(new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000))
gtsam::PinholeBase::pose
const Pose3 & pose() const
return pose, constant version
Definition: CalibratedCamera.h:154
gtsam::SmartProjectionParams::setEnableEPI
void setEnableEPI(bool enableEPI)
Definition: SmartFactorParams.h:109
Camera
PinholePose< Cal3_S2 > Camera
Definition: SFMExample_SmartFactor.cpp:38
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:41
gtsam::Point2Vector
std::vector< Point2, Eigen::aligned_allocator< Point2 > > Point2Vector
Definition: Point2.h:49
P
static double P[]
Definition: ellpe.c:68
gtsam::LevenbergMarquardtParams
Definition: LevenbergMarquardtParams.h:35
gtsam::Cal3_S2
The most common 5DOF 3D->2D calibration.
Definition: Cal3_S2.h:34
gtsam::TriangulationResult
Definition: triangulation.h:642
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
gtsam::SharedIsotropic
noiseModel::Isotropic::shared_ptr SharedIsotropic
Definition: NoiseModel.h:766
gtsam::StereoCamera::project
StereoPoint2 project(const Point3 &point) const
Project 3D point to StereoPoint2 (uL,uR,v)
Definition: StereoCamera.cpp:32
main
int main()
Definition: testSmartProjectionRigFactor.cpp:1597
gtsam::SmartProjectionParams::setLinearizationMode
void setLinearizationMode(LinearizationMode linMode)
Definition: SmartFactorParams.h:97
gtsam::SmartProjectionPoseFactor::shared_ptr
std::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
Definition: SmartProjectionPoseFactor.h:59
vanilla::level_uv
static const Point2 level_uv
Definition: smartFactorScenarios.h:63
M_PI
#define M_PI
Definition: mconf.h:117
TestProjectionFactor
GenericProjectionFactor< Pose3, Point3 > TestProjectionFactor
Definition: testSmartProjectionRigFactor.cpp:965
gtsam::FactorGraph::add
IsDerived< DERIVEDFACTOR > add(std::shared_ptr< DERIVEDFACTOR > factor)
add is a synonym for push_back.
Definition: FactorGraph.h:171
gtsam::PinholePose
Definition: PinholePose.h:239
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
vanillaRig::params
SmartProjectionParams params(gtsam::HESSIAN, gtsam::ZERO_ON_DEGENERACY)
ceres::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: gtsam/3rdparty/ceres/eigen.h:38
smartFactorScenarios.h
gtsam::EmptyCal::shared_ptr
std::shared_ptr< EmptyCal > shared_ptr
Definition: SphericalCamera.h:47
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
R
Rot2 R(Rot2::fromAngle(0.1))
gtsam::GaussianFactorGraph::add
void add(const GaussianFactor &factor)
Definition: GaussianFactorGraph.h:125
simple_graph::factor1
auto factor1
Definition: testJacobianFactor.cpp:196
projectToMultipleCameras
void projectToMultipleCameras(const CAMERA &cam1, const CAMERA &cam2, const CAMERA &cam3, Point3 landmark, typename CAMERA::MeasurementVector &measurements_cam)
Definition: smartFactorScenarios.h:162
model
static SharedIsotropic model(noiseModel::Isotropic::Sigma(2, sigma))
gtsam::SmartProjectionRigFactor
Definition: SmartProjectionRigFactor.h:52
gtsam::Symbol
Definition: inference/Symbol.h:37
gtsam::PinholeBase::MeasurementVector
Point2Vector MeasurementVector
Definition: CalibratedCamera.h:67


gtsam
Author(s):
autogenerated on Sun Dec 22 2024 04:17:25