testEssentialMatrixFactor.cpp
Go to the documentation of this file.
1 /*
2  * @file testEssentialMatrixFactor.cpp
3  * @brief Test EssentialMatrixFactor class
4  * @author Frank Dellaert
5  * @date December 17, 2013
6  */
7 
9 #include <gtsam/base/Testable.h>
10 #include <gtsam/geometry/Cal3_S2.h>
18 #include <gtsam/sfm/SfmData.h>
19 #include <gtsam/slam/dataset.h>
20 
21 using namespace std::placeholders;
22 using namespace std;
23 using namespace gtsam;
24 
25 // Noise model for first type of factor is evaluating algebraic error
27  noiseModel::Isotropic::Sigma(1, 0.01);
28 // Noise model for second type of factor is evaluating pixel coordinates
29 noiseModel::Unit::shared_ptr model2 = noiseModel::Unit::Create(2);
30 
31 // The rotation between body and camera is:
32 gtsam::Point3 bX(1, 0, 0), bY(0, 1, 0), bZ(0, 0, 1);
34 
35 namespace example1 {
36 
37 const string filename = findExampleDataFile("18pointExample1.txt");
38 SfmData data = SfmData::FromBalFile(filename);
39 Rot3 c1Rc2 = data.cameras[1].pose().rotation();
40 Point3 c1Tc2 = data.cameras[1].pose().translation();
41 // TODO: maybe default value not good; assert with 0th
44 Rot3 trueRotation(c1Rc2);
45 Unit3 trueDirection(c1Tc2);
47 double baseline = 0.1; // actual baseline of the camera
48 
49 Point2 pA(size_t i) { return data.tracks[i].measurements[0].second; }
50 Point2 pB(size_t i) { return data.tracks[i].measurements[1].second; }
51 Vector vA(size_t i) { return EssentialMatrix::Homogeneous(pA(i)); }
52 Vector vB(size_t i) { return EssentialMatrix::Homogeneous(pB(i)); }
53 
54 //*************************************************************************
56  // Check E matrix
57  Matrix expected(3, 3);
58  expected << 0, 0, 0, 0, 0, -0.1, 0.1, 0, 0;
59  Matrix aEb_matrix =
60  skewSymmetric(c1Tc2.x(), c1Tc2.y(), c1Tc2.z()) * c1Rc2.matrix();
61  EXPECT(assert_equal(expected, aEb_matrix, 1e-8));
62 
63  // Check some projections
64  EXPECT(assert_equal(Point2(0, 0), pA(0), 1e-8));
65  EXPECT(assert_equal(Point2(0, 0.1), pB(0), 1e-8));
66  EXPECT(assert_equal(Point2(0, -1), pA(4), 1e-8));
67  EXPECT(assert_equal(Point2(-1, 0.2), pB(4), 1e-8));
68 
69  // Check homogeneous version
70  EXPECT(assert_equal(Vector3(-1, 0.2, 1), vB(4), 1e-8));
71 
72  // Check epipolar constraint
73  for (size_t i = 0; i < 5; i++)
74  EXPECT_DOUBLES_EQUAL(0, vA(i).transpose() * aEb_matrix * vB(i), 1e-8);
75 
76  // Check epipolar constraint
77  for (size_t i = 0; i < 5; i++)
78  EXPECT_DOUBLES_EQUAL(0, trueE.error(vA(i), vB(i)), 1e-7);
79 }
80 
81 //*************************************************************************
83  Key key(1);
84  for (size_t i = 0; i < 5; i++) {
85  EssentialMatrixFactor factor(key, pA(i), pB(i), model1);
86 
87  // Check evaluation
88  Vector expected(1);
89  expected << 0;
90  Vector actual = factor.evaluateError(trueE);
91  EXPECT(assert_equal(expected, actual, 1e-7));
92 
93  Values val;
94  val.insert(key, trueE);
95  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, val, 1e-5, 1e-7);
96  }
97 }
98 
99 //*************************************************************************
101  Key key(1);
102  for (size_t i = 0; i < 5; i++) {
103  std::function<double(const EssentialMatrix &, OptionalJacobian<1, 5>)> f =
104  std::bind(&EssentialMatrix::error, std::placeholders::_1, vA(i), vB(i), std::placeholders::_2);
105  Expression<EssentialMatrix> E_(key); // leaf expression
106  Expression<double> expr(f, E_); // unary expression
107 
108  // Test the derivatives using Paul's magic
109  Values values;
110  values.insert(key, trueE);
111  EXPECT_CORRECT_EXPRESSION_JACOBIANS(expr, values, 1e-5, 1e-9);
112 
113  // Create the factor
114  ExpressionFactor<double> factor(model1, 0, expr);
115 
116  // Check evaluation
117  Vector expected(1);
118  expected << 0;
119  vector<Matrix> Hactual(1);
120  Vector actual = factor.unwhitenedError(values, Hactual);
121  EXPECT(assert_equal(expected, actual, 1e-7));
122  }
123 }
124 
125 //*************************************************************************
126 TEST(EssentialMatrixFactor, ExpressionFactorRotationOnly) {
127  Key key(1);
128  for (size_t i = 0; i < 5; i++) {
129  std::function<double(const EssentialMatrix &, OptionalJacobian<1, 5>)> f =
130  std::bind(&EssentialMatrix::error, std::placeholders::_1, vA(i), vB(i), std::placeholders::_2);
131  std::function<EssentialMatrix(const Rot3 &, const Unit3 &,
134  g;
135  Expression<Rot3> R_(key);
137  Expression<EssentialMatrix> E_(&EssentialMatrix::FromRotationAndDirection,
138  R_, d_);
139  Expression<double> expr(f, E_);
140 
141  // Test the derivatives using Paul's magic
142  Values values;
143  values.insert(key, trueRotation);
144  EXPECT_CORRECT_EXPRESSION_JACOBIANS(expr, values, 1e-5, 1e-9);
145 
146  // Create the factor
147  ExpressionFactor<double> factor(model1, 0, expr);
148 
149  // Check evaluation
150  Vector expected(1);
151  expected << 0;
152  vector<Matrix> Hactual(1);
153  Vector actual = factor.unwhitenedError(values, Hactual);
154  EXPECT(assert_equal(expected, actual, 1e-7));
155  }
156 }
157 
158 //*************************************************************************
159 TEST(EssentialMatrixFactor, minimization) {
160  // Here we want to optimize directly on essential matrix constraints
161  // Yi Ma's algorithm (Ma01ijcv) is a bit cumbersome to implement,
162  // but GTSAM does the equivalent anyway, provided we give the right
163  // factors. In this case, the factors are the constraints.
164 
165  // We start with a factor graph and add constraints to it
166  // Noise sigma is 1cm, assuming metric measurements
168  for (size_t i = 0; i < 5; i++)
170 
171  // Check error at ground truth
172  Values truth;
173  truth.insert(1, trueE);
174  EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);
175 
176  // Check error at initial estimate
177  Values initial;
178  EssentialMatrix initialE =
179  trueE.retract((Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished());
180  initial.insert(1, initialE);
181 #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
182  EXPECT_DOUBLES_EQUAL(643.26, graph.error(initial), 1e-2);
183 #else
184  EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2);
185 #endif
186 
187  // Optimize
189  LevenbergMarquardtOptimizer optimizer(graph, initial, parameters);
190  Values result = optimizer.optimize();
191 
192  // Check result
193  EssentialMatrix actual = result.at<EssentialMatrix>(1);
194  EXPECT(assert_equal(trueE, actual, 1e-1));
195 
196  // Check error at result
197  EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
198 
199  // Check errors individually
200  for (size_t i = 0; i < 5; i++)
201  EXPECT_DOUBLES_EQUAL(0, actual.error(vA(i), vB(i)), 1e-6);
202 }
203 
204 //*************************************************************************
206  for (size_t i = 0; i < 5; i++) {
207  EssentialMatrixFactor2 factor(100, i, pA(i), pB(i), model2);
208 
209  // Check evaluation
210  Point3 P1 = data.tracks[i].p, P2 = data.cameras[1].pose().transformTo(P1);
211  const Point2 pi = PinholeBase::Project(P2);
212  Point2 expected(pi - pB(i));
213 
214  Matrix Hactual1, Hactual2;
215  double d(baseline / P1.z());
216  Vector actual = factor.evaluateError(trueE, d, Hactual1, Hactual2);
217  EXPECT(assert_equal(expected, actual, 1e-7));
218 
219  Values val;
220  val.insert(100, trueE);
221  val.insert(i, d);
222  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, val, 1e-5, 1e-7);
223  }
224 }
225 
226 //*************************************************************************
227 TEST(EssentialMatrixFactor2, minimization) {
228  // Here we want to optimize for E and inverse depths at the same time
229 
230  // We start with a factor graph and add constraints to it
231  // Noise sigma is 1cm, assuming metric measurements
233  for (size_t i = 0; i < 5; i++)
234  graph.emplace_shared<EssentialMatrixFactor2>(100, i, pA(i), pB(i), model2);
235 
236  // Check error at ground truth
237  Values truth;
238  truth.insert(100, trueE);
239  for (size_t i = 0; i < 5; i++) {
240  Point3 P1 = data.tracks[i].p;
241  truth.insert(i, double(baseline / P1.z()));
242  }
243  EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);
244 
245  // Optimize
247  // parameters.setVerbosity("ERROR");
248  LevenbergMarquardtOptimizer optimizer(graph, truth, parameters);
249  Values result = optimizer.optimize();
250 
251  // Check result
252  EssentialMatrix actual = result.at<EssentialMatrix>(100);
253  EXPECT(assert_equal(trueE, actual, 1e-1));
254  for (size_t i = 0; i < 5; i++)
255  EXPECT_DOUBLES_EQUAL(truth.at<double>(i), result.at<double>(i), 1e-1);
256 
257  // Check error at result
258  EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
259 }
260 
261 //*************************************************************************
262 // Below we want to optimize for an essential matrix specified in a
263 // body coordinate frame B which is rotated with respect to the camera
264 // frame C, via the rotation bRc.
265 
266 // The "true E" in the body frame is then
268 
269 //*************************************************************************
271  for (size_t i = 0; i < 5; i++) {
272  EssentialMatrixFactor3 factor(100, i, pA(i), pB(i), cRb, model2);
273 
274  // Check evaluation
275  Point3 P1 = data.tracks[i].p;
276  const Point2 pi = camera2.project(P1);
277  Point2 expected(pi - pB(i));
278 
279  Matrix Hactual1, Hactual2;
280  double d(baseline / P1.z());
281  Vector actual = factor.evaluateError(bodyE, d, Hactual1, Hactual2);
282  EXPECT(assert_equal(expected, actual, 1e-7));
283 
284  Values val;
285  val.insert(100, bodyE);
286  val.insert(i, d);
287  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, val, 1e-6, 1e-7);
288  }
289 }
290 
291 //*************************************************************************
292 TEST(EssentialMatrixFactor3, minimization) {
293  // As before, we start with a factor graph and add constraints to it
295  for (size_t i = 0; i < 5; i++)
296  // but now we specify the rotation bRc
297  graph.emplace_shared<EssentialMatrixFactor3>(100, i, pA(i), pB(i), cRb,
298  model2);
299 
300  // Check error at ground truth
301  Values truth;
302  truth.insert(100, bodyE);
303  for (size_t i = 0; i < 5; i++) {
304  Point3 P1 = data.tracks[i].p;
305  truth.insert(i, double(baseline / P1.z()));
306  }
307  EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);
308 
309  // Optimize
311  // parameters.setVerbosity("ERROR");
312  LevenbergMarquardtOptimizer optimizer(graph, truth, parameters);
313  Values result = optimizer.optimize();
314 
315  // Check result
316  EssentialMatrix actual = result.at<EssentialMatrix>(100);
317  EXPECT(assert_equal(bodyE, actual, 1e-1));
318  for (size_t i = 0; i < 5; i++)
319  EXPECT_DOUBLES_EQUAL(truth.at<double>(i), result.at<double>(i), 1e-1);
320 
321  // Check error at result
322  EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
323 }
324 
325 //*************************************************************************
327  Key keyE(1);
328  Key keyK(2);
329  for (size_t i = 0; i < 5; i++) {
330  EssentialMatrixFactor4<Cal3_S2> factor(keyE, keyK, pA(i), pB(i), model1);
331 
332  // Check evaluation
334  expected << 0;
335  Vector actual = factor.evaluateError(trueE, trueK);
336  EXPECT(assert_equal(expected, actual, 1e-7));
337 
338  Values truth;
339  truth.insert(keyE, trueE);
340  truth.insert(keyK, trueK);
341  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, truth, 1e-6, 1e-7);
342  }
343 }
344 
345 //*************************************************************************
346 TEST(EssentialMatrixFactor4, evaluateErrorJacobiansCal3S2) {
347  Key keyE(1);
348  Key keyK(2);
349  // initialize essential matrix
350  Rot3 r = Rot3::Expmap(Vector3(M_PI / 6, M_PI / 3, M_PI / 9));
351  Unit3 t(Point3(2, -1, 0.5));
352  EssentialMatrix E = EssentialMatrix::FromRotationAndDirection(r, t);
353  Cal3_S2 K(200, 1, 1, 10, 10);
354  Values val;
355  val.insert(keyE, E);
356  val.insert(keyK, K);
357 
358  Point2 pA(10.0, 20.0);
359  Point2 pB(12.0, 15.0);
360 
361  EssentialMatrixFactor4<Cal3_S2> f(keyE, keyK, pA, pB, model1);
362  EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 1e-6);
363 }
364 
365 //*************************************************************************
366 TEST(EssentialMatrixFactor4, evaluateErrorJacobiansCal3Bundler) {
367  Key keyE(1);
368  Key keyK(2);
369  // initialize essential matrix
370  Rot3 r = Rot3::Expmap(Vector3(0, 0, M_PI_2));
371  Unit3 t(Point3(0.1, 0, 0));
372  EssentialMatrix E = EssentialMatrix::FromRotationAndDirection(r, t);
373  Cal3Bundler K;
374  Values val;
375  val.insert(keyE, E);
376  val.insert(keyK, K);
377 
378  Point2 pA(-0.1, 0.5);
379  Point2 pB(-0.5, -0.2);
380 
381  EssentialMatrixFactor4<Cal3Bundler> f(keyE, keyK, pA, pB, model1);
382  EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 1e-5);
383 }
384 
385 //*************************************************************************
386 TEST(EssentialMatrixFactor4, minimizationWithStrongCal3S2Prior) {
388  for (size_t i = 0; i < 5; i++)
390  model1);
391 
392  // Check error at ground truth
393  Values truth;
394  truth.insert(1, trueE);
395  truth.insert(2, trueK);
396  EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);
397 
398  // Initialization
399  Values initial;
400  EssentialMatrix initialE =
401  trueE.retract((Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished());
402  initial.insert(1, initialE);
403  initial.insert(2, trueK);
404 
405  // add prior factor for calibration
406  Vector5 priorNoiseModelSigma;
407  priorNoiseModelSigma << 10, 10, 10, 10, 10;
409  2, trueK, noiseModel::Diagonal::Sigmas(priorNoiseModelSigma));
410 
411  LevenbergMarquardtOptimizer optimizer(graph, initial);
412  Values result = optimizer.optimize();
413 
414  // Check result
415  EssentialMatrix actualE = result.at<EssentialMatrix>(1);
416  Cal3_S2 actualK = result.at<Cal3_S2>(2);
417  EXPECT(assert_equal(trueE, actualE, 1e-1));
418  EXPECT(assert_equal(trueK, actualK, 1e-2));
419 
420  // Check error at result
421  EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
422 
423  // Check errors individually
424  for (size_t i = 0; i < 5; i++)
426  0,
427  actualE.error(EssentialMatrix::Homogeneous(actualK.calibrate(pA(i))),
428  EssentialMatrix::Homogeneous(actualK.calibrate(pB(i)))),
429  1e-6);
430 }
431 
432 //*************************************************************************
433 TEST(EssentialMatrixFactor4, minimizationWithWeakCal3S2Prior) {
434  // We need 7 points here as the prior on the focal length parameters is weak
435  // and the initialization is noisy. So in total we are trying to optimize 7
436  // DOF, with a strong prior on the remaining 3 DOF.
438  for (size_t i = 0; i < 7; i++)
440  model1);
441 
442  // Check error at ground truth
443  Values truth;
444  truth.insert(1, trueE);
445  truth.insert(2, trueK);
446  EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);
447 
448  // Initialization
449  Values initial;
450  EssentialMatrix initialE =
451  trueE.retract((Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished());
452  Cal3_S2 initialK =
453  trueK.retract((Vector(5) << 0.1, -0.1, 0.0, -0.0, 0.0).finished());
454  initial.insert(1, initialE);
455  initial.insert(2, initialK);
456 
457  // add prior factor for calibration
458  Vector5 priorNoiseModelSigma;
459  priorNoiseModelSigma << 20, 20, 1, 1, 1;
461  2, initialK, noiseModel::Diagonal::Sigmas(priorNoiseModelSigma));
462 
463  LevenbergMarquardtOptimizer optimizer(graph, initial);
464  Values result = optimizer.optimize();
465 
466  // Check result
467  EssentialMatrix actualE = result.at<EssentialMatrix>(1);
468  Cal3_S2 actualK = result.at<Cal3_S2>(2);
469  EXPECT(assert_equal(trueE, actualE, 1e-1));
470  EXPECT(assert_equal(trueK, actualK, 1e-2));
471 
472  // Check error at result
473  EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
474 
475  // Check errors individually
476  for (size_t i = 0; i < 7; i++)
478  0,
479  actualE.error(EssentialMatrix::Homogeneous(actualK.calibrate(pA(i))),
480  EssentialMatrix::Homogeneous(actualK.calibrate(pB(i)))),
481  1e-5);
482 }
483 
484 //*************************************************************************
485 TEST(EssentialMatrixFactor4, minimizationWithStrongCal3BundlerPrior) {
487  for (size_t i = 0; i < 5; i++)
489  pB(i), model1);
490  Cal3Bundler trueK(1, 0, 0, 0, 0, /*tolerance=*/5e-3);
491  // Check error at ground truth
492  Values truth;
493  truth.insert(1, trueE);
494  truth.insert(2, trueK);
495  EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);
496 
497  // Check error at initial estimate
498  Values initial;
499  EssentialMatrix initialE =
500  trueE.retract((Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished());
501  Cal3Bundler initialK = trueK;
502  initial.insert(1, initialE);
503  initial.insert(2, initialK);
504 
505  // add prior factor for calibration
506  Vector3 priorNoiseModelSigma;
507  priorNoiseModelSigma << 0.1, 0.1, 0.1;
509  2, trueK, noiseModel::Diagonal::Sigmas(priorNoiseModelSigma));
510 
511  LevenbergMarquardtOptimizer optimizer(graph, initial);
512  Values result = optimizer.optimize();
513 
514  // Check result
515  EssentialMatrix actualE = result.at<EssentialMatrix>(1);
516  Cal3Bundler actualK = result.at<Cal3Bundler>(2);
517  EXPECT(assert_equal(trueE, actualE, 1e-1));
518  EXPECT(assert_equal(trueK, actualK, 1e-2));
519 
520  // Check error at result
521  EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
522 
523  // Check errors individually
524  for (size_t i = 0; i < 5; i++)
526  0,
527  actualE.error(EssentialMatrix::Homogeneous(actualK.calibrate(pA(i))),
528  EssentialMatrix::Homogeneous(actualK.calibrate(pB(i)))),
529  1e-6);
530 }
531 
532 } // namespace example1
533 
534 //*************************************************************************
535 
536 namespace example2 {
537 
538 const string filename = findExampleDataFile("5pointExample2.txt");
539 SfmData data = SfmData::FromBalFile(filename);
540 Rot3 aRb = data.cameras[1].pose().rotation();
541 Point3 aTb = data.cameras[1].pose().translation();
542 EssentialMatrix trueE(aRb, Unit3(aTb));
543 
544 double baseline = 10; // actual baseline of the camera
545 
546 Point2 pA(size_t i) { return data.tracks[i].measurements[0].second; }
547 Point2 pB(size_t i) { return data.tracks[i].measurements[1].second; }
548 
549 Cal3Bundler trueK = Cal3Bundler(500, 0, 0);
550 std::shared_ptr<Cal3Bundler> K = std::make_shared<Cal3Bundler>(trueK);
552 
553 Vector vA(size_t i) {
554  Point2 xy = trueK.calibrate(pA(i));
555  return EssentialMatrix::Homogeneous(xy);
556 }
557 Vector vB(size_t i) {
558  Point2 xy = trueK.calibrate(pB(i));
559  return EssentialMatrix::Homogeneous(xy);
560 }
561 
562 //*************************************************************************
563 TEST(EssentialMatrixFactor, extraMinimization) {
564  // Additional test with camera moving in positive X direction
565 
567  for (size_t i = 0; i < 5; i++)
568  graph.emplace_shared<EssentialMatrixFactor>(1, pA(i), pB(i), model1, K);
569 
570  // Check error at ground truth
571  Values truth;
572  truth.insert(1, trueE);
573  EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);
574 
575  // Check error at initial estimate
576  Values initial;
577  EssentialMatrix initialE =
578  trueE.retract((Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished());
579  initial.insert(1, initialE);
580 
581 #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
582  EXPECT_DOUBLES_EQUAL(643.26, graph.error(initial), 1e-2);
583 #else
584  EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2);
585 #endif
586 
587  // Optimize
589  LevenbergMarquardtOptimizer optimizer(graph, initial, parameters);
590  Values result = optimizer.optimize();
591 
592  // Check result
593  EssentialMatrix actual = result.at<EssentialMatrix>(1);
594  EXPECT(assert_equal(trueE, actual, 1e-1));
595 
596  // Check error at result
597  EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
598 
599  // Check errors individually
600  for (size_t i = 0; i < 5; i++)
601  EXPECT_DOUBLES_EQUAL(0, actual.error(vA(i), vB(i)), 1e-6);
602 }
603 
604 //*************************************************************************
606  for (size_t i = 0; i < 5; i++) {
607  EssentialMatrixFactor2 factor(100, i, pA(i), pB(i), model2, K);
608 
609  // Check evaluation
610  Point3 P1 = data.tracks[i].p;
611  const Point2 pi = camera2.project(P1);
612  Point2 expected(pi - pB(i));
613 
614  double d(baseline / P1.z());
615  Vector actual = factor.evaluateError(trueE, d);
616  EXPECT(assert_equal(expected, actual, 1e-7));
617 
618  Values val;
619  val.insert(100, trueE);
620  val.insert(i, d);
621  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, val, 1e-5, 1e-6);
622  }
623 }
624 
625 //*************************************************************************
626 TEST(EssentialMatrixFactor2, extraMinimization) {
627  // Additional test with camera moving in positive X direction
628 
629  // We start with a factor graph and add constraints to it
630  // Noise sigma is 1, assuming pixel measurements
632  for (size_t i = 0; i < data.numberTracks(); i++)
634  K);
635 
636  // Check error at ground truth
637  Values truth;
638  truth.insert(100, trueE);
639  for (size_t i = 0; i < data.numberTracks(); i++) {
640  Point3 P1 = data.tracks[i].p;
641  truth.insert(i, double(baseline / P1.z()));
642  }
643  EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);
644 
645  // Optimize
647  // parameters.setVerbosity("ERROR");
648  LevenbergMarquardtOptimizer optimizer(graph, truth, parameters);
649  Values result = optimizer.optimize();
650 
651  // Check result
652  EssentialMatrix actual = result.at<EssentialMatrix>(100);
653  EXPECT(assert_equal(trueE, actual, 1e-1));
654  for (size_t i = 0; i < data.numberTracks(); i++)
655  EXPECT_DOUBLES_EQUAL(truth.at<double>(i), result.at<double>(i), 1e-1);
656 
657  // Check error at result
658  EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
659 }
660 
661 //*************************************************************************
663  // The "true E" in the body frame is
664  EssentialMatrix bodyE = cRb.inverse() * trueE;
665 
666  for (size_t i = 0; i < 5; i++) {
667  EssentialMatrixFactor3 factor(100, i, pA(i), pB(i), cRb, model2, K);
668 
669  // Check evaluation
670  Point3 P1 = data.tracks[i].p;
671  const Point2 pi = camera2.project(P1);
672  Point2 expected(pi - pB(i));
673 
674  double d(baseline / P1.z());
675  Vector actual = factor.evaluateError(bodyE, d);
676  EXPECT(assert_equal(expected, actual, 1e-7));
677 
678  Values val;
679  val.insert(100, bodyE);
680  val.insert(i, d);
681  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, val, 1e-5, 1e-6);
682  }
683 }
684 
685 } // namespace example2
686 
687 /* ************************************************************************* */
688 int main() {
689  TestResult tr;
690  return TestRegistry::runAllTests(tr);
691 }
692 /* ************************************************************************* */
const gtsam::Symbol key('X', 0)
Vector vA(size_t i)
virtual const Values & optimize()
Concept check for values that can be used in unit tests.
SfmData stores a bunch of SfmTracks.
Definition: SfmData.h:39
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:196
static int runAllTests(TestResult &result)
Eigen::Vector3d Vector3
Definition: Vector.h:43
Factor Graph consisting of non-linear factors.
const ValueType at(Key j) const
Definition: Values-inl.h:204
Matrix expected
Definition: testMatrix.cpp:971
GTSAM_EXPORT double error(const Vector3 &vA, const Vector3 &vB, OptionalJacobian< 1, 5 > H={}) const
epipolar error, algebraic
Vector2 Point2
Definition: Point2.h:32
noiseModel::Unit::shared_ptr model2
const double baseline
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
std::shared_ptr< Cal3Bundler > K
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
gtsam::Point3 bX(1, 0, 0)
#define M_PI
Definition: main.h:106
gtsam::Point3 bY(0, 1, 0)
std::shared_ptr< Unit > shared_ptr
Definition: NoiseModel.h:600
leaf::MyValues values
Pose2_ Expmap(const Vector3_ &xi)
Values initial
TEST(EssentialMatrixFactor3, extraTest)
Vector evaluateError(const EssentialMatrix &E, const double &d, OptionalMatrixType DE, OptionalMatrixType Dd) const override
Definition: BFloat16.h:88
EssentialMatrix trueE(trueRotation, trueDirection)
Point2 pA(size_t i)
Matrix< float, 2, 1 > xy
Definition: LLT_solve.cpp:7
Evaluate derivatives of a nonlinear factor numerically.
PinholeCamera< Cal3Bundler > camera2(data.cameras[1].pose(), trueK)
NonlinearFactorGraph graph
gtsam::Rot3 cRb
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:161
noiseModel::Isotropic::shared_ptr model1
void g(const string &key, int i)
Definition: testBTree.cpp:41
Vector evaluateError(const EssentialMatrix &E, const CALIBRATION &K, OptionalMatrixType H1, OptionalMatrixType H2) const override
Calculate the algebraic epipolar error pA&#39; (K^-1)&#39; E K pB.
#define EXPECT_CORRECT_EXPRESSION_JACOBIANS(expression, values, numerical_derivative_step, tolerance)
Check the Jacobians produced by an expression against finite differences.
std::vector< SfmTrack > tracks
Sparse set of points.
Definition: SfmData.h:42
Rot3 inverse() const
inverse of a rotation
Definition: Rot3.h:308
EssentialMatrix retract(const Vector5 &xi) const
Retract delta to manifold.
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
Vector evaluateError(const EssentialMatrix &E, const double &d, OptionalMatrixType DE, OptionalMatrixType Dd) const override
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
Eigen::VectorXd Vector
Definition: Vector.h:38
Values result
Point2 pB(size_t i)
Test harness methods for expressions.
Point2 calibrate(const Point2 &pi, OptionalJacobian< 2, 3 > Dcal={}, OptionalJacobian< 2, 2 > Dp={}) const
Definition: Cal3Bundler.cpp:95
#define EXPECT(condition)
Definition: Test.h:150
Data structure for dealing with Structure from Motion data.
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Vector unwhitenedError(const Values &x, OptionalMatrixVecType H=nullptr) const override
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::Point3 bZ(0, 0, 1)
The most common 5DOF 3D->2D calibration.
Definition: Cal3_S2.h:34
double error(const Values &values) const
std::shared_ptr< Isotropic > shared_ptr
Definition: NoiseModel.h:542
static ConjugateGradientParameters parameters
Calibrated camera for which only pose is unknown.
static const Point3 P2(3.5,-8.2, 4.2)
Unit3 trueDirection(trueTranslation)
traits
Definition: chartTesting.h:28
Cal3_S2 retract(const Vector &d) const
Given 5-dim tangent vector, create new calibration.
Definition: Cal3_S2.h:121
Matrix3 skewSymmetric(double wx, double wy, double wz)
Definition: base/Matrix.h:400
GTSAM_EXPORT std::string findExampleDataFile(const std::string &name)
Definition: dataset.cpp:70
std::vector< SfmCamera > cameras
Set of cameras.
Definition: SfmData.h:40
EssentialMatrix bodyE
DiscreteKey E(5, 2)
Rot3 trueRotation
Calibration used by Bundler.
Definition: Cal3Bundler.h:32
#define EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, numerical_derivative_step, tolerance)
Check the Jacobians produced by a factor against finite differences.
Matrix3 matrix() const
Definition: Rot3M.cpp:218
Point2(* Project)(const Point3 &, OptionalJacobian< 2, 3 >)
void insert(Key j, const Value &val)
Definition: Values.cpp:155
static double error
Definition: testRot3.cpp:37
Vector evaluateError(const EssentialMatrix &E, OptionalMatrixType H) const override
vector of errors returns 1D vector
size_t numberTracks() const
The number of reconstructed 3D points.
Definition: SfmData.h:74
Vector3 Point3
Definition: Point3.h:38
The matrix class, also used for vectors and row-vectors.
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
utility functions for loading datasets
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
Vector vB(size_t i)
Point2 t(10, 10)
The most common 5DOF 3D->2D calibration.


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:38:04