testSmartStereoProjectionFactorPP.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 
19 #include <gtsam/slam/tests/smartFactorScenarios.h>
20 #include <gtsam_unstable/slam/SmartStereoProjectionFactorPP.h>
21 #include <gtsam_unstable/slam/ProjectionFactorPPP.h>
22 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
23 #include <gtsam/slam/PoseTranslationPrior.h>
24 #include <gtsam/slam/ProjectionFactor.h>
25 #include <gtsam/slam/StereoFactor.h>
26 #include <CppUnitLite/TestHarness.h>
27 #include <iostream>
28 
29 using namespace std;
30 using namespace gtsam;
31 
32 namespace {
33 // make a realistic calibration matrix
34 static double b = 1;
35 
36 static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fov, w, h, b));
37 static Cal3_S2Stereo::shared_ptr K2(new Cal3_S2Stereo(1500, 1200, 0, 640, 480,
38  b));
39 
40 static SmartStereoProjectionParams params;
41 
42 // static bool manageDegeneracy = true;
43 // Create a noise model for the pixel error
44 static SharedNoiseModel model(noiseModel::Isotropic::Sigma(3, 0.1));
45 
46 // Convenience for named keys
47 using symbol_shorthand::L;
48 using symbol_shorthand::X;
49 
50 // tests data
51 static Symbol x1('X', 1);
52 static Symbol x2('X', 2);
53 static Symbol x3('X', 3);
54 static Symbol body_P_cam1_key('P', 1);
55 static Symbol body_P_cam2_key('P', 2);
56 static Symbol body_P_cam3_key('P', 3);
57 
58 static Key poseKey1(x1);
59 static Key poseExtrinsicKey1(body_P_cam1_key);
60 static Key poseExtrinsicKey2(body_P_cam2_key);
61 static StereoPoint2 measurement1(
62  323.0, 300.0, 240.0); // potentially use more reasonable measurement value?
63 static StereoPoint2 measurement2(
64  350.0, 200.0, 240.0); // potentially use more reasonable measurement value?
65 static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
66  Point3(0.25, -0.10, 1.0));
67 
68 static double missing_uR = std::numeric_limits<double>::quiet_NaN();
69 
70 vector<StereoPoint2> stereo_projectToMultipleCameras(const StereoCamera& cam1,
71  const StereoCamera& cam2,
72  const StereoCamera& cam3,
73  Point3 landmark) {
74  vector<StereoPoint2> measurements_cam;
75 
76  StereoPoint2 cam1_uv1 = cam1.project(landmark);
77  StereoPoint2 cam2_uv1 = cam2.project(landmark);
78  StereoPoint2 cam3_uv1 = cam3.project(landmark);
79  measurements_cam.push_back(cam1_uv1);
80  measurements_cam.push_back(cam2_uv1);
81  measurements_cam.push_back(cam3_uv1);
82 
83  return measurements_cam;
84 }
85 
86 LevenbergMarquardtParams lm_params;
87 } // namespace
88 
89 /* ************************************************************************* */
90 TEST( SmartStereoProjectionFactorPP, params) {
91  SmartStereoProjectionParams p;
92 
93  // check default values and "get"
94  EXPECT(p.getLinearizationMode() == HESSIAN);
95  EXPECT(p.getDegeneracyMode() == IGNORE_DEGENERACY);
96  EXPECT_DOUBLES_EQUAL(p.getRetriangulationThreshold(), 1e-5, 1e-9);
97  EXPECT(p.getVerboseCheirality() == false);
98  EXPECT(p.getThrowCheirality() == false);
99 
100  // check "set"
101  p.setLinearizationMode(JACOBIAN_SVD);
102  p.setDegeneracyMode(ZERO_ON_DEGENERACY);
103  p.setRankTolerance(100);
104  p.setEnableEPI(true);
105  p.setLandmarkDistanceThreshold(200);
106  p.setDynamicOutlierRejectionThreshold(3);
107  p.setRetriangulationThreshold(1e-2);
108 
109  EXPECT(p.getLinearizationMode() == JACOBIAN_SVD);
110  EXPECT(p.getDegeneracyMode() == ZERO_ON_DEGENERACY);
111  EXPECT_DOUBLES_EQUAL(p.getTriangulationParameters().rankTolerance, 100, 1e-5);
112  EXPECT(p.getTriangulationParameters().enableEPI == true);
113  EXPECT_DOUBLES_EQUAL(p.getTriangulationParameters().landmarkDistanceThreshold, 200, 1e-5);
114  EXPECT_DOUBLES_EQUAL(p.getTriangulationParameters().dynamicOutlierRejectionThreshold, 3, 1e-5);
115  EXPECT_DOUBLES_EQUAL(p.getRetriangulationThreshold(), 1e-2, 1e-5);
116 }
117 
118 /* ************************************************************************* */
119 TEST( SmartStereoProjectionFactorPP, Constructor) {
120  SmartStereoProjectionFactorPP::shared_ptr factor1(new SmartStereoProjectionFactorPP(model));
121 }
122 
123 /* ************************************************************************* */
124 TEST( SmartStereoProjectionFactorPP, Constructor2) {
125  SmartStereoProjectionFactorPP factor1(model, params);
126 }
127 
128 /* ************************************************************************* */
129 TEST( SmartStereoProjectionFactorPP, Constructor3) {
130  SmartStereoProjectionFactorPP::shared_ptr factor1(new SmartStereoProjectionFactorPP(model));
131  factor1->add(measurement1, poseKey1, poseExtrinsicKey1, K);
132 }
133 
134 /* ************************************************************************* */
135 TEST( SmartStereoProjectionFactorPP, Constructor4) {
136  SmartStereoProjectionFactorPP factor1(model, params);
137  factor1.add(measurement1, poseKey1, poseExtrinsicKey1, K);
138 }
139 
140 /* ************************************************************************* */
141 TEST( SmartStereoProjectionFactorPP, Equals ) {
142  SmartStereoProjectionFactorPP::shared_ptr factor1(new SmartStereoProjectionFactorPP(model));
143  factor1->add(measurement1, poseKey1, poseExtrinsicKey1, K);
144 
145  SmartStereoProjectionFactorPP::shared_ptr factor2(new SmartStereoProjectionFactorPP(model));
146  factor2->add(measurement1, poseKey1, poseExtrinsicKey1, K);
147  // check these are equal
148  EXPECT(assert_equal(*factor1, *factor2));
149 
150  SmartStereoProjectionFactorPP::shared_ptr factor3(new SmartStereoProjectionFactorPP(model));
151  factor3->add(measurement2, poseKey1, poseExtrinsicKey1, K);
152  // check these are different
153  EXPECT(!factor1->equals(*factor3));
154 
155  SmartStereoProjectionFactorPP::shared_ptr factor4(new SmartStereoProjectionFactorPP(model));
156  factor4->add(measurement1, poseKey1, poseExtrinsicKey2, K);
157  // check these are different
158  EXPECT(!factor1->equals(*factor4));
159 }
160 
161 /* *************************************************************************/
162 TEST_UNSAFE( SmartStereoProjectionFactorPP, noiseless_error_identityExtrinsics ) {
163  // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
164  Pose3 w_Pose_cam1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2),
165  Point3(0, 0, 1));
166  StereoCamera w_Camera_cam1(w_Pose_cam1, K2);
167 
168  // create second camera 1 meter to the right of first camera
169  Pose3 w_Pose_cam2 = w_Pose_cam1 * Pose3(Rot3(), Point3(1, 0, 0));
170  StereoCamera w_Camera_cam2(w_Pose_cam2, K2);
171 
172  // landmark ~5 meters infront of camera
173  Point3 landmark(5, 0.5, 1.2);
174 
175  // 1. Project two landmarks into two cameras and triangulate
176  StereoPoint2 cam1_uv = w_Camera_cam1.project(landmark);
177  StereoPoint2 cam2_uv = w_Camera_cam2.project(landmark);
178 
179  Values values;
180  values.insert(x1, w_Pose_cam1);
181  values.insert(x2, w_Pose_cam2);
182  values.insert(body_P_cam1_key, Pose3::Identity());
183  values.insert(body_P_cam2_key, Pose3::Identity());
184 
185  SmartStereoProjectionFactorPP factor1(model);
186  factor1.add(cam1_uv, x1, body_P_cam1_key, K2);
187  factor1.add(cam2_uv, x2, body_P_cam2_key, K2);
188 
189  double actualError = factor1.error(values);
190  double expectedError = 0.0;
191  EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7);
192 
193  SmartStereoProjectionFactorPP::Cameras cameras = factor1.cameras(values);
194  double actualError2 = factor1.totalReprojectionError(cameras);
195  EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7);
196 }
197 
198 /* *************************************************************************/
199 TEST_UNSAFE( SmartStereoProjectionFactorPP, noiseless_error_multipleExtrinsics ) {
200  // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
201  Pose3 w_Pose_cam1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2),
202  Point3(0, 0, 1));
203  StereoCamera w_Camera_cam1(w_Pose_cam1, K2);
204 
205  // create second camera 1 meter to the right of first camera
206  Pose3 w_Pose_cam2 = w_Pose_cam1 * Pose3(Rot3(), Point3(1, 0, 0));
207  StereoCamera w_Camera_cam2(w_Pose_cam2, K2);
208 
209  // landmark ~5 meters infront of camera
210  Point3 landmark(5, 0.5, 1.2);
211 
212  // 1. Project two landmarks into two cameras and triangulate
213  StereoPoint2 cam1_uv = w_Camera_cam1.project(landmark);
214  StereoPoint2 cam2_uv = w_Camera_cam2.project(landmark);
215 
216  Pose3 body_Pose_cam1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., 0.0),
217  Point3(0, 1, 0));
218  Pose3 body_Pose_cam2 = Pose3(Rot3::Ypr(-M_PI / 4, 0., 0.0),
219  Point3(1, 1, 0));
220  Pose3 w_Pose_body1 = w_Pose_cam1.compose(body_Pose_cam1.inverse());
221  Pose3 w_Pose_body2 = w_Pose_cam2.compose(body_Pose_cam2.inverse());
222 
223  Values values;
224  values.insert(x1, w_Pose_body1);
225  values.insert(x2, w_Pose_body2);
226  values.insert(body_P_cam1_key, body_Pose_cam1);
227  values.insert(body_P_cam2_key, body_Pose_cam2);
228 
229  SmartStereoProjectionFactorPP factor1(model);
230  factor1.add(cam1_uv, x1, body_P_cam1_key, K2);
231  factor1.add(cam2_uv, x2, body_P_cam2_key, K2);
232 
233  double actualError = factor1.error(values);
234  double expectedError = 0.0;
235  EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7);
236 
237  SmartStereoProjectionFactorPP::Cameras cameras = factor1.cameras(values);
238  double actualError2 = factor1.totalReprojectionError(cameras);
239  EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7);
240 }
241 
242 /* *************************************************************************/
243 TEST( SmartProjectionPoseFactor, noiseless_error_multipleExtrinsics_missingMeasurements ) {
244 
245  // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
246  Pose3 w_Pose_cam1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2),
247  Point3(0, 0, 1));
248  StereoCamera w_Camera_cam1(w_Pose_cam1, K2);
249 
250  // create second camera 1 meter to the right of first camera
251  Pose3 w_Pose_cam2 = w_Pose_cam1 * Pose3(Rot3(), Point3(1, 0, 0));
252  StereoCamera w_Camera_cam2(w_Pose_cam2, K2);
253 
254  // landmark ~5 meters in front of camera
255  Point3 landmark(5, 0.5, 1.2);
256 
257  // 1. Project two landmarks into two cameras and triangulate
258  StereoPoint2 cam1_uv = w_Camera_cam1.project(landmark);
259  StereoPoint2 cam2_uv = w_Camera_cam2.project(landmark);
260  StereoPoint2 cam2_uv_right_missing(cam2_uv.uL(),missing_uR,cam2_uv.v());
261 
262  // fake extrinsic calibration
263  Pose3 body_Pose_cam1 = Pose3(Rot3::Ypr(-M_PI, 1., 0.1),
264  Point3(0, 1, 0));
265  Pose3 body_Pose_cam2 = Pose3(Rot3::Ypr(-M_PI / 4, 0.1, 1.0),
266  Point3(1, 1, 1));
267  Pose3 w_Pose_body1 = w_Pose_cam1.compose(body_Pose_cam1.inverse());
268  Pose3 w_Pose_body2 = w_Pose_cam2.compose(body_Pose_cam2.inverse());
269 
270  Values values;
271  values.insert(x1, w_Pose_body1);
272  values.insert(x2, w_Pose_body2);
273  values.insert(body_P_cam1_key, body_Pose_cam1);
274  values.insert(body_P_cam2_key, body_Pose_cam2);
275 
276  SmartStereoProjectionFactorPP factor1(model);
277  factor1.add(cam1_uv, x1, body_P_cam1_key, K2);
278  factor1.add(cam2_uv_right_missing, x2, body_P_cam2_key, K2);
279 
280  double actualError = factor1.error(values);
281  double expectedError = 0.0;
282  EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7);
283 
284  // TEST TRIANGULATION WITH MISSING VALUES: i) right pixel of second camera is missing:
285  SmartStereoProjectionFactorPP::Cameras cameras = factor1.cameras(values);
286  double actualError2 = factor1.totalReprojectionError(cameras);
287  EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7);
288 
289  // The following are generically exercising the triangulation
290  CameraSet<StereoCamera> cams{w_Camera_cam1, w_Camera_cam2};
291  TriangulationResult result = factor1.triangulateSafe(cams);
292  CHECK(result);
293  EXPECT(assert_equal(landmark, *result, 1e-7));
294 
295  // TEST TRIANGULATION WITH MISSING VALUES: ii) right pixels of both cameras are missing:
296  SmartStereoProjectionFactorPP factor2(model);
297  StereoPoint2 cam1_uv_right_missing(cam1_uv.uL(),missing_uR,cam1_uv.v());
298  factor2.add(cam1_uv_right_missing, x1, body_P_cam1_key, K2);
299  factor2.add(cam2_uv_right_missing, x2, body_P_cam2_key, K2);
300  result = factor2.triangulateSafe(cams);
301  CHECK(result);
302  EXPECT(assert_equal(landmark, *result, 1e-7));
303 }
304 
305 /* *************************************************************************/
306 TEST( SmartStereoProjectionFactorPP, noisy_error_multipleExtrinsics ) {
307  // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
308  Pose3 w_Pose_cam1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2),
309  Point3(0, 0, 1));
310  StereoCamera w_Camera_cam1(w_Pose_cam1, K2);
311 
312  // create second camera 1 meter to the right of first camera
313  Pose3 w_Pose_cam2 = w_Pose_cam1 * Pose3(Rot3(), Point3(1, 0, 0));
314  StereoCamera w_Camera_cam2(w_Pose_cam2, K2);
315 
316  // landmark ~5 meters infront of camera
317  Point3 landmark(5, 0.5, 1.2);
318 
319  // 1. Project two landmarks into two cameras and triangulate
320  StereoPoint2 pixelError(0.2, 0.2, 0);
321  StereoPoint2 cam1_uv = w_Camera_cam1.project(landmark) + pixelError;
322  StereoPoint2 cam2_uv = w_Camera_cam2.project(landmark);
323 
324  // fake extrinsic calibration
325  Pose3 body_Pose_cam1 = Pose3(Rot3::Ypr(-M_PI, 1., 0.1),
326  Point3(0, 1, 0));
327  Pose3 body_Pose_cam2 = Pose3(Rot3::Ypr(-M_PI / 4, 0.1, 1.0),
328  Point3(1, 1, 1));
329  Pose3 w_Pose_body1 = w_Pose_cam1.compose(body_Pose_cam1.inverse());
330  Pose3 w_Pose_body2 = w_Pose_cam2.compose(body_Pose_cam2.inverse());
331 
332  Values values;
333  values.insert(x1, w_Pose_body1);
334  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10),
335  Point3(0.5, 0.1, 0.3));
336  values.insert(x2, w_Pose_body2.compose(noise_pose));
337  values.insert(body_P_cam1_key, body_Pose_cam1);
338  values.insert(body_P_cam2_key, body_Pose_cam2);
339 
340  SmartStereoProjectionFactorPP::shared_ptr factor1(new SmartStereoProjectionFactorPP(model));
341  factor1->add(cam1_uv, x1, body_P_cam1_key, K);
342  factor1->add(cam2_uv, x2, body_P_cam2_key, K);
343 
344  double actualError1 = factor1->error(values);
345 
346  SmartStereoProjectionFactorPP::shared_ptr factor2(new SmartStereoProjectionFactorPP(model));
347  vector<StereoPoint2> measurements;
348  measurements.push_back(cam1_uv);
349  measurements.push_back(cam2_uv);
350 
351  vector<std::shared_ptr<Cal3_S2Stereo> > Ks;
352  Ks.push_back(K);
353  Ks.push_back(K);
354 
355  KeyVector poseKeys;
356  poseKeys.push_back(x1);
357  poseKeys.push_back(x2);
358 
359  KeyVector extrinsicKeys;
360  extrinsicKeys.push_back(body_P_cam1_key);
361  extrinsicKeys.push_back(body_P_cam2_key);
362 
363  factor2->add(measurements, poseKeys, extrinsicKeys, Ks);
364 
365  double actualError2 = factor2->error(values);
366 
367  DOUBLES_EQUAL(actualError1, actualError2, 1e-7);
368  DOUBLES_EQUAL(actualError1, 5381978, 1); // value freeze
369 }
370 
371 /* *************************************************************************/
372 TEST( SmartStereoProjectionFactorPP, 3poses_optimization_multipleExtrinsics ) {
373 
374  // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
375  Pose3 w_Pose_cam1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
376  StereoCamera cam1(w_Pose_cam1, K2);
377 
378  // create second camera 1 meter to the right of first camera
379  Pose3 w_Pose_cam2 = w_Pose_cam1 * Pose3(Rot3(), Point3(1, 0, 0));
380  StereoCamera cam2(w_Pose_cam2, K2);
381 
382  // create third camera 1 meter above the first camera
383  Pose3 w_Pose_cam3 = w_Pose_cam1 * Pose3(Rot3(), Point3(0, -1, 0));
384  StereoCamera cam3(w_Pose_cam3, K2);
385 
386  // three landmarks ~5 meters infront of camera
387  Point3 landmark1(5, 0.5, 1.2);
388  Point3 landmark2(5, -0.5, 1.2);
389  Point3 landmark3(3, 0, 3.0);
390 
391  // 1. Project three landmarks into three cameras and triangulate
392  vector<StereoPoint2> measurements_l1 = stereo_projectToMultipleCameras(cam1,
393  cam2, cam3, landmark1);
394  vector<StereoPoint2> measurements_l2 = stereo_projectToMultipleCameras(cam1,
395  cam2, cam3, landmark2);
396  vector<StereoPoint2> measurements_l3 = stereo_projectToMultipleCameras(cam1,
397  cam2, cam3, landmark3);
398 
399  KeyVector poseKeys;
400  poseKeys.push_back(x1);
401  poseKeys.push_back(x2);
402  poseKeys.push_back(x3);
403 
404  KeyVector extrinsicKeys;
405  extrinsicKeys.push_back(body_P_cam1_key);
406  extrinsicKeys.push_back(body_P_cam2_key);
407  extrinsicKeys.push_back(body_P_cam3_key);
408 
409  SmartStereoProjectionParams smart_params;
410  smart_params.triangulation.enableEPI = true;
411  SmartStereoProjectionFactorPP::shared_ptr smartFactor1(new SmartStereoProjectionFactorPP(model, smart_params));
412  smartFactor1->add(measurements_l1, poseKeys, extrinsicKeys, K2);
413 
414  SmartStereoProjectionFactorPP::shared_ptr smartFactor2(new SmartStereoProjectionFactorPP(model, smart_params));
415  smartFactor2->add(measurements_l2, poseKeys, extrinsicKeys, K2);
416 
417  SmartStereoProjectionFactorPP::shared_ptr smartFactor3(new SmartStereoProjectionFactorPP(model, smart_params));
418  smartFactor3->add(measurements_l3, poseKeys, extrinsicKeys, K2);
419 
420  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
421 
422  // Values
423  Pose3 body_Pose_cam1 = Pose3(Rot3::Ypr(-M_PI, 1., 0.1),Point3(0, 1, 0));
424  Pose3 body_Pose_cam2 = Pose3(Rot3::Ypr(-M_PI / 4, 0.1, 1.0),Point3(1, 1, 1));
425  Pose3 body_Pose_cam3 = Pose3::Identity();
426  Pose3 w_Pose_body1 = w_Pose_cam1.compose(body_Pose_cam1.inverse());
427  Pose3 w_Pose_body2 = w_Pose_cam2.compose(body_Pose_cam2.inverse());
428  Pose3 w_Pose_body3 = w_Pose_cam3.compose(body_Pose_cam3.inverse());
429 
430  Values values;
431  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise
432  values.insert(x1, w_Pose_body1);
433  values.insert(x2, w_Pose_body2);
434  values.insert(x3, w_Pose_body3);
435  values.insert(body_P_cam1_key, body_Pose_cam1);
436  values.insert(body_P_cam2_key, body_Pose_cam2);
437  // initialize third calibration with some noise, we expect it to move back to original pose3
438  values.insert(body_P_cam3_key, body_Pose_cam3 * noise_pose);
439 
440  // Graph
441  NonlinearFactorGraph graph;
442  graph.push_back(smartFactor1);
443  graph.push_back(smartFactor2);
444  graph.push_back(smartFactor3);
445  graph.addPrior(x1, w_Pose_body1, noisePrior);
446  graph.addPrior(x2, w_Pose_body2, noisePrior);
447  graph.addPrior(x3, w_Pose_body3, noisePrior);
448  // we might need some prior on the calibration too
449  graph.addPrior(body_P_cam1_key, body_Pose_cam1, noisePrior);
450  graph.addPrior(body_P_cam2_key, body_Pose_cam2, noisePrior);
451 
452  EXPECT(
453  assert_equal(
454  Pose3(
455  Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598,
456  -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598),
457  Point3(0.1, -0.1, 1.9)), values.at<Pose3>(x3) * values.at<Pose3>(body_P_cam3_key)));
458 
459  // cout << std::setprecision(10) << "\n----SmartStereoFactor graph initial error: " << graph.error(values) << endl;
460  EXPECT_DOUBLES_EQUAL(833953.92789459578, graph.error(values), 1e-7); // initial error (note - this also matches error below)
461 
462  // get triangulated landmarks from smart factors
463  Point3 landmark1_smart = *smartFactor1->point();
464  Point3 landmark2_smart = *smartFactor2->point();
465  Point3 landmark3_smart = *smartFactor3->point();
466 
467  // cost is large before optimization
468  double initialErrorSmart = graph.error(values);
469  EXPECT_DOUBLES_EQUAL(833953.92789459461, initialErrorSmart, 1e-5);
470 
471  Values result;
472  gttic_(SmartStereoProjectionFactorPP);
473  LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
474  result = optimizer.optimize();
475  gttoc_(SmartStereoProjectionFactorPP);
476  tictoc_finishedIteration_();
477 
478  // cout << std::setprecision(10) << "SmartStereoFactor graph optimized error: " << graph.error(result) << endl;
479  EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-5);
480 
481  GaussianFactorGraph::shared_ptr GFG = graph.linearize(result);
482  VectorValues delta = GFG->optimize();
483  VectorValues expected = VectorValues::Zero(delta);
484  EXPECT(assert_equal(expected, delta, 1e-6));
485 
486  // result.print("results of 3 camera, 3 landmark optimization \n");
487  EXPECT(assert_equal(body_Pose_cam3, result.at<Pose3>(body_P_cam3_key)));
488 
489  // ***************************************************************
490  // Same problem with regular Stereo factors, expect same error!
491  // ****************************************************************
492 
493  // add landmarks to values
494  Values values2;
495  values2.insert(x1, w_Pose_cam1); // note: this is the *camera* pose now
496  values2.insert(x2, w_Pose_cam2);
497  values2.insert(x3, w_Pose_cam3 * noise_pose); // equivalent to perturbing the extrinsic calibration
498  values2.insert(L(1), landmark1_smart);
499  values2.insert(L(2), landmark2_smart);
500  values2.insert(L(3), landmark3_smart);
501 
502  // add factors
503  NonlinearFactorGraph graph2;
504 
505  graph2.addPrior(x1, w_Pose_cam1, noisePrior);
506  graph2.addPrior(x2, w_Pose_cam2, noisePrior);
507 
508  typedef GenericStereoFactor<Pose3, Point3> ProjectionFactor;
509 
510  bool verboseCheirality = true;
511 
512  // NOTE: we cannot repeate this with ProjectionFactor, since they are not suitable for stereo calibration
513  graph2.push_back(ProjectionFactor(measurements_l1[0], model, x1, L(1), K2, false, verboseCheirality));
514  graph2.push_back(ProjectionFactor(measurements_l1[1], model, x2, L(1), K2, false, verboseCheirality));
515  graph2.push_back(ProjectionFactor(measurements_l1[2], model, x3, L(1), K2, false, verboseCheirality));
516 
517  graph2.push_back(ProjectionFactor(measurements_l2[0], model, x1, L(2), K2, false, verboseCheirality));
518  graph2.push_back(ProjectionFactor(measurements_l2[1], model, x2, L(2), K2, false, verboseCheirality));
519  graph2.push_back(ProjectionFactor(measurements_l2[2], model, x3, L(2), K2, false, verboseCheirality));
520 
521  graph2.push_back(ProjectionFactor(measurements_l3[0], model, x1, L(3), K2, false, verboseCheirality));
522  graph2.push_back(ProjectionFactor(measurements_l3[1], model, x2, L(3), K2, false, verboseCheirality));
523  graph2.push_back(ProjectionFactor(measurements_l3[2], model, x3, L(3), K2, false, verboseCheirality));
524 
525  // cout << std::setprecision(10) << "\n----StereoFactor graph initial error: " << graph2.error(values) << endl;
526  EXPECT_DOUBLES_EQUAL(833953.92789459578, graph2.error(values2), 1e-7);
527  EXPECT_DOUBLES_EQUAL(initialErrorSmart, graph2.error(values2), 1e-7); // identical to previous case!
528 
529  LevenbergMarquardtOptimizer optimizer2(graph2, values2, lm_params);
530  Values result2 = optimizer2.optimize();
531  EXPECT_DOUBLES_EQUAL(0, graph2.error(result2), 1e-5);
532 }
533 
534 /* *************************************************************************/
535 TEST( SmartStereoProjectionFactorPP, 3poses_error_sameExtrinsicKey ) {
536 
537  // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
538  Pose3 w_Pose_cam1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
539  StereoCamera cam1(w_Pose_cam1, K2);
540 
541  // create second camera 1 meter to the right of first camera
542  Pose3 w_Pose_cam2 = w_Pose_cam1 * Pose3(Rot3(), Point3(1, 0, 0));
543  StereoCamera cam2(w_Pose_cam2, K2);
544 
545  // create third camera 1 meter above the first camera
546  Pose3 w_Pose_cam3 = w_Pose_cam1 * Pose3(Rot3(), Point3(0, -1, 0));
547  StereoCamera cam3(w_Pose_cam3, K2);
548 
549  // three landmarks ~5 meters infront of camera
550  Point3 landmark1(5, 0.5, 1.2);
551  Point3 landmark2(5, -0.5, 1.2);
552  Point3 landmark3(3, 0, 3.0);
553 
554  // 1. Project three landmarks into three cameras and triangulate
555  vector<StereoPoint2> measurements_l1 = stereo_projectToMultipleCameras(cam1,
556  cam2, cam3, landmark1);
557  vector<StereoPoint2> measurements_l2 = stereo_projectToMultipleCameras(cam1,
558  cam2, cam3, landmark2);
559  vector<StereoPoint2> measurements_l3 = stereo_projectToMultipleCameras(cam1,
560  cam2, cam3, landmark3);
561 
562  KeyVector poseKeys;
563  poseKeys.push_back(x1);
564  poseKeys.push_back(x2);
565  poseKeys.push_back(x3);
566 
567  Symbol body_P_cam_key('P', 0);
568  KeyVector extrinsicKeys;
569  extrinsicKeys.push_back(body_P_cam_key);
570  extrinsicKeys.push_back(body_P_cam_key);
571  extrinsicKeys.push_back(body_P_cam_key);
572 
573  SmartStereoProjectionParams smart_params;
574  smart_params.triangulation.enableEPI = true;
575  SmartStereoProjectionFactorPP::shared_ptr smartFactor1(new SmartStereoProjectionFactorPP(model, smart_params));
576  smartFactor1->add(measurements_l1, poseKeys, extrinsicKeys, K2);
577 
578  SmartStereoProjectionFactorPP::shared_ptr smartFactor2(new SmartStereoProjectionFactorPP(model, smart_params));
579  smartFactor2->add(measurements_l2, poseKeys, extrinsicKeys, K2);
580 
581  SmartStereoProjectionFactorPP::shared_ptr smartFactor3(new SmartStereoProjectionFactorPP(model, smart_params));
582  smartFactor3->add(measurements_l3, poseKeys, extrinsicKeys, K2);
583 
584  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
585 
586  // Values
587  Pose3 body_Pose_cam = Pose3(Rot3::Ypr(-M_PI, 1., 0.1),Point3(0, 1, 0));
588  Pose3 w_Pose_body1 = w_Pose_cam1.compose(body_Pose_cam.inverse());
589  Pose3 w_Pose_body2 = w_Pose_cam2.compose(body_Pose_cam.inverse());
590  Pose3 w_Pose_body3 = w_Pose_cam3.compose(body_Pose_cam.inverse());
591 
592  Values values; // all noiseless
593  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.01, 0.01, 0.01)); // smaller noise
594  values.insert(x1, w_Pose_body1);
595  values.insert(x2, w_Pose_body2);
596  values.insert(x3, w_Pose_body3);
597  values.insert(body_P_cam_key, body_Pose_cam);
598 
599  // Graph
600  NonlinearFactorGraph graph;
601  graph.push_back(smartFactor1);
602  graph.push_back(smartFactor2);
603  graph.push_back(smartFactor3);
604  graph.addPrior(x1, w_Pose_body1, noisePrior);
605  graph.addPrior(x2, w_Pose_body2, noisePrior);
606  graph.addPrior(x3, w_Pose_body3, noisePrior);
607 
608  // cost is large before optimization
609  double initialErrorSmart = graph.error(values);
610  EXPECT_DOUBLES_EQUAL(0.0, initialErrorSmart, 1e-5); // initial guess is noisy, so nonzero error
611 }
612 
613 /* *************************************************************************/
614 TEST( SmartStereoProjectionFactorPP, 3poses_noisy_error_sameExtrinsicKey ) {
615 
616  // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
617  Pose3 w_Pose_cam1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
618  StereoCamera cam1(w_Pose_cam1, K2);
619 
620  // create second camera 1 meter to the right of first camera
621  Pose3 w_Pose_cam2 = w_Pose_cam1 * Pose3(Rot3(), Point3(1, 0, 0));
622  StereoCamera cam2(w_Pose_cam2, K2);
623 
624  // create third camera 1 meter above the first camera
625  Pose3 w_Pose_cam3 = w_Pose_cam1 * Pose3(Rot3(), Point3(0, -1, 0));
626  StereoCamera cam3(w_Pose_cam3, K2);
627 
628  // three landmarks ~5 meters infront of camera
629  Point3 landmark1(5, 0.5, 1.2);
630  Point3 landmark2(5, -0.5, 1.2);
631  Point3 landmark3(3, 0, 3.0);
632 
633  // 1. Project three landmarks into three cameras and triangulate
634  vector<StereoPoint2> measurements_l1 = stereo_projectToMultipleCameras(cam1,
635  cam2, cam3, landmark1);
636  vector<StereoPoint2> measurements_l2 = stereo_projectToMultipleCameras(cam1,
637  cam2, cam3, landmark2);
638  vector<StereoPoint2> measurements_l3 = stereo_projectToMultipleCameras(cam1,
639  cam2, cam3, landmark3);
640 
641  double initialError_expected, initialError_actual;
642  {
643  KeyVector poseKeys;
644  poseKeys.push_back(x1);
645  poseKeys.push_back(x2);
646  poseKeys.push_back(x3);
647 
648  KeyVector extrinsicKeys;
649  extrinsicKeys.push_back(body_P_cam1_key);
650  extrinsicKeys.push_back(body_P_cam2_key);
651  extrinsicKeys.push_back(body_P_cam3_key);
652 
653  SmartStereoProjectionParams smart_params;
654  smart_params.triangulation.enableEPI = true;
655  SmartStereoProjectionFactorPP::shared_ptr smartFactor1(new SmartStereoProjectionFactorPP(model, smart_params));
656  smartFactor1->add(measurements_l1, poseKeys, extrinsicKeys, K2);
657 
658  SmartStereoProjectionFactorPP::shared_ptr smartFactor2(new SmartStereoProjectionFactorPP(model, smart_params));
659  smartFactor2->add(measurements_l2, poseKeys, extrinsicKeys, K2);
660 
661  SmartStereoProjectionFactorPP::shared_ptr smartFactor3(new SmartStereoProjectionFactorPP(model, smart_params));
662  smartFactor3->add(measurements_l3, poseKeys, extrinsicKeys, K2);
663 
664  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
665 
666  // Values
667  Pose3 body_Pose_cam1 = Pose3(Rot3::Ypr(-M_PI, 1., 0.1),Point3(0, 1, 0));
668  Pose3 body_Pose_cam2 = body_Pose_cam1;
669  Pose3 body_Pose_cam3 = body_Pose_cam1;
670  Pose3 w_Pose_body1 = w_Pose_cam1.compose(body_Pose_cam1.inverse());
671  Pose3 w_Pose_body2 = w_Pose_cam2.compose(body_Pose_cam2.inverse());
672  Pose3 w_Pose_body3 = w_Pose_cam3.compose(body_Pose_cam3.inverse());
673 
674  Values values;
675  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise
676  values.insert(x1, w_Pose_body1);
677  values.insert(x2, w_Pose_body2);
678  values.insert(x3, w_Pose_body3);
679  values.insert(body_P_cam1_key, body_Pose_cam1 * noise_pose);
680  values.insert(body_P_cam2_key, body_Pose_cam2 * noise_pose);
681  // initialize third calibration with some noise, we expect it to move back to original pose3
682  values.insert(body_P_cam3_key, body_Pose_cam3 * noise_pose);
683 
684  // Graph
685  NonlinearFactorGraph graph;
686  graph.push_back(smartFactor1);
687  graph.push_back(smartFactor2);
688  graph.push_back(smartFactor3);
689  graph.addPrior(x1, w_Pose_body1, noisePrior);
690  graph.addPrior(x2, w_Pose_body2, noisePrior);
691  graph.addPrior(x3, w_Pose_body3, noisePrior);
692 
693  initialError_expected = graph.error(values);
694  }
695 
696  {
697  KeyVector poseKeys;
698  poseKeys.push_back(x1);
699  poseKeys.push_back(x2);
700  poseKeys.push_back(x3);
701 
702  KeyVector extrinsicKeys;
703  extrinsicKeys.push_back(body_P_cam1_key);
704  extrinsicKeys.push_back(body_P_cam1_key);
705  extrinsicKeys.push_back(body_P_cam1_key);
706 
707  SmartStereoProjectionParams smart_params;
708  smart_params.triangulation.enableEPI = true;
709  SmartStereoProjectionFactorPP::shared_ptr smartFactor1(new SmartStereoProjectionFactorPP(model, smart_params));
710  smartFactor1->add(measurements_l1, poseKeys, extrinsicKeys, K2);
711 
712  SmartStereoProjectionFactorPP::shared_ptr smartFactor2(new SmartStereoProjectionFactorPP(model, smart_params));
713  smartFactor2->add(measurements_l2, poseKeys, extrinsicKeys, K2);
714 
715  SmartStereoProjectionFactorPP::shared_ptr smartFactor3(new SmartStereoProjectionFactorPP(model, smart_params));
716  smartFactor3->add(measurements_l3, poseKeys, extrinsicKeys, K2);
717 
718  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
719 
720  // Values
721  Pose3 body_Pose_cam1 = Pose3(Rot3::Ypr(-M_PI, 1., 0.1),Point3(0, 1, 0));
722  Pose3 w_Pose_body1 = w_Pose_cam1.compose(body_Pose_cam1.inverse());
723  Pose3 w_Pose_body2 = w_Pose_cam2.compose(body_Pose_cam1.inverse());
724  Pose3 w_Pose_body3 = w_Pose_cam3.compose(body_Pose_cam1.inverse());
725 
726  Values values;
727  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise
728  values.insert(x1, w_Pose_body1);
729  values.insert(x2, w_Pose_body2);
730  values.insert(x3, w_Pose_body3);
731  values.insert(body_P_cam1_key, body_Pose_cam1 * noise_pose);
732 
733  // Graph
734  NonlinearFactorGraph graph;
735  graph.push_back(smartFactor1);
736  graph.push_back(smartFactor2);
737  graph.push_back(smartFactor3);
738  graph.addPrior(x1, w_Pose_body1, noisePrior);
739  graph.addPrior(x2, w_Pose_body2, noisePrior);
740  graph.addPrior(x3, w_Pose_body3, noisePrior);
741 
742  initialError_actual = graph.error(values);
743  }
744 
745  //std::cout << " initialError_expected " << initialError_expected << std::endl;
746  EXPECT_DOUBLES_EQUAL(initialError_expected, initialError_actual, 1e-7);
747 }
748 
749 /* *************************************************************************/
750 TEST( SmartStereoProjectionFactorPP, 3poses_optimization_sameExtrinsicKey ) {
751 
752  // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
753  Pose3 w_Pose_cam1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
754  StereoCamera cam1(w_Pose_cam1, K2);
755 
756  // create second camera 1 meter to the right of first camera
757  Pose3 w_Pose_cam2 = w_Pose_cam1 * Pose3(Rot3(), Point3(1, 0, 0));
758  StereoCamera cam2(w_Pose_cam2, K2);
759 
760  // create third camera 1 meter above the first camera
761  Pose3 w_Pose_cam3 = w_Pose_cam1 * Pose3(Rot3(), Point3(0, -1, 0));
762  StereoCamera cam3(w_Pose_cam3, K2);
763 
764  // three landmarks ~5 meters infront of camera
765  Point3 landmark1(5, 0.5, 1.2);
766  Point3 landmark2(5, -0.5, 1.2);
767  Point3 landmark3(3, 0, 3.0);
768 
769  // 1. Project three landmarks into three cameras and triangulate
770  vector<StereoPoint2> measurements_l1 = stereo_projectToMultipleCameras(cam1,
771  cam2, cam3, landmark1);
772  vector<StereoPoint2> measurements_l2 = stereo_projectToMultipleCameras(cam1,
773  cam2, cam3, landmark2);
774  vector<StereoPoint2> measurements_l3 = stereo_projectToMultipleCameras(cam1,
775  cam2, cam3, landmark3);
776 
777  KeyVector poseKeys;
778  poseKeys.push_back(x1);
779  poseKeys.push_back(x2);
780  poseKeys.push_back(x3);
781 
782  Symbol body_P_cam_key('P', 0);
783  KeyVector extrinsicKeys;
784  extrinsicKeys.push_back(body_P_cam_key);
785  extrinsicKeys.push_back(body_P_cam_key);
786  extrinsicKeys.push_back(body_P_cam_key);
787 
788  SmartStereoProjectionParams smart_params;
789  smart_params.triangulation.enableEPI = true;
790  SmartStereoProjectionFactorPP::shared_ptr smartFactor1(new SmartStereoProjectionFactorPP(model, smart_params));
791  smartFactor1->add(measurements_l1, poseKeys, extrinsicKeys, K2);
792 
793  SmartStereoProjectionFactorPP::shared_ptr smartFactor2(new SmartStereoProjectionFactorPP(model, smart_params));
794  smartFactor2->add(measurements_l2, poseKeys, extrinsicKeys, K2);
795 
796  SmartStereoProjectionFactorPP::shared_ptr smartFactor3(new SmartStereoProjectionFactorPP(model, smart_params));
797  smartFactor3->add(measurements_l3, poseKeys, extrinsicKeys, K2);
798 
799  // relevant poses:
800  Pose3 body_Pose_cam = Pose3(Rot3::Ypr(-M_PI, 1., 0.1),Point3(0, 1, 0));
801  Pose3 w_Pose_body1 = w_Pose_cam1.compose(body_Pose_cam.inverse());
802  Pose3 w_Pose_body2 = w_Pose_cam2.compose(body_Pose_cam.inverse());
803  Pose3 w_Pose_body3 = w_Pose_cam3.compose(body_Pose_cam.inverse());
804 
805  // Graph
806  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
807  NonlinearFactorGraph graph;
808  graph.push_back(smartFactor1);
809  graph.push_back(smartFactor2);
810  graph.push_back(smartFactor3);
811  graph.addPrior(x1, w_Pose_body1, noisePrior);
812  graph.addPrior(x2, w_Pose_body2, noisePrior);
813  graph.addPrior(x3, w_Pose_body3, noisePrior);
814  // we might need some prior on the calibration too
815  // graph.addPrior(body_P_cam_key, body_Pose_cam, noisePrior); // no need! the measurements will fix this!
816 
817  // Values
818  Values values;
819  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.01, 0.01, 0.01)); // smaller noise
820  values.insert(x1, w_Pose_body1);
821  values.insert(x2, w_Pose_body2);
822  values.insert(x3, w_Pose_body3);
823  values.insert(body_P_cam_key, body_Pose_cam*noise_pose);
824 
825  // cost is large before optimization
826  double initialErrorSmart = graph.error(values);
827  EXPECT_DOUBLES_EQUAL(31986.961831653316, initialErrorSmart, 1e-5); // initial guess is noisy, so nonzero error
828 
830  // What the factor is doing is the following Schur complement math (this matches augmentedHessianPP in code):
831  // size_t numMeasurements = measured_.size();
832  // Matrix F = Matrix::Zero(3*numMeasurements, 6 * nrUniqueKeys);
833  // for(size_t k=0; k<numMeasurements; k++){
834  // Key key_body = w_P_body_keys_.at(k);
835  // Key key_cal = body_P_cam_keys_.at(k);
836  // F.block<3,6>( 3*k , 6*keyToSlotMap[key_body] ) = Fs[k].block<3,6>(0,0);
837  // F.block<3,6>( 3*k , 6*keyToSlotMap[key_cal] ) = Fs[k].block<3,6>(0,6);
838  // }
839  // Matrix augH = Matrix::Zero(6*nrUniqueKeys+1,6*nrUniqueKeys+1);
840  // augH.block(0,0,6*nrUniqueKeys,6*nrUniqueKeys) = F.transpose() * F - F.transpose() * E * P * E.transpose() * F;
841  // Matrix infoVec = F.transpose() * b - F.transpose() * E * P * E.transpose() * b;
842  // augH.block(0,6*nrUniqueKeys,6*nrUniqueKeys,1) = infoVec;
843  // augH.block(6*nrUniqueKeys,0,1,6*nrUniqueKeys) = infoVec.transpose();
844  // augH(6*nrUniqueKeys,6*nrUniqueKeys) = b.squaredNorm();
845  // // The following is close to zero:
846  // std::cout << "norm diff: \n"<< Matrix(augH - Matrix(augmentedHessianUniqueKeys.selfadjointView())).lpNorm<Eigen::Infinity>() << std::endl;
847  // std::cout << "TEST MATRIX:" << std::endl;
848  // augmentedHessianUniqueKeys = SymmetricBlockMatrix(dims, augH);
850 
851  Values result;
852  gttic_(SmartStereoProjectionFactorPP);
853  LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
854  result = optimizer.optimize();
855  gttoc_(SmartStereoProjectionFactorPP);
856  tictoc_finishedIteration_();
857 
858  EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-5);
859 
860  // This passes on my machine but gets and indeterminant linear system exception in CI.
861  // This is a very redundant test, so it's not a problem to omit.
862  // GaussianFactorGraph::shared_ptr GFG = graph.linearize(result);
863  // Matrix H = GFG->hessian().first;
864  // double det = H.determinant();
865  // // std::cout << "det " << det << std::endl; // det = 2.27581e+80 (so it's not underconstrained)
866  // VectorValues delta = GFG->optimize();
867  // VectorValues expected = VectorValues::Zero(delta);
868  // EXPECT(assert_equal(expected, delta, 1e-4));
869 }
870 
871 /* *************************************************************************/
872 TEST( SmartStereoProjectionFactorPP, 3poses_optimization_2ExtrinsicKeys ) {
873 
874  // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
875  Pose3 w_Pose_cam1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
876  StereoCamera cam1(w_Pose_cam1, K2);
877 
878  // create second camera 1 meter to the right of first camera
879  Pose3 w_Pose_cam2 = w_Pose_cam1 * Pose3(Rot3(), Point3(1, 0, 0));
880  StereoCamera cam2(w_Pose_cam2, K2);
881 
882  // create third camera 1 meter above the first camera
883  Pose3 w_Pose_cam3 = w_Pose_cam1 * Pose3(Rot3(), Point3(0, -1, 0));
884  StereoCamera cam3(w_Pose_cam3, K2);
885 
886  // three landmarks ~5 meters infront of camera
887  Point3 landmark1(5, 0.5, 1.2);
888  Point3 landmark2(5, -0.5, 1.2);
889  Point3 landmark3(3, 0, 3.0);
890 
891  // 1. Project three landmarks into three cameras and triangulate
892  vector<StereoPoint2> measurements_l1 = stereo_projectToMultipleCameras(cam1,
893  cam2, cam3, landmark1);
894  vector<StereoPoint2> measurements_l2 = stereo_projectToMultipleCameras(cam1,
895  cam2, cam3, landmark2);
896  vector<StereoPoint2> measurements_l3 = stereo_projectToMultipleCameras(cam1,
897  cam2, cam3, landmark3);
898 
899  KeyVector poseKeys;
900  poseKeys.push_back(x1);
901  poseKeys.push_back(x2);
902  poseKeys.push_back(x3);
903 
904  KeyVector extrinsicKeys;
905  extrinsicKeys.push_back(body_P_cam1_key);
906  extrinsicKeys.push_back(body_P_cam1_key);
907  extrinsicKeys.push_back(body_P_cam3_key);
908 
909  SmartStereoProjectionParams smart_params;
910  smart_params.triangulation.enableEPI = true;
911  SmartStereoProjectionFactorPP::shared_ptr smartFactor1(new SmartStereoProjectionFactorPP(model, smart_params));
912  smartFactor1->add(measurements_l1, poseKeys, extrinsicKeys, K2);
913 
914  SmartStereoProjectionFactorPP::shared_ptr smartFactor2(new SmartStereoProjectionFactorPP(model, smart_params));
915  smartFactor2->add(measurements_l2, poseKeys, extrinsicKeys, K2);
916 
917  SmartStereoProjectionFactorPP::shared_ptr smartFactor3(new SmartStereoProjectionFactorPP(model, smart_params));
918  smartFactor3->add(measurements_l3, poseKeys, extrinsicKeys, K2);
919 
920  // relevant poses:
921  Pose3 body_Pose_cam = Pose3(Rot3::Ypr(-M_PI, 1., 0.1),Point3(0, 1, 0));
922  Pose3 w_Pose_body1 = w_Pose_cam1.compose(body_Pose_cam.inverse());
923  Pose3 w_Pose_body2 = w_Pose_cam2.compose(body_Pose_cam.inverse());
924  Pose3 w_Pose_body3 = w_Pose_cam3.compose(body_Pose_cam.inverse());
925 
926  // Graph
927  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
928  NonlinearFactorGraph graph;
929  graph.push_back(smartFactor1);
930  graph.push_back(smartFactor2);
931  graph.push_back(smartFactor3);
932  graph.addPrior(x1, w_Pose_body1, noisePrior);
933  graph.addPrior(x2, w_Pose_body2, noisePrior);
934  graph.addPrior(x3, w_Pose_body3, noisePrior);
935  // graph.addPrior(body_P_cam1_key, body_Pose_cam, noisePrior);
936  // we might need some prior on the calibration too
937  // graph.addPrior(body_P_cam_key, body_Pose_cam, noisePrior); // no need! the measurements will fix this!
938 
939  // Values
940  Values values;
941  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.01, 0.01, 0.01)); // smaller noise
942  values.insert(x1, w_Pose_body1);
943  values.insert(x2, w_Pose_body2);
944  values.insert(x3, w_Pose_body3);
945  values.insert(body_P_cam1_key, body_Pose_cam*noise_pose);
946  values.insert(body_P_cam3_key, body_Pose_cam*noise_pose);
947 
948  // cost is large before optimization
949  double initialErrorSmart = graph.error(values);
950  EXPECT_DOUBLES_EQUAL(31986.961831653316, initialErrorSmart, 1e-5); // initial guess is noisy, so nonzero error
951 
952  Values result;
953  gttic_(SmartStereoProjectionFactorPP);
954  LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
955  result = optimizer.optimize();
956  gttoc_(SmartStereoProjectionFactorPP);
957  tictoc_finishedIteration_();
958 
959  EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-5);
960 
961  // NOTE: the following would fail since the problem is underconstrained (while LM above works just fine!)
962  // GaussianFactorGraph::shared_ptr GFG = graph.linearize(result);
963  // VectorValues delta = GFG->optimize();
964  // VectorValues expected = VectorValues::Zero(delta);
965  // EXPECT(assert_equal(expected, delta, 1e-4));
966 }
967 
968 /* *************************************************************************/
969 TEST( SmartStereoProjectionFactorPP, monocular_multipleExtrinsicKeys ){
970  // make a realistic calibration matrix
971  double fov = 60; // degrees
972  size_t w=640,h=480;
973 
974  Cal3_S2::shared_ptr K(new Cal3_S2(fov,w,h));
975 
976  // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
977  Pose3 cameraPose1 = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); // body poses
978  Pose3 cameraPose2 = cameraPose1 * Pose3(Rot3(), Point3(1,0,0));
979  Pose3 cameraPose3 = cameraPose1 * Pose3(Rot3(), Point3(0,-1,0));
980 
981  PinholeCamera<Cal3_S2> cam1(cameraPose1, *K); // with camera poses
982  PinholeCamera<Cal3_S2> cam2(cameraPose2, *K);
983  PinholeCamera<Cal3_S2> cam3(cameraPose3, *K);
984 
985  // create arbitrary body_Pose_sensor (transforms from sensor to body)
986  Pose3 sensor_to_body = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(1, 1, 1)); // Pose3(); //
987 
988  // These are the poses we want to estimate, from camera measurements
989  Pose3 bodyPose1 = cameraPose1.compose(sensor_to_body.inverse());
990  Pose3 bodyPose2 = cameraPose2.compose(sensor_to_body.inverse());
991  Pose3 bodyPose3 = cameraPose3.compose(sensor_to_body.inverse());
992 
993  // three landmarks ~5 meters infront of camera
994  Point3 landmark1(5, 0.5, 1.2);
995  Point3 landmark2(5, -0.5, 1.2);
996  Point3 landmark3(5, 0, 3.0);
997 
998  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
999 
1000  // Project three landmarks into three cameras
1001  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
1002  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
1003  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
1004 
1005  // convert measurement to (degenerate) stereoPoint2 (with right pixel being NaN)
1006  vector<StereoPoint2> measurements_cam1_stereo, measurements_cam2_stereo, measurements_cam3_stereo;
1007  for(size_t k=0; k<measurements_cam1.size();k++)
1008  measurements_cam1_stereo.push_back(StereoPoint2(measurements_cam1[k].x() , missing_uR , measurements_cam1[k].y()));
1009 
1010  for(size_t k=0; k<measurements_cam2.size();k++)
1011  measurements_cam2_stereo.push_back(StereoPoint2(measurements_cam2[k].x() , missing_uR , measurements_cam2[k].y()));
1012 
1013  for(size_t k=0; k<measurements_cam3.size();k++)
1014  measurements_cam3_stereo.push_back(StereoPoint2(measurements_cam3[k].x() , missing_uR , measurements_cam3[k].y()));
1015 
1016  KeyVector poseKeys;
1017  poseKeys.push_back(x1);
1018  poseKeys.push_back(x2);
1019  poseKeys.push_back(x3);
1020 
1021  Symbol body_P_cam_key('P', 0);
1022  KeyVector extrinsicKeys;
1023  extrinsicKeys.push_back(body_P_cam_key);
1024  extrinsicKeys.push_back(body_P_cam_key);
1025  extrinsicKeys.push_back(body_P_cam_key);
1026 
1027  SmartStereoProjectionParams smart_params;
1028  smart_params.setRankTolerance(1.0);
1029  smart_params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY);
1030  smart_params.setEnableEPI(false);
1031 
1032  Cal3_S2Stereo::shared_ptr Kmono(new Cal3_S2Stereo(fov,w,h,b));
1033 
1034  SmartStereoProjectionFactorPP smartFactor1(model, smart_params);
1035  smartFactor1.add(measurements_cam1_stereo, poseKeys, extrinsicKeys, Kmono);
1036 
1037  SmartStereoProjectionFactorPP smartFactor2(model, smart_params);
1038  smartFactor2.add(measurements_cam2_stereo, poseKeys, extrinsicKeys, Kmono);
1039 
1040  SmartStereoProjectionFactorPP smartFactor3(model, smart_params);
1041  smartFactor3.add(measurements_cam3_stereo, poseKeys, extrinsicKeys, Kmono);
1042 
1043  // Graph
1044  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
1045  NonlinearFactorGraph graph;
1046  graph.push_back(smartFactor1);
1047  graph.push_back(smartFactor2);
1048  graph.push_back(smartFactor3);
1049  graph.addPrior(x1, bodyPose1, noisePrior);
1050  graph.addPrior(x2, bodyPose2, noisePrior);
1051  graph.addPrior(x3, bodyPose3, noisePrior);
1052  // we might need some prior on the calibration too
1053  // graph.addPrior(body_P_cam_key, body_Pose_cam, noisePrior); // no need! the measurements will fix this!
1054 
1055  // Check errors at ground truth poses
1056  Values gtValues;
1057  gtValues.insert(x1, bodyPose1);
1058  gtValues.insert(x2, bodyPose2);
1059  gtValues.insert(x3, bodyPose3);
1060  gtValues.insert(body_P_cam_key, sensor_to_body);
1061  double actualError = graph.error(gtValues);
1062  double expectedError = 0.0;
1063  DOUBLES_EQUAL(expectedError, actualError, 1e-7)
1064 
1065  // noisy values
1066  Values values;
1067  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.01, 0.01, 0.01)); // smaller noise
1068  values.insert(x1, bodyPose1);
1069  values.insert(x2, bodyPose2);
1070  values.insert(x3, bodyPose3);
1071  values.insert(body_P_cam_key, sensor_to_body*noise_pose);
1072 
1073  // cost is large before optimization
1074  double initialErrorSmart = graph.error(values);
1075  EXPECT_DOUBLES_EQUAL(2379.0012816261642, initialErrorSmart, 1e-5); // freeze value
1076 
1077  Values result;
1078  gttic_(SmartStereoProjectionFactorPP);
1079  LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
1080  result = optimizer.optimize();
1081  gttoc_(SmartStereoProjectionFactorPP);
1082  tictoc_finishedIteration_();
1083 
1084  EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-5);
1085  EXPECT(assert_equal(sensor_to_body,result.at<Pose3>(body_P_cam_key), 1e-1));
1086 }
1087 
1088 /* *************************************************************************/
1089 TEST( SmartStereoProjectionFactorPP, landmarkDistance ) {
1090 
1091  // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
1092  Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
1093  StereoCamera cam1(pose1, K);
1094  // create second camera 1 meter to the right of first camera
1095  Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0));
1096  StereoCamera cam2(pose2, K);
1097  // create third camera 1 meter above the first camera
1098  Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0));
1099  StereoCamera cam3(pose3, K);
1100 
1101  KeyVector views;
1102  views.push_back(x1);
1103  views.push_back(x2);
1104  views.push_back(x3);
1105 
1106  Symbol body_P_cam_key('P', 0);
1107  KeyVector extrinsicKeys;
1108  extrinsicKeys.push_back(body_P_cam_key);
1109  extrinsicKeys.push_back(body_P_cam_key);
1110  extrinsicKeys.push_back(body_P_cam_key);
1111 
1112  // three landmarks ~5 meters infront of camera
1113  Point3 landmark1(5, 0.5, 1.2);
1114  Point3 landmark2(5, -0.5, 1.2);
1115  Point3 landmark3(3, 0, 3.0);
1116 
1117  // 1. Project three landmarks into three cameras and triangulate
1118  vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1,
1119  cam2, cam3, landmark1);
1120  vector<StereoPoint2> measurements_cam2 = stereo_projectToMultipleCameras(cam1,
1121  cam2, cam3, landmark2);
1122  vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1,
1123  cam2, cam3, landmark3);
1124 
1125  SmartStereoProjectionParams params;
1126  params.setLinearizationMode(HESSIAN);
1127  params.setLandmarkDistanceThreshold(2);
1128 
1129  SmartStereoProjectionFactorPP::shared_ptr smartFactor1(new SmartStereoProjectionFactorPP(model, params));
1130  smartFactor1->add(measurements_cam1, views, extrinsicKeys, K);
1131 
1132  SmartStereoProjectionFactorPP::shared_ptr smartFactor2(new SmartStereoProjectionFactorPP(model, params));
1133  smartFactor2->add(measurements_cam2, views, extrinsicKeys, K);
1134 
1135  SmartStereoProjectionFactorPP::shared_ptr smartFactor3(new SmartStereoProjectionFactorPP(model, params));
1136  smartFactor3->add(measurements_cam3, views, extrinsicKeys, K);
1137 
1138  // create graph
1139  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
1140  NonlinearFactorGraph graph;
1141  graph.push_back(smartFactor1);
1142  graph.push_back(smartFactor2);
1143  graph.push_back(smartFactor3);
1144  graph.addPrior(x1, pose1, noisePrior);
1145  graph.addPrior(x2, pose2, noisePrior);
1146  graph.addPrior(body_P_cam_key, Pose3::Identity(), noisePrior);
1147 
1148  // 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
1149  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
1150  Point3(0.1, 0.1, 0.1)); // smaller noise
1151  Values values;
1152  values.insert(x1, pose1);
1153  values.insert(x2, pose2);
1154  values.insert(x3, pose3 * noise_pose);
1155  values.insert(body_P_cam_key, Pose3::Identity());
1156 
1157  // All smart factors are disabled and pose should remain where it is
1158  Values result;
1159  LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
1160  result = optimizer.optimize();
1161  EXPECT(assert_equal(values.at<Pose3>(x3), result.at<Pose3>(x3), 1e-5));
1162  EXPECT_DOUBLES_EQUAL(graph.error(values), graph.error(result), 1e-5);
1163 }
1164 
1165 /* *************************************************************************/
1166 TEST( SmartStereoProjectionFactorPP, dynamicOutlierRejection ) {
1167 
1168  KeyVector views;
1169  views.push_back(x1);
1170  views.push_back(x2);
1171  views.push_back(x3);
1172 
1173  Symbol body_P_cam_key('P', 0);
1174  KeyVector extrinsicKeys;
1175  extrinsicKeys.push_back(body_P_cam_key);
1176  extrinsicKeys.push_back(body_P_cam_key);
1177  extrinsicKeys.push_back(body_P_cam_key);
1178 
1179  // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
1180  Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
1181  StereoCamera cam1(pose1, K);
1182  // create second camera 1 meter to the right of first camera
1183  Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0));
1184  StereoCamera cam2(pose2, K);
1185  // create third camera 1 meter above the first camera
1186  Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0));
1187  StereoCamera cam3(pose3, K);
1188 
1189  // three landmarks ~5 meters infront of camera
1190  Point3 landmark1(5, 0.5, 1.2);
1191  Point3 landmark2(5, -0.5, 1.2);
1192  Point3 landmark3(3, 0, 3.0);
1193  Point3 landmark4(5, -0.5, 1);
1194 
1195  // 1. Project four landmarks into three cameras and triangulate
1196  vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1,
1197  cam2, cam3, landmark1);
1198  vector<StereoPoint2> measurements_cam2 = stereo_projectToMultipleCameras(cam1,
1199  cam2, cam3, landmark2);
1200  vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1,
1201  cam2, cam3, landmark3);
1202  vector<StereoPoint2> measurements_cam4 = stereo_projectToMultipleCameras(cam1,
1203  cam2, cam3, landmark4);
1204 
1205  measurements_cam4.at(0) = measurements_cam4.at(0) + StereoPoint2(10, 10, 1); // add outlier
1206 
1207  SmartStereoProjectionParams params;
1208  params.setLinearizationMode(HESSIAN);
1209  params.setDynamicOutlierRejectionThreshold(1);
1210 
1211  SmartStereoProjectionFactorPP::shared_ptr smartFactor1(new SmartStereoProjectionFactorPP(model, params));
1212  smartFactor1->add(measurements_cam1, views, extrinsicKeys, K);
1213 
1214  SmartStereoProjectionFactorPP::shared_ptr smartFactor2(new SmartStereoProjectionFactorPP(model, params));
1215  smartFactor2->add(measurements_cam2, views, extrinsicKeys, K);
1216 
1217  SmartStereoProjectionFactorPP::shared_ptr smartFactor3(new SmartStereoProjectionFactorPP(model, params));
1218  smartFactor3->add(measurements_cam3, views, extrinsicKeys, K);
1219 
1220  SmartStereoProjectionFactorPP::shared_ptr smartFactor4(new SmartStereoProjectionFactorPP(model, params));
1221  smartFactor4->add(measurements_cam4, views, extrinsicKeys, K);
1222 
1223  // same as factor 4, but dynamic outlier rejection is off
1224  SmartStereoProjectionFactorPP::shared_ptr smartFactor4b(new SmartStereoProjectionFactorPP(model));
1225  smartFactor4b->add(measurements_cam4, views, extrinsicKeys, K);
1226 
1227  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
1228 
1229  NonlinearFactorGraph graph;
1230  graph.push_back(smartFactor1);
1231  graph.push_back(smartFactor2);
1232  graph.push_back(smartFactor3);
1233  graph.push_back(smartFactor4);
1234  graph.addPrior(x1, pose1, noisePrior);
1235  graph.addPrior(x2, pose2, noisePrior);
1236  graph.addPrior(x3, pose3, noisePrior);
1237 
1238  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
1239  Point3(0.1, 0.1, 0.1)); // smaller noise
1240  Values values;
1241  values.insert(x1, pose1);
1242  values.insert(x2, pose2);
1243  values.insert(x3, pose3);
1244  values.insert(body_P_cam_key, Pose3::Identity());
1245 
1246  EXPECT_DOUBLES_EQUAL(0, smartFactor1->error(values), 1e-9);
1247  EXPECT_DOUBLES_EQUAL(0, smartFactor2->error(values), 1e-9);
1248  EXPECT_DOUBLES_EQUAL(0, smartFactor3->error(values), 1e-9);
1249  // zero error due to dynamic outlier rejection
1250  EXPECT_DOUBLES_EQUAL(0, smartFactor4->error(values), 1e-9);
1251 
1252  // dynamic outlier rejection is off
1253  EXPECT_DOUBLES_EQUAL(6147.3947317473921, smartFactor4b->error(values), 1e-9);
1254 
1255  // Factors 1-3 should have valid point, factor 4 should not
1256  EXPECT(smartFactor1->point());
1257  EXPECT(smartFactor2->point());
1258  EXPECT(smartFactor3->point());
1259  EXPECT(smartFactor4->point().outlier());
1260  EXPECT(smartFactor4b->point());
1261 
1262  // Factor 4 is disabled, pose 3 stays put
1263  Values result;
1264  LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
1265  result = optimizer.optimize();
1266  EXPECT(assert_equal(Pose3::Identity(), result.at<Pose3>(body_P_cam_key)));
1267 }
1268 
1269 /* ************************************************************************* */
1270 int main() {
1271  TestResult tr;
1272  return TestRegistry::runAllTests(tr);
1273 }
1274 /* ************************************************************************* */
1275 
simple_graph::factor1
auto factor1
Definition: testJacobianFactor.cpp:187
CHECK
#define CHECK(condition)
Definition: Test.h:108
simple_graph::factor2
auto factor2
Definition: testJacobianFactor.cpp:194
cam1
static PinholeCamera< Cal3_S2 > cam1(pose3, cal1)
y
Scalar * y
Definition: level1_cplx_impl.h:124
gtsam::NonlinearOptimizer::optimize
virtual const Values & optimize()
Definition: NonlinearOptimizer.h:98
TEST
TEST(SmartStereoProjectionFactorPP, params)
Definition: testSmartStereoProjectionFactorPP.cpp:90
vanillaPoseRS::cam3
Camera cam3(interp_pose3, sharedK)
b
Scalar * b
Definition: benchVecAdd.cpp:17
gttic_
#define gttic_(label)
Definition: timing.h:245
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
gtsam::Values::at
const ValueType at(Key j) const
Definition: Values-inl.h:204
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
gtsam::SmartProjectionParams::setRetriangulationThreshold
void setRetriangulationThreshold(double retriangulationTh)
Definition: SmartFactorParams.h:103
cam2
static StereoCamera cam2(pose3, cal4ptr)
DOUBLES_EQUAL
#define DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:141
gtsam::FactorGraph::push_back
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:190
gtsam::SmartProjectionParams::setEnableEPI
void setEnableEPI(bool enableEPI)
Definition: SmartFactorParams.h:109
vanillaPose::Cameras
CameraSet< Camera > Cameras
Definition: smartFactorScenarios.h:76
gtsam::SmartStereoProjectionFactorPP::error
double error(const Values &values) const override
Definition: SmartStereoProjectionFactorPP.cpp:103
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
M_PI
#define M_PI
Definition: main.h:106
leaf::values
leaf::MyValues values
gtsam::SmartProjectionParams::triangulation
TriangulationParameters triangulation
Definition: SmartFactorParams.h:49
K
static Cal3_S2 K(500, 500, 0.1, 640/2, 480/2)
L
MatrixXd L
Definition: LLT_example.cpp:6
std
Definition: BFloat16.h:88
x2
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
measurement2
static Point2 measurement2(200.0, 220.0)
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
gtsam::CameraSet
A set of cameras, all with their own calibration.
Definition: CameraSet.h:36
landmark1
Point2 landmark1(5.0, 1.5)
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
EXPECT_DOUBLES_EQUAL
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:161
gtsam::SmartProjectionParams::getDegeneracyMode
DegeneracyMode getDegeneracyMode() const
Definition: SmartFactorParams.h:81
gtsam::Pose3::inverse
Pose3 inverse() const
inverse transformation with derivatives
Definition: Pose3.cpp:49
vanilla::params
static const SmartProjectionParams params
Definition: smartFactorScenarios.h:69
landmark
static Point3 landmark(0, 0, 5)
gtsam::NonlinearFactorGraph::linearize
std::shared_ptr< GaussianFactorGraph > linearize(const Values &linearizationPoint) const
Linearize a nonlinear factor graph.
Definition: NonlinearFactorGraph.cpp:239
gtsam::NonlinearFactorGraph::addPrior
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
Definition: NonlinearFactorGraph.h:199
gtsam::SmartStereoProjectionFactor::totalReprojectionError
double totalReprojectionError(const Cameras &cameras, std::optional< Point3 > externalPoint={}) const
Definition: SmartStereoProjectionFactor.h:420
gtsam::SmartProjectionParams::setRankTolerance
void setRankTolerance(double rankTol)
Definition: SmartFactorParams.h:106
PoseTranslationPrior.h
Implements a prior on the translation component of a pose.
K2
static const Cal3Bundler K2(625, 1e-3, 1e-3)
gtsam::SmartStereoProjectionFactorPP::shared_ptr
std::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
Definition: SmartStereoProjectionFactorPP.h:65
landmark2
Point2 landmark2(7.0, 1.5)
gtsam::SmartProjectionParams::getRetriangulationThreshold
double getRetriangulationThreshold() const
Definition: SmartFactorParams.h:93
gtsam::Cal3_S2::shared_ptr
std::shared_ptr< Cal3_S2 > shared_ptr
Definition: Cal3_S2.h:39
result
Values result
Definition: OdometryOptimize.cpp:8
gtsam::Cal3_S2Stereo
The most common 5DOF 3D->2D calibration, stereo version.
Definition: Cal3_S2Stereo.h:30
gtsam::TriangulationParameters::enableEPI
bool enableEPI
if set to true, will refine triangulation using LM
Definition: triangulation.h:562
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:560
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
pose3
static Pose3 pose3(rt3, pt3)
gtsam::StereoPoint2::v
double v() const
get v
Definition: StereoPoint2.h:116
measurement1
static Point2 measurement1(323.0, 240.0)
projectToMultipleCameras
void projectToMultipleCameras(const CAMERA &cam1, const CAMERA &cam2, const CAMERA &cam3, Point3 landmark, typename CAMERA::MeasurementVector &measurements_cam)
Definition: smartFactorScenarios.h:162
gtsam::SmartProjectionParams::setLandmarkDistanceThreshold
void setLandmarkDistanceThreshold(double landmarkDistanceThreshold)
Definition: SmartFactorParams.h:112
TEST_UNSAFE
TEST_UNSAFE(SmartStereoProjectionFactorPP, noiseless_error_identityExtrinsics)
Definition: testSmartStereoProjectionFactorPP.cpp:162
LevenbergMarquardtOptimizer.h
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
simple_graph::factor3
auto factor3
Definition: testJacobianFactor.cpp:201
ProjectionFactorPPP.h
Derived from ProjectionFactor, but estimates body-camera transform in addition to body pose and 3D la...
gtsam::Cal3_S2
The most common 5DOF 3D->2D calibration.
Definition: Cal3_S2.h:34
gtsam::StereoCamera::project
StereoPoint2 project(const Point3 &point) const
Project 3D point to StereoPoint2 (uL,uR,v)
Definition: StereoCamera.cpp:32
gtsam::GaussianFactorGraph::shared_ptr
std::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactorGraph.h:82
gtsam::NonlinearFactorGraph::error
double error(const Values &values) const
Definition: NonlinearFactorGraph.cpp:170
gtsam::LieGroup::compose
Class compose(const Class &g) const
Definition: Lie.h:48
ProjectionFactor.h
Reprojection of a LANDMARK to a 2D point.
w
RowVector3d w
Definition: Matrix_resize_int.cpp:3
x3
Pose3 x3(Rot3::Ypr(M_PI/4.0, 0.0, 0.0), l2)
gtsam::Cal3_S2Stereo::shared_ptr
std::shared_ptr< Cal3_S2Stereo > shared_ptr
Definition: Cal3_S2Stereo.h:38
pose1
static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5))
StereoFactor.h
A non-linear factor for stereo measurements.
simple::graph2
NonlinearFactorGraph graph2()
Definition: testInitializePose3.cpp:72
gtsam::SharedDiagonal
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:743
poseKey1
static Key poseKey1(X(1))
gtsam
traits
Definition: chartTesting.h:28
gtsam::SmartStereoProjectionFactor::triangulateSafe
TriangulationResult triangulateSafe(const Cameras &cameras) const
triangulateSafe
Definition: SmartStereoProjectionFactor.h:167
gtsam::SmartStereoProjectionFactorPP::add
void add(const StereoPoint2 &measured, const Key &world_P_body_key, const Key &body_P_cam_key, const std::shared_ptr< Cal3_S2Stereo > &K)
Definition: SmartStereoProjectionFactorPP.cpp:28
h
const double h
Definition: testSimpleHelicopter.cpp:19
HESSIAN
#define HESSIAN
Definition: timeSchurFactors.cpp:24
gtsam::SmartProjectionParams::setDegeneracyMode
void setDegeneracyMode(DegeneracyMode degMode)
Definition: SmartFactorParams.h:100
gtsam::StereoPoint2::uL
double uL() const
get uL
Definition: StereoPoint2.h:110
gtsam::SmartStereoProjectionFactorPP::cameras
Base::Cameras cameras(const Values &values) const override
Definition: SmartStereoProjectionFactorPP.cpp:112
x1
Pose3 x1
Definition: testPose3.cpp:663
gtsam::Point2Vector
std::vector< Point2, Eigen::aligned_allocator< Point2 > > Point2Vector
Definition: Point2.h:49
p
float * p
Definition: Tutorial_Map_using.cpp:9
simple::pose2
static Pose3 pose2
Definition: testInitializePose3.cpp:58
gtsam::Values::insert
void insert(Key j, const Value &val)
Definition: Values.cpp:155
gtsam::SmartProjectionParams::setLinearizationMode
void setLinearizationMode(LinearizationMode linMode)
Definition: SmartFactorParams.h:97
gtsam::tictoc_finishedIteration_
void tictoc_finishedIteration_()
Definition: timing.h:264
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
gtsam::SmartProjectionParams::getLinearizationMode
LinearizationMode getLinearizationMode() const
Definition: SmartFactorParams.h:78
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
X
#define X
Definition: icosphere.cpp:20
gttoc_
#define gttoc_(label)
Definition: timing.h:250
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::SmartProjectionParams::setDynamicOutlierRejectionThreshold
void setDynamicOutlierRejectionThreshold(double dynOutRejectionThreshold)
Definition: SmartFactorParams.h:115
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
fov
static double fov
Definition: testProjectionFactor.cpp:33
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:741
gtsam::SmartProjectionParams::getTriangulationParameters
TriangulationParameters getTriangulationParameters() const
Definition: SmartFactorParams.h:84
SmartStereoProjectionFactorPP.h
Smart stereo factor on poses and extrinsic calibration.

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