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

gtsam
Author(s):
autogenerated on Sat May 8 2021 02:49:53