testSmartProjectionPoseFactor.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/ProjectionFactor.h>
24 #include <gtsam/slam/PoseTranslationPrior.h>
25 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
26 #include <gtsam/base/numericalDerivative.h>
27 #include <CppUnitLite/TestHarness.h>
28 #include <iostream>
29 
30 using namespace std::placeholders;
31 
32 namespace {
33 static const double rankTol = 1.0;
34 // Create a noise model for the pixel error
35 static const double sigma = 0.1;
36 static SharedIsotropic model(noiseModel::Isotropic::Sigma(2, sigma));
37 
38 // Convenience for named keys
39 using symbol_shorthand::X;
40 using symbol_shorthand::L;
41 
42 // tests data
43 static Symbol x1('X', 1);
44 static Symbol x2('X', 2);
45 static Symbol x3('X', 3);
46 
47 static Point2 measurement1(323.0, 240.0);
48 
49 LevenbergMarquardtParams lmParams;
50 // Make more verbose like so (in tests):
51 // lmParams.verbosityLM = LevenbergMarquardtParams::SUMMARY;
52 }
53 
54 /* ************************************************************************* */
55 TEST( SmartProjectionPoseFactor, Constructor) {
56  using namespace vanillaPose;
57  SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK));
58 }
59 
60 /* ************************************************************************* */
61 TEST( SmartProjectionPoseFactor, Constructor2) {
62  using namespace vanillaPose;
63  SmartProjectionParams params;
64  params.setRankTolerance(rankTol);
65  SmartFactor factor1(model, sharedK, params);
66 }
67 
68 /* ************************************************************************* */
69 TEST( SmartProjectionPoseFactor, Constructor3) {
70  using namespace vanillaPose;
71  SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK));
72  factor1->add(measurement1, x1);
73 }
74 
75 /* ************************************************************************* */
76 TEST( SmartProjectionPoseFactor, Constructor4) {
77  using namespace vanillaPose;
78  SmartProjectionParams params;
79  params.setRankTolerance(rankTol);
80  SmartFactor factor1(model, sharedK, params);
81  factor1.add(measurement1, x1);
82 }
83 
84 /* ************************************************************************* */
85 TEST( SmartProjectionPoseFactor, params) {
86  using namespace vanillaPose;
87  SmartProjectionParams params;
88  double rt = params.getRetriangulationThreshold();
89  EXPECT_DOUBLES_EQUAL(1e-5, rt, 1e-7);
90  params.setRetriangulationThreshold(1e-3);
91  rt = params.getRetriangulationThreshold();
92  EXPECT_DOUBLES_EQUAL(1e-3, rt, 1e-7);
93 }
94 
95 /* ************************************************************************* */
96 TEST( SmartProjectionPoseFactor, Equals ) {
97  using namespace vanillaPose;
98  SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK));
99  factor1->add(measurement1, x1);
100 
101  SmartFactor::shared_ptr factor2(new SmartFactor(model,sharedK));
102  factor2->add(measurement1, x1);
103 
104  CHECK(assert_equal(*factor1, *factor2));
105 }
106 
107 /* *************************************************************************/
108 TEST( SmartProjectionPoseFactor, noiseless ) {
109 
110  using namespace vanillaPose;
111 
112  // Project two landmarks into two cameras
113  Point2 level_uv = level_camera.project(landmark1);
114  Point2 level_uv_right = level_camera_right.project(landmark1);
115 
116  SmartFactor factor(model, sharedK);
117  factor.add(level_uv, x1);
118  factor.add(level_uv_right, x2);
119 
120  Values values; // it's a pose factor, hence these are poses
121  values.insert(x1, cam1.pose());
122  values.insert(x2, cam2.pose());
123 
124  double actualError = factor.error(values);
125  double expectedError = 0.0;
126  EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7);
127 
128  SmartFactor::Cameras cameras = factor.cameras(values);
129  double actualError2 = factor.totalReprojectionError(cameras);
130  EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7);
131 
132  // Calculate expected derivative for point (easiest to check)
133  std::function<Vector(Point3)> f = //
134  std::bind(&SmartFactor::whitenedError<Point3>, factor, cameras, std::placeholders::_1);
135 
136  // Calculate using computeEP
137  Matrix actualE;
138  factor.triangulateAndComputeE(actualE, values);
139 
140  // get point
141  auto point = factor.point();
142  CHECK(point);
143 
144  // calculate numerical derivative with triangulated point
145  Matrix expectedE = sigma * numericalDerivative11<Vector, Point3>(f, *point);
146  EXPECT(assert_equal(expectedE, actualE, 1e-7));
147 
148  // Calculate using reprojectionError
149  SmartFactor::Cameras::FBlocks F;
150  Matrix E;
151  Vector actualErrors = factor.unwhitenedError(cameras, *point, F, E);
152  EXPECT(assert_equal(expectedE, E, 1e-7));
153 
154  EXPECT(assert_equal(Z_4x1, actualErrors, 1e-7));
155 
156  // Calculate using computeJacobians
157  Vector b;
158  SmartFactor::FBlocks Fs;
159  factor.computeJacobians(Fs, E, b, cameras, *point);
160  double actualError3 = b.squaredNorm();
161  EXPECT(assert_equal(expectedE, E, 1e-7));
162  EXPECT_DOUBLES_EQUAL(expectedError, actualError3, 1e-6);
163 }
164 
165 /* *************************************************************************/
166 TEST( SmartProjectionPoseFactor, noisy ) {
167 
168  using namespace vanillaPose;
169 
170  // Project two landmarks into two cameras
171  Point2 pixelError(0.2, 0.2);
172  Point2 level_uv = level_camera.project(landmark1) + pixelError;
173  Point2 level_uv_right = level_camera_right.project(landmark1);
174 
175  Values values;
176  values.insert(x1, cam1.pose());
177  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10),
178  Point3(0.5, 0.1, 0.3));
179  values.insert(x2, pose_right.compose(noise_pose));
180 
181  SmartFactor::shared_ptr factor(new SmartFactor(model, sharedK));
182  factor->add(level_uv, x1);
183  factor->add(level_uv_right, x2);
184 
185  double actualError1 = factor->error(values);
186 
187  SmartFactor::shared_ptr factor2(new SmartFactor(model, sharedK));
188  Point2Vector measurements;
189  measurements.push_back(level_uv);
190  measurements.push_back(level_uv_right);
191 
192  KeyVector views {x1, x2};
193 
194  factor2->add(measurements, views);
195  double actualError2 = factor2->error(values);
196  DOUBLES_EQUAL(actualError1, actualError2, 1e-7);
197 }
198 
199 /* *************************************************************************/
200 TEST(SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform) {
201  using namespace vanillaPose;
202 
203  // create arbitrary body_T_sensor (transforms from sensor to body)
204  Pose3 body_T_sensor = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(1, 1, 1));
205 
206  // These are the poses we want to estimate, from camera measurements
207  const Pose3 sensor_T_body = body_T_sensor.inverse();
208  Pose3 wTb1 = cam1.pose() * sensor_T_body;
209  Pose3 wTb2 = cam2.pose() * sensor_T_body;
210  Pose3 wTb3 = cam3.pose() * sensor_T_body;
211 
212  // three landmarks ~5 meters infront of camera
213  Point3 landmark1(5, 0.5, 1.2), landmark2(5, -0.5, 1.2), landmark3(5, 0, 3.0);
214 
215  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
216 
217  // Project three landmarks into three cameras
218  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
219  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
220  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
221 
222  // Create smart factors
223  KeyVector views {x1, x2, x3};
224 
225  SmartProjectionParams params;
226  params.setRankTolerance(1.0);
227  params.setDegeneracyMode(IGNORE_DEGENERACY);
228  params.setEnableEPI(false);
229 
230  SmartFactor smartFactor1(model, sharedK, body_T_sensor, params);
231  smartFactor1.add(measurements_cam1, views);
232 
233  SmartFactor smartFactor2(model, sharedK, body_T_sensor, params);
234  smartFactor2.add(measurements_cam2, views);
235 
236  SmartFactor smartFactor3(model, sharedK, body_T_sensor, params);
237  smartFactor3.add(measurements_cam3, views);
238 
239  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
240 
241  // Put all factors in factor graph, adding priors
242  NonlinearFactorGraph graph;
243  graph.push_back(smartFactor1);
244  graph.push_back(smartFactor2);
245  graph.push_back(smartFactor3);
246  graph.addPrior(x1, wTb1, noisePrior);
247  graph.addPrior(x2, wTb2, noisePrior);
248 
249  // Check errors at ground truth poses
250  Values gtValues;
251  gtValues.insert(x1, wTb1);
252  gtValues.insert(x2, wTb2);
253  gtValues.insert(x3, wTb3);
254  double actualError = graph.error(gtValues);
255  double expectedError = 0.0;
256  DOUBLES_EQUAL(expectedError, actualError, 1e-7)
257 
258  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
259  Point3(0.1, 0.1, 0.1));
260  Values values;
261  values.insert(x1, wTb1);
262  values.insert(x2, wTb2);
263  // initialize third pose with some noise, we expect it to move back to
264  // original pose3
265  values.insert(x3, wTb3 * noise_pose);
266 
267  LevenbergMarquardtParams lmParams;
268  Values result;
269  LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
270  result = optimizer.optimize();
271  EXPECT(assert_equal(wTb3, result.at<Pose3>(x3)));
272 }
273 
274 /* *************************************************************************/
275 TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) {
276 
277  using namespace vanillaPose2;
278  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
279 
280  // Project three landmarks into three cameras
281  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
282  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
283  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
284 
285  KeyVector views;
286  views.push_back(x1);
287  views.push_back(x2);
288  views.push_back(x3);
289 
290  SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK2));
291  smartFactor1->add(measurements_cam1, views);
292 
293  SmartFactor::shared_ptr smartFactor2(new SmartFactor(model, sharedK2));
294  smartFactor2->add(measurements_cam2, views);
295 
296  SmartFactor::shared_ptr smartFactor3(new SmartFactor(model, sharedK2));
297  smartFactor3->add(measurements_cam3, views);
298 
299  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
300 
301  NonlinearFactorGraph graph;
302  graph.push_back(smartFactor1);
303  graph.push_back(smartFactor2);
304  graph.push_back(smartFactor3);
305  graph.addPrior(x1, cam1.pose(), noisePrior);
306  graph.addPrior(x2, cam2.pose(), noisePrior);
307 
308  Values groundTruth;
309  groundTruth.insert(x1, cam1.pose());
310  groundTruth.insert(x2, cam2.pose());
311  groundTruth.insert(x3, cam3.pose());
312  DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9);
313 
314  // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
315  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
316  Point3(0.1, 0.1, 0.1)); // smaller noise
317  Values values;
318  values.insert(x1, cam1.pose());
319  values.insert(x2, cam2.pose());
320  // initialize third pose with some noise, we expect it to move back to original pose_above
321  values.insert(x3, pose_above * noise_pose);
322  EXPECT(
323  assert_equal(
324  Pose3(
325  Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598,
326  -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598),
327  Point3(0.1, -0.1, 1.9)), values.at<Pose3>(x3)));
328 
329  Values result;
330  LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
331  result = optimizer.optimize();
332  EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-6));
333 }
334 
335 /* *************************************************************************/
336 TEST( SmartProjectionPoseFactor, Factors ) {
337 
338  using namespace vanillaPose;
339 
340  // Default cameras for simple derivatives
341  Rot3 R;
342  static Cal3_S2::shared_ptr sharedK(new Cal3_S2(100, 100, 0, 0, 0));
343  Camera cam1(Pose3(R, Point3(0, 0, 0)), sharedK), cam2(
344  Pose3(R, Point3(1, 0, 0)), sharedK);
345 
346  // one landmarks 1m in front of camera
347  Point3 landmark1(0, 0, 10);
348 
349  Point2Vector measurements_cam1;
350 
351  // Project 2 landmarks into 2 cameras
352  measurements_cam1.push_back(cam1.project(landmark1));
353  measurements_cam1.push_back(cam2.project(landmark1));
354 
355  // Create smart factors
356  KeyVector views {x1, x2};
357 
358  SmartFactor::shared_ptr smartFactor1 = std::make_shared<SmartFactor>(model, sharedK);
359  smartFactor1->add(measurements_cam1, views);
360 
361  SmartFactor::Cameras cameras;
362  cameras.push_back(cam1);
363  cameras.push_back(cam2);
364 
365  // Make sure triangulation works
366  CHECK(smartFactor1->triangulateSafe(cameras));
367  CHECK(!smartFactor1->isDegenerate());
368  CHECK(!smartFactor1->isPointBehindCamera());
369  auto p = smartFactor1->point();
370  CHECK(p);
371  EXPECT(assert_equal(landmark1, *p));
372 
373  VectorValues zeroDelta;
374  Vector6 delta;
375  delta.setZero();
376  zeroDelta.insert(x1, delta);
377  zeroDelta.insert(x2, delta);
378 
379  VectorValues perturbedDelta;
380  delta.setOnes();
381  perturbedDelta.insert(x1, delta);
382  perturbedDelta.insert(x2, delta);
383  double expectedError = 2500;
384 
385  // After eliminating the point, A1 and A2 contain 2-rank information on cameras:
386  Matrix16 A1, A2;
387  A1 << -10, 0, 0, 0, 1, 0;
388  A2 << 10, 0, 1, 0, -1, 0;
389  A1 *= 10. / sigma;
390  A2 *= 10. / sigma;
391  Matrix expectedInformation; // filled below
392  {
393  // createHessianFactor
394  Matrix66 G11 = 0.5 * A1.transpose() * A1;
395  Matrix66 G12 = 0.5 * A1.transpose() * A2;
396  Matrix66 G22 = 0.5 * A2.transpose() * A2;
397 
398  Vector6 g1;
399  g1.setZero();
400  Vector6 g2;
401  g2.setZero();
402 
403  double f = 0;
404 
405  RegularHessianFactor<6> expected(x1, x2, G11, G12, g1, G22, g2, f);
406  expectedInformation = expected.information();
407 
408  std::shared_ptr<RegularHessianFactor<6> > actual =
409  smartFactor1->createHessianFactor(cameras, 0.0);
410  EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6));
411  EXPECT(assert_equal(expected, *actual, 1e-6));
412  EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6);
413  EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6);
414  }
415 
416  {
417  Matrix26 F1;
418  F1.setZero();
419  F1(0, 1) = -100;
420  F1(0, 3) = -10;
421  F1(1, 0) = 100;
422  F1(1, 4) = -10;
423  Matrix26 F2;
424  F2.setZero();
425  F2(0, 1) = -101;
426  F2(0, 3) = -10;
427  F2(0, 5) = -1;
428  F2(1, 0) = 100;
429  F2(1, 2) = 10;
430  F2(1, 4) = -10;
431  Matrix E(4, 3);
432  E.setZero();
433  E(0, 0) = 10;
434  E(1, 1) = 10;
435  E(2, 0) = 10;
436  E(2, 2) = 1;
437  E(3, 1) = 10;
438  SmartFactor::FBlocks Fs {F1, F2};
439  Vector b(4);
440  b.setZero();
441 
442  // Create smart factors
443  KeyVector keys;
444  keys.push_back(x1);
445  keys.push_back(x2);
446 
447  // createJacobianQFactor
448  SharedIsotropic n = noiseModel::Isotropic::Sigma(4, sigma);
449  Matrix3 P = (E.transpose() * E).inverse();
450  JacobianFactorQ<6, 2> expectedQ(keys, Fs, E, P, b, n);
451  EXPECT(assert_equal(expectedInformation, expectedQ.information(), 1e-6));
452 
453  std::shared_ptr<JacobianFactorQ<6, 2> > actualQ =
454  smartFactor1->createJacobianQFactor(cameras, 0.0);
455  CHECK(actualQ);
456  EXPECT(assert_equal(expectedInformation, actualQ->information(), 1e-6));
457  EXPECT(assert_equal(expectedQ, *actualQ));
458  EXPECT_DOUBLES_EQUAL(0, actualQ->error(zeroDelta), 1e-6);
459  EXPECT_DOUBLES_EQUAL(expectedError, actualQ->error(perturbedDelta), 1e-6);
460 
461  // Whiten for RegularImplicitSchurFactor (does not have noise model)
462  model->WhitenSystem(E, b);
463  Matrix3 whiteP = (E.transpose() * E).inverse();
464  Fs[0] = model->Whiten(Fs[0]);
465  Fs[1] = model->Whiten(Fs[1]);
466 
467  // createRegularImplicitSchurFactor
468  RegularImplicitSchurFactor<Camera> expected(keys, Fs, E, whiteP, b);
469 
470  std::shared_ptr<RegularImplicitSchurFactor<Camera> > actual =
471  smartFactor1->createRegularImplicitSchurFactor(cameras, 0.0);
472  CHECK(actual);
473  EXPECT(assert_equal(expectedInformation, expected.information(), 1e-6));
474  EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6));
475  EXPECT(assert_equal(expected, *actual));
476  EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6);
477  EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6);
478  }
479 
480  {
481  // createJacobianSVDFactor
482  Vector1 b;
483  b.setZero();
484  double s = sigma * sin(M_PI_4);
485  SharedIsotropic n = noiseModel::Isotropic::Sigma(4 - 3, sigma);
486  JacobianFactor expected(x1, s * A1, x2, s * A2, b, n);
487  EXPECT(assert_equal(expectedInformation, expected.information(), 1e-6));
488 
489  std::shared_ptr<JacobianFactor> actual =
490  smartFactor1->createJacobianSVDFactor(cameras, 0.0);
491  CHECK(actual);
492  EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6));
493  EXPECT(assert_equal(expected, *actual));
494  EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6);
495  EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6);
496  }
497 }
498 
499 /* *************************************************************************/
500 TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) {
501 
502  using namespace vanillaPose;
503 
504  KeyVector views {x1, x2, x3};
505 
506  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
507 
508  // Project three landmarks into three cameras
509  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
510  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
511  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
512 
513  SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK));
514  smartFactor1->add(measurements_cam1, views);
515 
516  SmartFactor::shared_ptr smartFactor2(new SmartFactor(model, sharedK));
517  smartFactor2->add(measurements_cam2, views);
518 
519  SmartFactor::shared_ptr smartFactor3(new SmartFactor(model, sharedK));
520  smartFactor3->add(measurements_cam3, views);
521 
522  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
523 
524  NonlinearFactorGraph graph;
525  graph.push_back(smartFactor1);
526  graph.push_back(smartFactor2);
527  graph.push_back(smartFactor3);
528  graph.addPrior(x1, cam1.pose(), noisePrior);
529  graph.addPrior(x2, cam2.pose(), noisePrior);
530 
531  // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
532  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
533  Point3(0.1, 0.1, 0.1)); // smaller noise
534  Values values;
535  values.insert(x1, cam1.pose());
536  values.insert(x2, cam2.pose());
537  // initialize third pose with some noise, we expect it to move back to original pose_above
538  values.insert(x3, pose_above * noise_pose);
539  EXPECT(
540  assert_equal(
541  Pose3(
542  Rot3(1.11022302e-16, -0.0314107591, 0.99950656, -0.99950656,
543  -0.0313952598, -0.000986635786, 0.0314107591, -0.999013364,
544  -0.0313952598), Point3(0.1, -0.1, 1.9)),
545  values.at<Pose3>(x3)));
546 
547  Values result;
548  LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
549  result = optimizer.optimize();
550  EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-7));
551 }
552 
553 /* *************************************************************************/
554 TEST( SmartProjectionPoseFactor, jacobianSVD ) {
555 
556  using namespace vanillaPose;
557 
558  KeyVector views {x1, x2, x3};
559 
560  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
561 
562  // Project three landmarks into three cameras
563  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
564  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
565  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
566 
567  SmartProjectionParams params;
568  params.setRankTolerance(1.0);
569  params.setLinearizationMode(gtsam::JACOBIAN_SVD);
570  params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY);
571  params.setEnableEPI(false);
572  SmartFactor factor1(model, sharedK, params);
573 
574  SmartFactor::shared_ptr smartFactor1(
575  new SmartFactor(model, sharedK, params));
576  smartFactor1->add(measurements_cam1, views);
577 
578  SmartFactor::shared_ptr smartFactor2(
579  new SmartFactor(model, sharedK, params));
580  smartFactor2->add(measurements_cam2, views);
581 
582  SmartFactor::shared_ptr smartFactor3(
583  new SmartFactor(model, sharedK, params));
584  smartFactor3->add(measurements_cam3, views);
585 
586  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
587 
588  NonlinearFactorGraph graph;
589  graph.push_back(smartFactor1);
590  graph.push_back(smartFactor2);
591  graph.push_back(smartFactor3);
592  graph.addPrior(x1, cam1.pose(), noisePrior);
593  graph.addPrior(x2, cam2.pose(), noisePrior);
594 
595  // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
596  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
597  Point3(0.1, 0.1, 0.1)); // smaller noise
598  Values values;
599  values.insert(x1, cam1.pose());
600  values.insert(x2, cam2.pose());
601  values.insert(x3, pose_above * noise_pose);
602 
603  Values result;
604  LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
605  result = optimizer.optimize();
606  EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-6));
607 }
608 
609 /* *************************************************************************/
610 TEST( SmartProjectionPoseFactor, landmarkDistance ) {
611 
612  using namespace vanillaPose;
613 
614  double excludeLandmarksFutherThanDist = 2;
615 
616  KeyVector views {x1, x2, x3};
617 
618  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
619 
620  // Project three landmarks into three cameras
621  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
622  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
623  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
624 
625  SmartProjectionParams params;
626  params.setRankTolerance(1.0);
627  params.setLinearizationMode(gtsam::JACOBIAN_SVD);
628  params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY);
629  params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist);
630  params.setEnableEPI(false);
631 
632  SmartFactor::shared_ptr smartFactor1(
633  new SmartFactor(model, sharedK, params));
634  smartFactor1->add(measurements_cam1, views);
635 
636  SmartFactor::shared_ptr smartFactor2(
637  new SmartFactor(model, sharedK, params));
638  smartFactor2->add(measurements_cam2, views);
639 
640  SmartFactor::shared_ptr smartFactor3(
641  new SmartFactor(model, sharedK, params));
642  smartFactor3->add(measurements_cam3, views);
643 
644  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
645 
646  NonlinearFactorGraph graph;
647  graph.push_back(smartFactor1);
648  graph.push_back(smartFactor2);
649  graph.push_back(smartFactor3);
650  graph.addPrior(x1, cam1.pose(), noisePrior);
651  graph.addPrior(x2, cam2.pose(), noisePrior);
652 
653  // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
654  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
655  Point3(0.1, 0.1, 0.1)); // smaller noise
656  Values values;
657  values.insert(x1, cam1.pose());
658  values.insert(x2, cam2.pose());
659  values.insert(x3, pose_above * noise_pose);
660 
661  // All factors are disabled and pose should remain where it is
662  Values result;
663  LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
664  result = optimizer.optimize();
665  EXPECT(assert_equal(values.at<Pose3>(x3), result.at<Pose3>(x3)));
666 }
667 
668 /* *************************************************************************/
669 TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) {
670 
671  using namespace vanillaPose;
672 
673  double excludeLandmarksFutherThanDist = 1e10;
674  double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error
675 
676  KeyVector views {x1, x2, x3};
677 
678  // add fourth landmark
679  Point3 landmark4(5, -0.5, 1);
680 
681  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3,
682  measurements_cam4;
683 
684  // Project 4 landmarks into three cameras
685  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
686  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
687  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
688  projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4);
689  measurements_cam4.at(0) = measurements_cam4.at(0) + Point2(10, 10); // add outlier
690 
691  SmartProjectionParams params;
692  params.setLinearizationMode(gtsam::JACOBIAN_SVD);
693  params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist);
694  params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold);
695 
696  SmartFactor::shared_ptr smartFactor1(
697  new SmartFactor(model, sharedK, params));
698  smartFactor1->add(measurements_cam1, views);
699 
700  SmartFactor::shared_ptr smartFactor2(
701  new SmartFactor(model, sharedK, params));
702  smartFactor2->add(measurements_cam2, views);
703 
704  SmartFactor::shared_ptr smartFactor3(
705  new SmartFactor(model, sharedK, params));
706  smartFactor3->add(measurements_cam3, views);
707 
708  SmartFactor::shared_ptr smartFactor4(
709  new SmartFactor(model, sharedK, params));
710  smartFactor4->add(measurements_cam4, views);
711 
712  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
713 
714  NonlinearFactorGraph graph;
715  graph.push_back(smartFactor1);
716  graph.push_back(smartFactor2);
717  graph.push_back(smartFactor3);
718  graph.push_back(smartFactor4);
719  graph.addPrior(x1, cam1.pose(), noisePrior);
720  graph.addPrior(x2, cam2.pose(), noisePrior);
721 
722  Values values;
723  values.insert(x1, cam1.pose());
724  values.insert(x2, cam2.pose());
725  values.insert(x3, cam3.pose());
726 
727  // All factors are disabled and pose should remain where it is
728  Values result;
729  LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
730  result = optimizer.optimize();
731  EXPECT(assert_equal(cam3.pose(), result.at<Pose3>(x3)));
732 }
733 
734 /* *************************************************************************/
735 TEST( SmartProjectionPoseFactor, jacobianQ ) {
736 
737  using namespace vanillaPose;
738 
739  KeyVector views {x1, x2, x3};
740 
741  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
742 
743  // Project three landmarks into three cameras
744  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
745  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
746  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
747 
748  SmartProjectionParams params;
749  params.setLinearizationMode(gtsam::JACOBIAN_Q);
750 
751  SmartFactor::shared_ptr smartFactor1(
752  new SmartFactor(model, sharedK, params));
753  smartFactor1->add(measurements_cam1, views);
754 
755  SmartFactor::shared_ptr smartFactor2(
756  new SmartFactor(model, sharedK, params));
757  smartFactor2->add(measurements_cam2, views);
758 
759  SmartFactor::shared_ptr smartFactor3(
760  new SmartFactor(model, sharedK, params));
761  smartFactor3->add(measurements_cam3, views);
762 
763  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
764 
765  NonlinearFactorGraph graph;
766  graph.push_back(smartFactor1);
767  graph.push_back(smartFactor2);
768  graph.push_back(smartFactor3);
769  graph.addPrior(x1, cam1.pose(), noisePrior);
770  graph.addPrior(x2, cam2.pose(), noisePrior);
771 
772  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
773  Point3(0.1, 0.1, 0.1)); // smaller noise
774  Values values;
775  values.insert(x1, cam1.pose());
776  values.insert(x2, cam2.pose());
777  values.insert(x3, pose_above * noise_pose);
778 
779  Values result;
780  LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
781  result = optimizer.optimize();
782  EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-6));
783 }
784 
785 /* *************************************************************************/
786 TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) {
787 
788  using namespace vanillaPose2;
789 
790  KeyVector views {x1, x2, x3};
791 
792  typedef GenericProjectionFactor<Pose3, Point3> ProjectionFactor;
793  NonlinearFactorGraph graph;
794 
795  // Project three landmarks into three cameras
796  graph.emplace_shared<ProjectionFactor>(cam1.project(landmark1), model, x1, L(1), sharedK2);
797  graph.emplace_shared<ProjectionFactor>(cam2.project(landmark1), model, x2, L(1), sharedK2);
798  graph.emplace_shared<ProjectionFactor>(cam3.project(landmark1), model, x3, L(1), sharedK2);
799 
800  graph.emplace_shared<ProjectionFactor>(cam1.project(landmark2), model, x1, L(2), sharedK2);
801  graph.emplace_shared<ProjectionFactor>(cam2.project(landmark2), model, x2, L(2), sharedK2);
802  graph.emplace_shared<ProjectionFactor>(cam3.project(landmark2), model, x3, L(2), sharedK2);
803 
804  graph.emplace_shared<ProjectionFactor>(cam1.project(landmark3), model, x1, L(3), sharedK2);
805  graph.emplace_shared<ProjectionFactor>(cam2.project(landmark3), model, x2, L(3), sharedK2);
806  graph.emplace_shared<ProjectionFactor>(cam3.project(landmark3), model, x3, L(3), sharedK2);
807 
808  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
809  graph.addPrior(x1, level_pose, noisePrior);
810  graph.addPrior(x2, pose_right, noisePrior);
811 
812  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10),
813  Point3(0.5, 0.1, 0.3));
814  Values values;
815  values.insert(x1, level_pose);
816  values.insert(x2, pose_right);
817  values.insert(x3, pose_above * noise_pose);
818  values.insert(L(1), landmark1);
819  values.insert(L(2), landmark2);
820  values.insert(L(3), landmark3);
821 
822  DOUBLES_EQUAL(48406055, graph.error(values), 1);
823 
824  LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
825  Values result = optimizer.optimize();
826 
827  DOUBLES_EQUAL(0, graph.error(result), 1e-9);
828 
829  EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-7));
830 }
831 
832 /* *************************************************************************/
833 TEST( SmartProjectionPoseFactor, CheckHessian) {
834 
835  KeyVector views {x1, x2, x3};
836 
837  using namespace vanillaPose;
838 
839  // Two slightly different cameras
840  Pose3 pose2 = level_pose
841  * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
842  Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
843  Camera cam2(pose2, sharedK);
844  Camera cam3(pose3, sharedK);
845 
846  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
847 
848  // Project three landmarks into three cameras
849  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
850  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
851  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
852 
853  SmartProjectionParams params;
854  params.setRankTolerance(10);
855 
856  SmartFactor::shared_ptr smartFactor1(
857  new SmartFactor(model, sharedK, params)); // HESSIAN, by default
858  smartFactor1->add(measurements_cam1, views);
859 
860  SmartFactor::shared_ptr smartFactor2(
861  new SmartFactor(model, sharedK, params)); // HESSIAN, by default
862  smartFactor2->add(measurements_cam2, views);
863 
864  SmartFactor::shared_ptr smartFactor3(
865  new SmartFactor(model, sharedK, params)); // HESSIAN, by default
866  smartFactor3->add(measurements_cam3, views);
867 
868  NonlinearFactorGraph graph;
869  graph.push_back(smartFactor1);
870  graph.push_back(smartFactor2);
871  graph.push_back(smartFactor3);
872 
873  // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
874  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
875  Point3(0.1, 0.1, 0.1)); // smaller noise
876  Values values;
877  values.insert(x1, cam1.pose());
878  values.insert(x2, cam2.pose());
879  // initialize third pose with some noise, we expect it to move back to original pose_above
880  values.insert(x3, pose3 * noise_pose);
881  EXPECT(
882  assert_equal(
883  Pose3(
884  Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265,
885  -0.130426831, -0.0115837907, 0.130819108, -0.98278564,
886  -0.130455917),
887  Point3(0.0897734171, -0.110201006, 0.901022872)),
888  values.at<Pose3>(x3)));
889 
890  std::shared_ptr<GaussianFactor> factor1 = smartFactor1->linearize(values);
891  std::shared_ptr<GaussianFactor> factor2 = smartFactor2->linearize(values);
892  std::shared_ptr<GaussianFactor> factor3 = smartFactor3->linearize(values);
893 
894  Matrix CumulativeInformation = factor1->information() + factor2->information()
895  + factor3->information();
896 
897  std::shared_ptr<GaussianFactorGraph> GaussianGraph = graph.linearize(
898  values);
899  Matrix GraphInformation = GaussianGraph->hessian().first;
900 
901  // Check Hessian
902  EXPECT(assert_equal(GraphInformation, CumulativeInformation, 1e-6));
903 
904  Matrix AugInformationMatrix = factor1->augmentedInformation()
905  + factor2->augmentedInformation() + factor3->augmentedInformation();
906 
907  // Check Information vector
908  Vector InfoVector = AugInformationMatrix.block(0, 18, 18, 1); // 18x18 Hessian + information vector
909 
910  // Check Hessian
911  EXPECT(assert_equal(InfoVector, GaussianGraph->hessian().second, 1e-6));
912 }
913 
914 /* *************************************************************************/
915 TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_factor ) {
916  using namespace vanillaPose2;
917 
918  KeyVector views {x1, x2, x3};
919 
920  // Two different cameras, at the same position, but different rotations
921  Pose3 pose2 = level_pose * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0));
922  Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0));
923  Camera cam2(pose2, sharedK2);
924  Camera cam3(pose3, sharedK2);
925 
926  Point2Vector measurements_cam1, measurements_cam2;
927 
928  // Project three landmarks into three cameras
929  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
930  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
931 
932  SmartProjectionParams params;
933  params.setRankTolerance(50);
934  params.setDegeneracyMode(gtsam::HANDLE_INFINITY);
935 
936  SmartFactor::shared_ptr smartFactor1(
937  new SmartFactor(model, sharedK2, params));
938  smartFactor1->add(measurements_cam1, views);
939 
940  SmartFactor::shared_ptr smartFactor2(
941  new SmartFactor(model, sharedK2, params));
942  smartFactor2->add(measurements_cam2, views);
943 
944  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
945  const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10);
946  Point3 positionPrior = Point3(0, 0, 1);
947 
948  NonlinearFactorGraph graph;
949  graph.push_back(smartFactor1);
950  graph.push_back(smartFactor2);
951  graph.addPrior(x1, cam1.pose(), noisePrior);
952  graph.emplace_shared<PoseTranslationPrior<Pose3> >(x2, positionPrior, noisePriorTranslation);
953  graph.emplace_shared<PoseTranslationPrior<Pose3> >(x3, positionPrior, noisePriorTranslation);
954 
955  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
956  Point3(0.1, 0.1, 0.1)); // smaller noise
957  Values values;
958  values.insert(x1, cam1.pose());
959  values.insert(x2, pose2 * noise_pose);
960  values.insert(x3, pose3 * noise_pose);
961 
962  // params.verbosityLM = LevenbergMarquardtParams::SUMMARY;
963  LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
964  Values result = optimizer.optimize();
965  EXPECT(assert_equal(pose3, result.at<Pose3>(x3)));
966 }
967 
968 /* *************************************************************************/
969 TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) {
970 
971  // this test considers a condition in which the cheirality constraint is triggered
972  using namespace vanillaPose;
973 
974  KeyVector views {x1, x2, x3};
975 
976  // Two different cameras, at the same position, but different rotations
977  Pose3 pose2 = level_pose
978  * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
979  Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
980  Camera cam2(pose2, sharedK);
981  Camera cam3(pose3, sharedK);
982 
983  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
984 
985  // Project three landmarks into three cameras
986  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
987  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
988  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
989 
990  SmartProjectionParams params;
991  params.setRankTolerance(10);
992  params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY);
993 
994  SmartFactor::shared_ptr smartFactor1(
995  new SmartFactor(model, sharedK, params));
996  smartFactor1->add(measurements_cam1, views);
997 
998  SmartFactor::shared_ptr smartFactor2(
999  new SmartFactor(model, sharedK, params));
1000  smartFactor2->add(measurements_cam2, views);
1001 
1002  SmartFactor::shared_ptr smartFactor3(
1003  new SmartFactor(model, sharedK, params));
1004  smartFactor3->add(measurements_cam3, views);
1005 
1006  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
1007  const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3,
1008  0.10);
1009  Point3 positionPrior = Point3(0, 0, 1);
1010 
1011  NonlinearFactorGraph graph;
1012  graph.push_back(smartFactor1);
1013  graph.push_back(smartFactor2);
1014  graph.push_back(smartFactor3);
1015  graph.addPrior(x1, cam1.pose(), noisePrior);
1016  graph.emplace_shared<PoseTranslationPrior<Pose3> >(x2, positionPrior, noisePriorTranslation);
1017  graph.emplace_shared<PoseTranslationPrior<Pose3> >(x3, positionPrior, noisePriorTranslation);
1018 
1019  // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
1020  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
1021  Point3(0.1, 0.1, 0.1)); // smaller noise
1022  Values values;
1023  values.insert(x1, cam1.pose());
1024  values.insert(x2, cam2.pose());
1025  values.insert(x3, pose3 * noise_pose);
1026  EXPECT(
1027  assert_equal(
1028  Pose3(
1029  Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265,
1030  -0.130426831, -0.0115837907, 0.130819108, -0.98278564,
1031  -0.130455917),
1032  Point3(0.0897734171, -0.110201006, 0.901022872)),
1033  values.at<Pose3>(x3)));
1034 
1035  Values result;
1036  LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
1037  result = optimizer.optimize();
1038 
1039  // Since we do not do anything on degenerate instances (ZERO_ON_DEGENERACY)
1040  // rotation remains the same as the initial guess, but position is fixed by PoseTranslationPrior
1041 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
1042  EXPECT(assert_equal(Pose3(values.at<Pose3>(x3).rotation(),
1043  Point3(0,0,1)), result.at<Pose3>(x3)));
1044 #else
1045  // if the check is disabled, no cheirality exception if thrown and the pose converges to the right rotation
1046  // with modest accuracy since the configuration is essentially degenerate without the translation due to noise (noise_pose)
1047  EXPECT(assert_equal(pose3, result.at<Pose3>(x3),1e-3));
1048 #endif
1049 }
1050 
1051 /* *************************************************************************/
1052 TEST( SmartProjectionPoseFactor, Hessian ) {
1053 
1054  using namespace vanillaPose2;
1055 
1056  KeyVector views {x1, x2};
1057 
1058  // Project three landmarks into 2 cameras
1059  Point2 cam1_uv1 = cam1.project(landmark1);
1060  Point2 cam2_uv1 = cam2.project(landmark1);
1061  Point2Vector measurements_cam1;
1062  measurements_cam1.push_back(cam1_uv1);
1063  measurements_cam1.push_back(cam2_uv1);
1064 
1065  SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK2));
1066  smartFactor1->add(measurements_cam1, views);
1067 
1068  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10),
1069  Point3(0.5, 0.1, 0.3));
1070  Values values;
1071  values.insert(x1, cam1.pose());
1072  values.insert(x2, cam2.pose());
1073 
1074  std::shared_ptr<GaussianFactor> factor = smartFactor1->linearize(values);
1075 
1076  // compute triangulation from linearization point
1077  // compute reprojection errors (sum squared)
1078  // compare with factor.info(): the bottom right element is the squared sum of the reprojection errors (normalized by the covariance)
1079  // check that it is correctly scaled when using noiseProjection = [1/4 0; 0 1/4]
1080 }
1081 
1082 /* *************************************************************************/
1083 TEST( SmartProjectionPoseFactor, HessianWithRotation ) {
1084  // cout << " ************************ SmartProjectionPoseFactor: rotated Hessian **********************" << endl;
1085 
1086  using namespace vanillaPose;
1087 
1088  KeyVector views {x1, x2, x3};
1089 
1090  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
1091 
1092  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
1093 
1094  SmartFactor::shared_ptr smartFactorInstance(new SmartFactor(model, sharedK));
1095  smartFactorInstance->add(measurements_cam1, views);
1096 
1097  Values values;
1098  values.insert(x1, cam1.pose());
1099  values.insert(x2, cam2.pose());
1100  values.insert(x3, cam3.pose());
1101 
1102  std::shared_ptr<GaussianFactor> factor = smartFactorInstance->linearize(
1103  values);
1104 
1105  Pose3 poseDrift = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0));
1106 
1107  Values rotValues;
1108  rotValues.insert(x1, poseDrift.compose(level_pose));
1109  rotValues.insert(x2, poseDrift.compose(pose_right));
1110  rotValues.insert(x3, poseDrift.compose(pose_above));
1111 
1112  std::shared_ptr<GaussianFactor> factorRot = smartFactorInstance->linearize(
1113  rotValues);
1114 
1115  // Hessian is invariant to rotations in the nondegenerate case
1116  EXPECT(assert_equal(factor->information(), factorRot->information(), 1e-7));
1117 
1118  Pose3 poseDrift2 = Pose3(Rot3::Ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2),
1119  Point3(10, -4, 5));
1120 
1121  Values tranValues;
1122  tranValues.insert(x1, poseDrift2.compose(level_pose));
1123  tranValues.insert(x2, poseDrift2.compose(pose_right));
1124  tranValues.insert(x3, poseDrift2.compose(pose_above));
1125 
1126  std::shared_ptr<GaussianFactor> factorRotTran =
1127  smartFactorInstance->linearize(tranValues);
1128 
1129  // Hessian is invariant to rotations and translations in the nondegenerate case
1130  EXPECT(assert_equal(factor->information(), factorRotTran->information(), 1e-7));
1131 }
1132 
1133 /* *************************************************************************/
1134 TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) {
1135 
1136  using namespace vanillaPose2;
1137 
1138  KeyVector views {x1, x2, x3};
1139 
1140  // All cameras have the same pose so will be degenerate !
1141  Camera cam2(level_pose, sharedK2);
1142  Camera cam3(level_pose, sharedK2);
1143 
1144  Point2Vector measurements_cam1;
1145  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
1146 
1147  SmartFactor::shared_ptr smartFactor(new SmartFactor(model, sharedK2));
1148  smartFactor->add(measurements_cam1, views);
1149 
1150  Values values;
1151  values.insert(x1, cam1.pose());
1152  values.insert(x2, cam2.pose());
1153  values.insert(x3, cam3.pose());
1154 
1155  std::shared_ptr<GaussianFactor> factor = smartFactor->linearize(values);
1156 
1157  Pose3 poseDrift = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0));
1158 
1159  Values rotValues;
1160  rotValues.insert(x1, poseDrift.compose(level_pose));
1161  rotValues.insert(x2, poseDrift.compose(level_pose));
1162  rotValues.insert(x3, poseDrift.compose(level_pose));
1163 
1164  std::shared_ptr<GaussianFactor> factorRot = //
1165  smartFactor->linearize(rotValues);
1166 
1167  // Hessian is invariant to rotations in the nondegenerate case
1168  EXPECT(assert_equal(factor->information(), factorRot->information(), 1e-7));
1169 
1170  Pose3 poseDrift2 = Pose3(Rot3::Ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2),
1171  Point3(10, -4, 5));
1172 
1173  Values tranValues;
1174  tranValues.insert(x1, poseDrift2.compose(level_pose));
1175  tranValues.insert(x2, poseDrift2.compose(level_pose));
1176  tranValues.insert(x3, poseDrift2.compose(level_pose));
1177 
1178  std::shared_ptr<GaussianFactor> factorRotTran = smartFactor->linearize(
1179  tranValues);
1180 
1181  // Hessian is invariant to rotations and translations in the nondegenerate case
1182  EXPECT(assert_equal(factor->information(), factorRotTran->information(), 1e-7));
1183 }
1184 
1185 /* ************************************************************************* */
1186 TEST( SmartProjectionPoseFactor, ConstructorWithCal3Bundler) {
1187  using namespace bundlerPose;
1188  SmartProjectionParams params;
1189  params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY);
1190  SmartFactor factor(model, sharedBundlerK, params);
1191  factor.add(measurement1, x1);
1192 }
1193 
1194 /* *************************************************************************/
1195 TEST( SmartProjectionPoseFactor, Cal3Bundler ) {
1196 
1197  using namespace bundlerPose;
1198 
1199  // three landmarks ~5 meters in front of camera
1200  Point3 landmark3(3, 0, 3.0);
1201 
1202  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
1203 
1204  // Project three landmarks into three cameras
1205  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
1206  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
1207  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
1208 
1209  KeyVector views {x1, x2, x3};
1210 
1211  SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedBundlerK));
1212  smartFactor1->add(measurements_cam1, views);
1213 
1214  SmartFactor::shared_ptr smartFactor2(new SmartFactor(model, sharedBundlerK));
1215  smartFactor2->add(measurements_cam2, views);
1216 
1217  SmartFactor::shared_ptr smartFactor3(new SmartFactor(model, sharedBundlerK));
1218  smartFactor3->add(measurements_cam3, views);
1219 
1220  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
1221 
1222  NonlinearFactorGraph graph;
1223  graph.push_back(smartFactor1);
1224  graph.push_back(smartFactor2);
1225  graph.push_back(smartFactor3);
1226  graph.addPrior(x1, cam1.pose(), noisePrior);
1227  graph.addPrior(x2, cam2.pose(), noisePrior);
1228 
1229  // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
1230  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
1231  Point3(0.1, 0.1, 0.1)); // smaller noise
1232  Values values;
1233  values.insert(x1, cam1.pose());
1234  values.insert(x2, cam2.pose());
1235  // initialize third pose with some noise, we expect it to move back to original pose_above
1236  values.insert(x3, pose_above * noise_pose);
1237  EXPECT(
1238  assert_equal(
1239  Pose3(
1240  Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598,
1241  -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598),
1242  Point3(0.1, -0.1, 1.9)), values.at<Pose3>(x3)));
1243 
1244  Values result;
1245  LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
1246  result = optimizer.optimize();
1247  EXPECT(assert_equal(cam3.pose(), result.at<Pose3>(x3), 1e-6));
1248 }
1249 
1250 /* *************************************************************************/
1251 TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) {
1252 
1253  using namespace bundlerPose;
1254 
1255  KeyVector views {x1, x2, x3};
1256 
1257  // Two different cameras
1258  Pose3 pose2 = level_pose
1259  * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
1260  Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
1261  Camera cam2(pose2, sharedBundlerK);
1262  Camera cam3(pose3, sharedBundlerK);
1263 
1264  // landmark3 at 3 meters now
1265  Point3 landmark3(3, 0, 3.0);
1266 
1267  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
1268 
1269  // Project three landmarks into three cameras
1270  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
1271  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
1272  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
1273 
1274  SmartProjectionParams params;
1275  params.setRankTolerance(10);
1276  params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY);
1277 
1278  SmartFactor::shared_ptr smartFactor1(
1279  new SmartFactor(model, sharedBundlerK, params));
1280  smartFactor1->add(measurements_cam1, views);
1281 
1282  SmartFactor::shared_ptr smartFactor2(
1283  new SmartFactor(model, sharedBundlerK, params));
1284  smartFactor2->add(measurements_cam2, views);
1285 
1286  SmartFactor::shared_ptr smartFactor3(
1287  new SmartFactor(model, sharedBundlerK, params));
1288  smartFactor3->add(measurements_cam3, views);
1289 
1290  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
1291  const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3,
1292  0.10);
1293  Point3 positionPrior = Point3(0, 0, 1);
1294 
1295  NonlinearFactorGraph graph;
1296  graph.push_back(smartFactor1);
1297  graph.push_back(smartFactor2);
1298  graph.push_back(smartFactor3);
1299  graph.addPrior(x1, cam1.pose(), noisePrior);
1300  graph.emplace_shared<PoseTranslationPrior<Pose3> >(x2, positionPrior, noisePriorTranslation);
1301  graph.emplace_shared<PoseTranslationPrior<Pose3> >(x3, positionPrior, noisePriorTranslation);
1302 
1303  // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
1304  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
1305  Point3(0.1, 0.1, 0.1)); // smaller noise
1306  Values values;
1307  values.insert(x1, cam1.pose());
1308  values.insert(x2, cam2.pose());
1309  // initialize third pose with some noise, we expect it to move back to original pose_above
1310  values.insert(x3, pose3 * noise_pose);
1311  EXPECT(
1312  assert_equal(
1313  Pose3(
1314  Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265,
1315  -0.130426831, -0.0115837907, 0.130819108, -0.98278564,
1316  -0.130455917),
1317  Point3(0.0897734171, -0.110201006, 0.901022872)),
1318  values.at<Pose3>(x3)));
1319 
1320  Values result;
1321  LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
1322  result = optimizer.optimize();
1323 
1324  EXPECT(
1325  assert_equal(
1326  Pose3(
1327  Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265,
1328  -0.130426831, -0.0115837907, 0.130819108, -0.98278564,
1329  -0.130455917),
1330  Point3(0.0897734171, -0.110201006, 0.901022872)),
1331  values.at<Pose3>(x3)));
1332 }
1333 
1334 /* ************************************************************************* */
1335 int main() {
1336  TestResult tr;
1337  return TestRegistry::runAllTests(tr);
1338 }
1339 /* ************************************************************************* */
1340 
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
cam1
static PinholeCamera< Cal3_S2 > cam1(pose3, cal1)
pose3
Definition: testFrobeniusFactor.cpp:229
PoseTranslationPrior.h
Implements a prior on the translation component of a pose.
gtsam::NonlinearOptimizer::optimize
virtual const Values & optimize()
Definition: NonlinearOptimizer.h:98
gtsam::StereoCamera::pose
const Pose3 & pose() const
pose
Definition: StereoCamera.h:127
ProjectionFactor.h
Reprojection of a LANDMARK to a 2D point.
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
simple_graph::factor2
auto factor2
Definition: testJacobianFactor.cpp:203
s
RealScalar s
Definition: level1_cplx_impl.h:126
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
ceres::sin
Jet< T, N > sin(const Jet< T, N > &f)
Definition: jet.h:439
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
TestHarness.h
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::SmartProjectionParams::setRetriangulationThreshold
void setRetriangulationThreshold(double retriangulationTh)
Definition: SmartFactorParams.h:103
gtsam::RegularHessianFactor
A HessianFactor where all variables have the same dimension D.
Definition: RegularHessianFactor.h:57
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
LevenbergMarquardtOptimizer.h
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
measurement1
static Point2 measurement1(323.0, 240.0)
gtsam::LieGroup::compose
Class compose(const Class &g) const
Definition: Lie.h:48
gtsam::JacobianFactor
Definition: JacobianFactor.h:91
gtsam::JACOBIAN_Q
@ JACOBIAN_Q
Definition: SmartFactorParams.h:31
vanillaPose2::sharedK2
static const Cal3_S2::shared_ptr sharedK2(new Cal3_S2(1500, 1200, 0, 640, 480))
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:57
gtsam::RegularImplicitSchurFactor
Definition: RegularImplicitSchurFactor.h:39
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:247
gtsam::NonlinearFactorGraph::linearize
std::shared_ptr< GaussianFactorGraph > linearize(const Values &linearizationPoint) const
Linearize a nonlinear factor graph.
Definition: NonlinearFactorGraph.cpp:239
X
#define X
Definition: icosphere.cpp:20
vanilla::params
static const SmartProjectionParams params
Definition: smartFactorScenarios.h:69
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:91
result
Values result
Definition: OdometryOptimize.cpp:8
gtsam::CameraSet
A set of cameras, all with their own calibration.
Definition: CameraSet.h:37
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
x3
Pose3 x3(Rot3::Ypr(M_PI/4.0, 0.0, 0.0), l2)
landmark2
Point2 landmark2(7.0, 1.5)
sampling::sigma
static const double sigma
Definition: testGaussianBayesNet.cpp:170
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)
n
int n
Definition: BiCGSTAB_simple.cpp:1
gtsam::ZERO_ON_DEGENERACY
@ ZERO_ON_DEGENERACY
Definition: SmartFactorParams.h:36
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:259
gtsam::SmartProjectionParams::setDegeneracyMode
void setDegeneracyMode(DegeneracyMode degMode)
Definition: SmartFactorParams.h:100
gtsam::HANDLE_INFINITY
@ HANDLE_INFINITY
Definition: SmartFactorParams.h:36
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.
TEST
TEST(SmartProjectionPoseFactor, Constructor)
Definition: testSmartProjectionPoseFactor.cpp:55
gtsam.examples.PlanarManipulatorExample.delta
def delta(g0, g1)
Definition: PlanarManipulatorExample.py:45
gtsam::VectorValues
Definition: VectorValues.h:74
gtsam::JacobianFactor::information
Matrix information() const override
Definition: JacobianFactor.cpp:528
x1
Pose3 x1
Definition: testPose3.cpp:692
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
gtsam::SmartProjectionPoseFactor
Definition: SmartProjectionPoseFactor.h:45
g2
Pose3 g2(g1.expmap(h *V1_g1))
M_PI_4
#define M_PI_4
Definition: mconf.h:119
gtsam::JACOBIAN_SVD
@ JACOBIAN_SVD
Definition: SmartFactorParams.h:31
gtsam::Pose3::inverse
Pose3 inverse() const
inverse transformation with derivatives
Definition: Pose3.cpp:61
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:916
gtsam::NonlinearFactorGraph::addPrior
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
Definition: NonlinearFactorGraph.h:199
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
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::PoseTranslationPrior
Definition: PoseTranslationPrior.h:21
gtsam::SmartProjectionParams::getRetriangulationThreshold
double getRetriangulationThreshold() const
Definition: SmartFactorParams.h:93
gtsam::symbol_shorthand::F
Key F(std::uint64_t j)
Definition: inference/Symbol.h:153
F1
const Matrix26 F1
Definition: testRegularImplicitSchurFactor.cpp:37
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
EXPECT_DOUBLES_EQUAL
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:161
TestResult
Definition: TestResult.h:26
E
DiscreteKey E(5, 2)
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:218
estimation_fixture::measurements
std::vector< double > measurements
Definition: testHybridEstimation.cpp:52
gtsam::SmartFactorBase< PinholePose< CALIBRATION > >::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
gtsam::Pose3::rotation
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself={}) const
get rotation
Definition: Pose3.cpp:348
pose2
Definition: testFrobeniusFactor.cpp:193
vanillaPose2
Definition: smartFactorScenarios.h:89
vanilla::level_uv_right
static const Point2 level_uv_right
Definition: smartFactorScenarios.h:64
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::Values
Definition: Values.h:65
CHECK
#define CHECK(condition)
Definition: Test.h:108
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
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
gtsam::IGNORE_DEGENERACY
@ IGNORE_DEGENERACY
Definition: SmartFactorParams.h:36
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::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
Eigen::Matrix< double, 1, 1 >
gtsam::SmartProjectionParams::setLinearizationMode
void setLinearizationMode(LinearizationMode linMode)
Definition: SmartFactorParams.h:97
gtsam::JacobianFactorQ
Definition: JacobianFactorQ.h:27
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
gtsam::SmartFactorBase::add
void add(const Z &measured, const Key &key)
Definition: SmartFactorBase.h:127
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
ceres::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: gtsam/3rdparty/ceres/eigen.h:38
smartFactorScenarios.h
x2
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
R
Rot2 R(Rot2::fromAngle(0.1))
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
gtsam::FactorGraph::emplace_shared
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:153
gtsam::Symbol
Definition: inference/Symbol.h:37
main
int main()
Definition: testSmartProjectionPoseFactor.cpp:1335

gtsam
Author(s):
autogenerated on Wed May 28 2025 03:07:38