testSmartProjectionPoseFactorRollingShutter.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 <CppUnitLite/TestHarness.h>
20 #include <gtsam/base/numericalDerivative.h>
21 #include <gtsam/base/serializationTestHelpers.h>
22 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
23 #include <gtsam/slam/PoseTranslationPrior.h>
24 #include <gtsam/slam/ProjectionFactor.h>
25 #include <gtsam_unstable/slam/ProjectionFactorRollingShutter.h>
26 #include <gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h>
27 
28 #include <iostream>
29 
30 #include "gtsam/slam/tests/smartFactorScenarios.h"
31 #define DISABLE_TIMING
32 
33 using namespace gtsam;
34 using namespace std::placeholders;
35 
36 static const double rankTol = 1.0;
37 // Create a noise model for the pixel error
38 static const double sigma = 0.1;
39 static SharedIsotropic model(noiseModel::Isotropic::Sigma(2, sigma));
40 
41 // Convenience for named keys
42 using symbol_shorthand::L;
43 using symbol_shorthand::X;
44 
45 // tests data
46 static Symbol x1('X', 1);
47 static Symbol x2('X', 2);
48 static Symbol x3('X', 3);
49 static Symbol x4('X', 4);
50 static Symbol l0('L', 0);
51 static Pose3 body_P_sensor =
52  Pose3(Rot3::Ypr(-0.1, 0.2, -0.2), Point3(0.1, 0.0, 0.0));
53 
54 static Point2 measurement1(323.0, 240.0);
55 static Point2 measurement2(200.0, 220.0);
56 static Point2 measurement3(320.0, 10.0);
57 static double interp_factor = 0.5;
58 static double interp_factor1 = 0.3;
59 static double interp_factor2 = 0.4;
60 static double interp_factor3 = 0.5;
61 
62 static size_t cameraId1 = 0;
63 
64 /* ************************************************************************* */
65 // default Cal3_S2 poses with rolling shutter effect
66 namespace vanillaPoseRS {
67 typedef PinholePose<Cal3_S2> Camera;
68 typedef CameraSet<Camera> Cameras;
69 static Cal3_S2::shared_ptr sharedK(new Cal3_S2(fov, w, h));
70 Pose3 interp_pose1 = interpolate<Pose3>(level_pose, pose_right, interp_factor1);
71 Pose3 interp_pose2 = interpolate<Pose3>(pose_right, pose_above, interp_factor2);
72 Pose3 interp_pose3 = interpolate<Pose3>(pose_above, level_pose, interp_factor3);
73 Camera cam1(interp_pose1, sharedK);
74 Camera cam2(interp_pose2, sharedK);
75 Camera cam3(interp_pose3, sharedK);
76 SmartProjectionParams params(
77  gtsam::HESSIAN,
78  gtsam::ZERO_ON_DEGENERACY); // only config that works with RS factors
79 } // namespace vanillaPoseRS
80 
81 LevenbergMarquardtParams lmParams;
82 typedef SmartProjectionPoseFactorRollingShutter<PinholePose<Cal3_S2>>
83  SmartFactorRS;
84 
85 /* ************************************************************************* */
86 TEST(SmartProjectionPoseFactorRollingShutter, Constructor) {
87  using namespace vanillaPoseRS;
88  std::shared_ptr<Cameras> cameraRig(new Cameras());
89  cameraRig->push_back(Camera(Pose3::Identity(), sharedK));
90  SmartFactorRS::shared_ptr factor1(
91  new SmartFactorRS(model, cameraRig, params));
92 }
93 
94 /* ************************************************************************* */
95 TEST(SmartProjectionPoseFactorRollingShutter, Constructor2) {
96  using namespace vanillaPoseRS;
97  std::shared_ptr<Cameras> cameraRig(new Cameras());
98  cameraRig->push_back(Camera(Pose3::Identity(), sharedK));
99  params.setRankTolerance(rankTol);
100  SmartFactorRS factor1(model, cameraRig, params);
101 }
102 
103 /* ************************************************************************* */
104 TEST(SmartProjectionPoseFactorRollingShutter, add) {
105  using namespace vanillaPoseRS;
106  std::shared_ptr<Cameras> cameraRig(new Cameras());
107  cameraRig->push_back(Camera(Pose3::Identity(), sharedK));
108  SmartFactorRS::shared_ptr factor1(
109  new SmartFactorRS(model, cameraRig, params));
110  factor1->add(measurement1, x1, x2, interp_factor);
111 }
112 
113 /* ************************************************************************* */
114 TEST(SmartProjectionPoseFactorRollingShutter, Equals) {
115  using namespace vanillaPoseRS;
116 
117  // create fake measurements
118  Point2Vector measurements;
119  measurements.push_back(measurement1);
120  measurements.push_back(measurement2);
121  measurements.push_back(measurement3);
122 
123  std::vector<std::pair<Key, Key>> key_pairs;
124  key_pairs.push_back(std::make_pair(x1, x2));
125  key_pairs.push_back(std::make_pair(x2, x3));
126  key_pairs.push_back(std::make_pair(x3, x4));
127 
128  std::vector<double> interp_factors;
129  interp_factors.push_back(interp_factor1);
130  interp_factors.push_back(interp_factor2);
131  interp_factors.push_back(interp_factor3);
132 
133  FastVector<size_t> cameraIds{0, 0, 0};
134 
135  std::shared_ptr<Cameras> cameraRig(new Cameras());
136  cameraRig->push_back(Camera(body_P_sensor, sharedK));
137 
138  // create by adding a batch of measurements with a bunch of calibrations
139  SmartFactorRS::shared_ptr factor2(
140  new SmartFactorRS(model, cameraRig, params));
141  factor2->add(measurements, key_pairs, interp_factors, cameraIds);
142 
143  // create by adding a batch of measurements with a single calibrations
144  SmartFactorRS::shared_ptr factor3(
145  new SmartFactorRS(model, cameraRig, params));
146  factor3->add(measurements, key_pairs, interp_factors, cameraIds);
147 
148  { // create equal factors and show equal returns true
149  SmartFactorRS::shared_ptr factor1(
150  new SmartFactorRS(model, cameraRig, params));
151  factor1->add(measurement1, x1, x2, interp_factor1, cameraId1);
152  factor1->add(measurement2, x2, x3, interp_factor2, cameraId1);
153  factor1->add(measurement3, x3, x4, interp_factor3, cameraId1);
154 
155  EXPECT(factor1->equals(*factor2));
156  EXPECT(factor1->equals(*factor3));
157  }
158  { // create equal factors and show equal returns true (use default cameraId)
159  SmartFactorRS::shared_ptr factor1(
160  new SmartFactorRS(model, cameraRig, params));
161  factor1->add(measurement1, x1, x2, interp_factor1);
162  factor1->add(measurement2, x2, x3, interp_factor2);
163  factor1->add(measurement3, x3, x4, interp_factor3);
164 
165  EXPECT(factor1->equals(*factor2));
166  EXPECT(factor1->equals(*factor3));
167  }
168  { // create equal factors and show equal returns true (use default cameraId)
169  SmartFactorRS::shared_ptr factor1(
170  new SmartFactorRS(model, cameraRig, params));
171  factor1->add(measurements, key_pairs, interp_factors);
172 
173  EXPECT(factor1->equals(*factor2));
174  EXPECT(factor1->equals(*factor3));
175  }
176  { // create slightly different factors (different keys) and show equal
177  // returns false (use default cameraIds)
178  SmartFactorRS::shared_ptr factor1(
179  new SmartFactorRS(model, cameraRig, params));
180  factor1->add(measurement1, x1, x2, interp_factor1, cameraId1);
181  factor1->add(measurement2, x2, x2, interp_factor2,
182  cameraId1); // different!
183  factor1->add(measurement3, x3, x4, interp_factor3, cameraId1);
184 
185  EXPECT(!factor1->equals(*factor2));
186  EXPECT(!factor1->equals(*factor3));
187  }
188  { // create slightly different factors (different extrinsics) and show equal
189  // returns false
190  std::shared_ptr<Cameras> cameraRig2(new Cameras());
191  cameraRig2->push_back(Camera(body_P_sensor * body_P_sensor, sharedK));
192  SmartFactorRS::shared_ptr factor1(
193  new SmartFactorRS(model, cameraRig2, params));
194  factor1->add(measurement1, x1, x2, interp_factor1, cameraId1);
195  factor1->add(measurement2, x2, x3, interp_factor2,
196  cameraId1); // different!
197  factor1->add(measurement3, x3, x4, interp_factor3, cameraId1);
198 
199  EXPECT(!factor1->equals(*factor2));
200  EXPECT(!factor1->equals(*factor3));
201  }
202  { // create slightly different factors (different interp factors) and show
203  // equal returns false
204  SmartFactorRS::shared_ptr factor1(
205  new SmartFactorRS(model, cameraRig, params));
206  factor1->add(measurement1, x1, x2, interp_factor1, cameraId1);
207  factor1->add(measurement2, x2, x3, interp_factor1,
208  cameraId1); // different!
209  factor1->add(measurement3, x3, x4, interp_factor3, cameraId1);
210 
211  EXPECT(!factor1->equals(*factor2));
212  EXPECT(!factor1->equals(*factor3));
213  }
214 }
215 
216 static const int DimBlock = 12;
217 static const int ZDim = 2;
219 typedef Eigen::Matrix<double, ZDim, DimBlock>
220  MatrixZD; // F blocks (derivatives wrt camera)
221 typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD>>
222  FBlocks; // vector of F blocks
223 
224 /* *************************************************************************/
225 TEST(SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians) {
226  using namespace vanillaPoseRS;
227 
228  // Project two landmarks into two cameras
229  Point2 level_uv = cam1.project(landmark1);
230  Point2 level_uv_right = cam2.project(landmark1);
231  Pose3 body_P_sensorId = Pose3::Identity();
232 
233  std::shared_ptr<Cameras> cameraRig(new Cameras());
234  cameraRig->push_back(Camera(body_P_sensorId, sharedK));
235 
236  SmartFactorRS factor(model, cameraRig, params);
237  factor.add(level_uv, x1, x2, interp_factor1);
238  factor.add(level_uv_right, x2, x3, interp_factor2);
239 
240  Values values; // it's a pose factor, hence these are poses
241  values.insert(x1, level_pose);
242  values.insert(x2, pose_right);
243  values.insert(x3, pose_above);
244 
245  double actualError = factor.error(values);
246  double expectedError = 0.0;
247  EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7);
248 
249  // Check triangulation
250  factor.triangulateSafe(factor.cameras(values));
251  TriangulationResult point = factor.point();
252  EXPECT(point.valid()); // check triangulated point is valid
253  EXPECT(assert_equal(
254  landmark1,
255  *point)); // check triangulation result matches expected 3D landmark
256 
257  // Check Jacobians
258  // -- actual Jacobians
259  FBlocks actualFs;
260  Matrix actualE;
261  Vector actualb;
262  factor.computeJacobiansWithTriangulatedPoint(actualFs, actualE, actualb,
263  values);
264  EXPECT(actualE.rows() == 4);
265  EXPECT(actualE.cols() == 3);
266  EXPECT(actualb.rows() == 4);
267  EXPECT(actualb.cols() == 1);
268  EXPECT(actualFs.size() == 2);
269 
270  // -- expected Jacobians from ProjectionFactorsRollingShutter
271  ProjectionFactorRollingShutter factor1(level_uv, interp_factor1, model, x1,
272  x2, l0, sharedK, body_P_sensorId);
273  Matrix expectedF11, expectedF12, expectedE1;
274  Vector expectedb1 = factor1.evaluateError(
275  level_pose, pose_right, landmark1, expectedF11, expectedF12, expectedE1);
276  EXPECT(
277  assert_equal(expectedF11, Matrix(actualFs[0].block(0, 0, 2, 6)), 1e-5));
278  EXPECT(
279  assert_equal(expectedF12, Matrix(actualFs[0].block(0, 6, 2, 6)), 1e-5));
280  EXPECT(assert_equal(expectedE1, Matrix(actualE.block(0, 0, 2, 3)), 1e-5));
281  // by definition computeJacobiansWithTriangulatedPoint returns minus
282  // reprojectionError
283  EXPECT(assert_equal(expectedb1, -Vector(actualb.segment<2>(0)), 1e-5));
284 
285  ProjectionFactorRollingShutter factor2(level_uv_right, interp_factor2, model,
286  x2, x3, l0, sharedK, body_P_sensorId);
287  Matrix expectedF21, expectedF22, expectedE2;
288  Vector expectedb2 = factor2.evaluateError(
289  pose_right, pose_above, landmark1, expectedF21, expectedF22, expectedE2);
290  EXPECT(
291  assert_equal(expectedF21, Matrix(actualFs[1].block(0, 0, 2, 6)), 1e-5));
292  EXPECT(
293  assert_equal(expectedF22, Matrix(actualFs[1].block(0, 6, 2, 6)), 1e-5));
294  EXPECT(assert_equal(expectedE2, Matrix(actualE.block(2, 0, 2, 3)), 1e-5));
295  // by definition computeJacobiansWithTriangulatedPoint returns minus
296  // reprojectionError
297  EXPECT(assert_equal(expectedb2, -Vector(actualb.segment<2>(2)), 1e-5));
298 }
299 
300 /* *************************************************************************/
301 TEST(SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians) {
302  // also includes non-identical extrinsic calibration
303  using namespace vanillaPoseRS;
304 
305  // Project two landmarks into two cameras
306  Point2 pixelError(0.5, 1.0);
307  Point2 level_uv = cam1.project(landmark1) + pixelError;
308  Point2 level_uv_right = cam2.project(landmark1);
309  Pose3 body_P_sensorNonId = body_P_sensor;
310 
311  std::shared_ptr<Cameras> cameraRig(new Cameras());
312  cameraRig->push_back(Camera(body_P_sensorNonId, sharedK));
313 
314  SmartFactorRS factor(model, cameraRig, params);
315  factor.add(level_uv, x1, x2, interp_factor1);
316  factor.add(level_uv_right, x2, x3, interp_factor2);
317 
318  Values values; // it's a pose factor, hence these are poses
319  values.insert(x1, level_pose);
320  values.insert(x2, pose_right);
321  values.insert(x3, pose_above);
322 
323  // Perform triangulation
324  factor.triangulateSafe(factor.cameras(values));
325  TriangulationResult point = factor.point();
326  EXPECT(point.valid()); // check triangulated point is valid
327  Point3 landmarkNoisy = *point;
328 
329  // Check Jacobians
330  // -- actual Jacobians
331  FBlocks actualFs;
332  Matrix actualE;
333  Vector actualb;
334  factor.computeJacobiansWithTriangulatedPoint(actualFs, actualE, actualb,
335  values);
336  EXPECT(actualE.rows() == 4);
337  EXPECT(actualE.cols() == 3);
338  EXPECT(actualb.rows() == 4);
339  EXPECT(actualb.cols() == 1);
340  EXPECT(actualFs.size() == 2);
341 
342  // -- expected Jacobians from ProjectionFactorsRollingShutter
343  ProjectionFactorRollingShutter factor1(level_uv, interp_factor1, model, x1,
344  x2, l0, sharedK, body_P_sensorNonId);
345  Matrix expectedF11, expectedF12, expectedE1;
346  Vector expectedb1 =
347  factor1.evaluateError(level_pose, pose_right, landmarkNoisy, expectedF11,
348  expectedF12, expectedE1);
349  EXPECT(
350  assert_equal(expectedF11, Matrix(actualFs[0].block(0, 0, 2, 6)), 1e-5));
351  EXPECT(
352  assert_equal(expectedF12, Matrix(actualFs[0].block(0, 6, 2, 6)), 1e-5));
353  EXPECT(assert_equal(expectedE1, Matrix(actualE.block(0, 0, 2, 3)), 1e-5));
354  // by definition computeJacobiansWithTriangulatedPoint returns minus
355  // reprojectionError
356  EXPECT(assert_equal(expectedb1, -Vector(actualb.segment<2>(0)), 1e-5));
357 
358  ProjectionFactorRollingShutter factor2(level_uv_right, interp_factor2, model,
359  x2, x3, l0, sharedK,
360  body_P_sensorNonId);
361  Matrix expectedF21, expectedF22, expectedE2;
362  Vector expectedb2 =
363  factor2.evaluateError(pose_right, pose_above, landmarkNoisy, expectedF21,
364  expectedF22, expectedE2);
365  EXPECT(
366  assert_equal(expectedF21, Matrix(actualFs[1].block(0, 0, 2, 6)), 1e-5));
367  EXPECT(
368  assert_equal(expectedF22, Matrix(actualFs[1].block(0, 6, 2, 6)), 1e-5));
369  EXPECT(assert_equal(expectedE2, Matrix(actualE.block(2, 0, 2, 3)), 1e-5));
370  // by definition computeJacobiansWithTriangulatedPoint returns minus
371  // reprojectionError
372  EXPECT(assert_equal(expectedb2, -Vector(actualb.segment<2>(2)), 1e-5));
373 
374  // Check errors
375  double actualError = factor.error(values); // from smart factor
376  NonlinearFactorGraph nfg;
377  nfg.add(factor1);
378  nfg.add(factor2);
379  values.insert(l0, landmarkNoisy);
380  double expectedError = nfg.error(values);
381  EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7);
382 }
383 
384 /* *************************************************************************/
385 TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses) {
386  using namespace vanillaPoseRS;
387  Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3;
388 
389  // Project three landmarks into three cameras
390  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1);
391  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2);
392  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3);
393 
394  // create inputs
395  std::vector<std::pair<Key, Key>> key_pairs;
396  key_pairs.push_back(std::make_pair(x1, x2));
397  key_pairs.push_back(std::make_pair(x2, x3));
398  key_pairs.push_back(std::make_pair(x3, x1));
399 
400  std::vector<double> interp_factors;
401  interp_factors.push_back(interp_factor1);
402  interp_factors.push_back(interp_factor2);
403  interp_factors.push_back(interp_factor3);
404 
405  std::shared_ptr<Cameras> cameraRig(new Cameras());
406  cameraRig->push_back(Camera(Pose3::Identity(), sharedK));
407 
408  SmartFactorRS::shared_ptr smartFactor1(
409  new SmartFactorRS(model, cameraRig, params));
410  smartFactor1->add(measurements_lmk1, key_pairs, interp_factors);
411 
412  SmartFactorRS::shared_ptr smartFactor2(
413  new SmartFactorRS(model, cameraRig, params));
414  smartFactor2->add(measurements_lmk2, key_pairs, interp_factors);
415 
416  SmartFactorRS::shared_ptr smartFactor3(
417  new SmartFactorRS(model, cameraRig, params));
418  smartFactor3->add(measurements_lmk3, key_pairs, interp_factors);
419 
420  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
421 
422  NonlinearFactorGraph graph;
423  graph.push_back(smartFactor1);
424  graph.push_back(smartFactor2);
425  graph.push_back(smartFactor3);
426  graph.addPrior(x1, level_pose, noisePrior);
427  graph.addPrior(x2, pose_right, noisePrior);
428 
429  Values groundTruth;
430  groundTruth.insert(x1, level_pose);
431  groundTruth.insert(x2, pose_right);
432  groundTruth.insert(x3, pose_above);
433  DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9);
434 
435  // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10),
436  // Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
437  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
438  Point3(0.1, 0.1, 0.1)); // smaller noise
439  Values values;
440  values.insert(x1, level_pose);
441  values.insert(x2, pose_right);
442  // initialize third pose with some noise, we expect it to move back to
443  // original pose_above
444  values.insert(x3, pose_above * noise_pose);
445  EXPECT( // check that the pose is actually noisy
446  assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656,
447  -0.0313952598, -0.000986635786, 0.0314107591,
448  -0.999013364, -0.0313952598),
449  Point3(0.1, -0.1, 1.9)),
450  values.at<Pose3>(x3)));
451 
452  Values result;
453  LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
454  result = optimizer.optimize();
455  EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-6));
456 }
457 
458 /* *************************************************************************/
459 TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_multiCam) {
460  using namespace vanillaPoseRS;
461  Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3;
462 
463  // Project three landmarks into three cameras
464  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1);
465  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2);
466  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3);
467 
468  // create inputs
469  std::vector<std::pair<Key, Key>> key_pairs;
470  key_pairs.push_back(std::make_pair(x1, x2));
471  key_pairs.push_back(std::make_pair(x2, x3));
472  key_pairs.push_back(std::make_pair(x3, x1));
473 
474  std::vector<double> interp_factors;
475  interp_factors.push_back(interp_factor1);
476  interp_factors.push_back(interp_factor2);
477  interp_factors.push_back(interp_factor3);
478 
479  std::shared_ptr<Cameras> cameraRig(new Cameras());
480  cameraRig->push_back(Camera(body_P_sensor, sharedK));
481  cameraRig->push_back(Camera(Pose3::Identity(), sharedK));
482 
483  SmartFactorRS::shared_ptr smartFactor1(
484  new SmartFactorRS(model, cameraRig, params));
485  smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, {1, 1, 1});
486 
487  SmartFactorRS::shared_ptr smartFactor2(
488  new SmartFactorRS(model, cameraRig, params));
489  smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, {1, 1, 1});
490 
491  SmartFactorRS::shared_ptr smartFactor3(
492  new SmartFactorRS(model, cameraRig, params));
493  smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, {1, 1, 1});
494 
495  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
496 
497  NonlinearFactorGraph graph;
498  graph.push_back(smartFactor1);
499  graph.push_back(smartFactor2);
500  graph.push_back(smartFactor3);
501  graph.addPrior(x1, level_pose, noisePrior);
502  graph.addPrior(x2, pose_right, noisePrior);
503 
504  Values groundTruth;
505  groundTruth.insert(x1, level_pose);
506  groundTruth.insert(x2, pose_right);
507  groundTruth.insert(x3, pose_above); // pose above is the pose of the camera
508  DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9);
509 
510  // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10),
511  // Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
512  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
513  Point3(0.1, 0.1, 0.1)); // smaller noise
514  Values values;
515  values.insert(x1, level_pose);
516  values.insert(x2, pose_right);
517  // initialize third pose with some noise, we expect it to move back to
518  // original pose_above
519  values.insert(x3, pose_above * noise_pose);
520  EXPECT( // check that the pose is actually noisy
521  assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656,
522  -0.0313952598, -0.000986635786, 0.0314107591,
523  -0.999013364, -0.0313952598),
524  Point3(0.1, -0.1, 1.9)),
525  values.at<Pose3>(x3)));
526 
527  Values result;
528  LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
529  result = optimizer.optimize();
530  EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-6));
531 }
532 
533 /* *************************************************************************/
534 TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_multiCam2) {
535  using namespace vanillaPoseRS;
536 
537  Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3;
538 
539  // create arbitrary body_T_sensor (transforms from sensor to body)
540  Pose3 body_T_sensor1 = Pose3(Rot3::Ypr(-0.03, 0., 0.01), Point3(1, 1, 1));
541  Pose3 body_T_sensor2 = Pose3(Rot3::Ypr(-0.1, 0., 0.05), Point3(0, 0, 1));
542  Pose3 body_T_sensor3 = Pose3(Rot3::Ypr(-0.3, 0., -0.05), Point3(0, 1, 1));
543 
544  Camera camera1(interp_pose1 * body_T_sensor1, sharedK);
545  Camera camera2(interp_pose2 * body_T_sensor2, sharedK);
546  Camera camera3(interp_pose3 * body_T_sensor3, sharedK);
547 
548  // Project three landmarks into three cameras
549  projectToMultipleCameras(camera1, camera2, camera3, landmark1,
550  measurements_lmk1);
551  projectToMultipleCameras(camera1, camera2, camera3, landmark2,
552  measurements_lmk2);
553  projectToMultipleCameras(camera1, camera2, camera3, landmark3,
554  measurements_lmk3);
555 
556  // create inputs
557  std::vector<std::pair<Key, Key>> key_pairs;
558  key_pairs.push_back(std::make_pair(x1, x2));
559  key_pairs.push_back(std::make_pair(x2, x3));
560  key_pairs.push_back(std::make_pair(x3, x1));
561 
562  std::vector<double> interp_factors;
563  interp_factors.push_back(interp_factor1);
564  interp_factors.push_back(interp_factor2);
565  interp_factors.push_back(interp_factor3);
566 
567  std::shared_ptr<Cameras> cameraRig(new Cameras());
568  cameraRig->push_back(Camera(body_T_sensor1, sharedK));
569  cameraRig->push_back(Camera(body_T_sensor2, sharedK));
570  cameraRig->push_back(Camera(body_T_sensor3, sharedK));
571 
572  SmartFactorRS::shared_ptr smartFactor1(
573  new SmartFactorRS(model, cameraRig, params));
574  smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, {0, 1, 2});
575 
576  SmartFactorRS::shared_ptr smartFactor2(
577  new SmartFactorRS(model, cameraRig, params));
578  smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, {0, 1, 2});
579 
580  SmartFactorRS::shared_ptr smartFactor3(
581  new SmartFactorRS(model, cameraRig, params));
582  smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, {0, 1, 2});
583 
584  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
585 
586  NonlinearFactorGraph graph;
587  graph.push_back(smartFactor1);
588  graph.push_back(smartFactor2);
589  graph.push_back(smartFactor3);
590  graph.addPrior(x1, level_pose, noisePrior);
591  graph.addPrior(x2, pose_right, noisePrior);
592 
593  Values groundTruth;
594  groundTruth.insert(x1, level_pose);
595  groundTruth.insert(x2, pose_right);
596  groundTruth.insert(x3, pose_above); // pose above is the pose of the camera
597  DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9);
598 
599  // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10),
600  // Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
601  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
602  Point3(0.1, 0.1, 0.1)); // smaller noise
603  Values values;
604  values.insert(x1, level_pose);
605  values.insert(x2, pose_right);
606  // initialize third pose with some noise, we expect it to move back to
607  // original pose_above
608  values.insert(x3, pose_above * noise_pose);
609  EXPECT( // check that the pose is actually noisy
610  assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656,
611  -0.0313952598, -0.000986635786, 0.0314107591,
612  -0.999013364, -0.0313952598),
613  Point3(0.1, -0.1, 1.9)),
614  values.at<Pose3>(x3)));
615 
616  Values result;
617  LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
618  result = optimizer.optimize();
619  EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-4));
620 }
621 
622 /* *************************************************************************/
623 TEST(SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses) {
624  // here we replicate a test in SmartProjectionPoseFactor by setting
625  // interpolation factors to 0 and 1 (such that the rollingShutter measurements
626  // falls back to standard pixel measurements) Note: this is a quite extreme
627  // test since in typical camera you would not have more than 1 measurement per
628  // landmark at each interpolated pose
629  using namespace vanillaPoseRS;
630 
631  // Default cameras for simple derivatives
632  static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0));
633 
634  Rot3 R = Rot3::Identity();
635  Pose3 pose1 = Pose3(R, Point3(0, 0, 0));
636  Pose3 pose2 = Pose3(R, Point3(1, 0, 0));
637  Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple);
638  Pose3 body_P_sensorId = Pose3::Identity();
639 
640  // one landmarks 1m in front of camera
641  Point3 landmark1(0, 0, 10);
642 
643  Point2Vector measurements_lmk1;
644 
645  // Project 2 landmarks into 2 cameras
646  measurements_lmk1.push_back(cam1.project(landmark1));
647  measurements_lmk1.push_back(cam2.project(landmark1));
648 
649  std::shared_ptr<Cameras> cameraRig(new Cameras());
650  cameraRig->push_back(Camera(body_P_sensorId, sharedKSimple));
651 
652  SmartFactorRS::shared_ptr smartFactor1(
653  new SmartFactorRS(model, cameraRig, params));
654  double interp_factor = 0; // equivalent to measurement taken at pose 1
655  smartFactor1->add(measurements_lmk1[0], x1, x2, interp_factor);
656  interp_factor = 1; // equivalent to measurement taken at pose 2
657  smartFactor1->add(measurements_lmk1[1], x1, x2, interp_factor);
658 
659  SmartFactorRS::Cameras cameras;
660  cameras.push_back(cam1);
661  cameras.push_back(cam2);
662 
663  // Make sure triangulation works
664  EXPECT(smartFactor1->triangulateSafe(cameras));
665  EXPECT(!smartFactor1->isDegenerate());
666  EXPECT(!smartFactor1->isPointBehindCamera());
667  std::optional<Point3> p = smartFactor1->point();
668  EXPECT(p);
669  EXPECT(assert_equal(landmark1, *p));
670 
671  VectorValues zeroDelta;
672  Vector6 delta;
673  delta.setZero();
674  zeroDelta.insert(x1, delta);
675  zeroDelta.insert(x2, delta);
676 
677  VectorValues perturbedDelta;
678  delta.setOnes();
679  perturbedDelta.insert(x1, delta);
680  perturbedDelta.insert(x2, delta);
681  double expectedError = 2500;
682 
683  // After eliminating the point, A1 and A2 contain 2-rank information on
684  // cameras:
685  Matrix16 A1, A2;
686  A1 << -10, 0, 0, 0, 1, 0;
687  A2 << 10, 0, 1, 0, -1, 0;
688  A1 *= 10. / sigma;
689  A2 *= 10. / sigma;
690  Matrix expectedInformation; // filled below
691 
692  // createHessianFactor
693  Matrix66 G11 = 0.5 * A1.transpose() * A1;
694  Matrix66 G12 = 0.5 * A1.transpose() * A2;
695  Matrix66 G22 = 0.5 * A2.transpose() * A2;
696 
697  Vector6 g1;
698  g1.setZero();
699  Vector6 g2;
700  g2.setZero();
701 
702  double f = 0;
703 
704  RegularHessianFactor<6> expected(x1, x2, G11, G12, g1, G22, g2, f);
705  expectedInformation = expected.information();
706 
707  Values values;
708  values.insert(x1, pose1);
709  values.insert(x2, pose2);
710  std::shared_ptr<RegularHessianFactor<6>> actual =
711  smartFactor1->createHessianFactor(values);
712  EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6));
713  EXPECT(assert_equal(expected, *actual, 1e-6));
714  EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6);
715  EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6);
716 }
717 
718 /* *************************************************************************/
719 TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI) {
720  using namespace vanillaPoseRS;
721  Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3;
722 
723  // Project three landmarks into three cameras
724  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1);
725  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2);
726  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3);
727 
728  // create inputs
729  std::vector<std::pair<Key, Key>> key_pairs;
730  key_pairs.push_back(std::make_pair(x1, x2));
731  key_pairs.push_back(std::make_pair(x2, x3));
732  key_pairs.push_back(std::make_pair(x3, x1));
733 
734  std::vector<double> interp_factors;
735  interp_factors.push_back(interp_factor1);
736  interp_factors.push_back(interp_factor2);
737  interp_factors.push_back(interp_factor3);
738 
739  double excludeLandmarksFutherThanDist = 1e10; // very large
740  SmartProjectionParams params;
741  params.setRankTolerance(1.0);
742  params.setLinearizationMode(gtsam::HESSIAN);
743  params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY);
744  params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist);
745  params.setEnableEPI(true);
746 
747  std::shared_ptr<Cameras> cameraRig(new Cameras());
748  cameraRig->push_back(Camera(Pose3::Identity(), sharedK));
749 
750  SmartFactorRS smartFactor1(model, cameraRig, params);
751  smartFactor1.add(measurements_lmk1, key_pairs, interp_factors);
752 
753  SmartFactorRS smartFactor2(model, cameraRig, params);
754  smartFactor2.add(measurements_lmk2, key_pairs, interp_factors);
755 
756  SmartFactorRS smartFactor3(model, cameraRig, params);
757  smartFactor3.add(measurements_lmk3, key_pairs, interp_factors);
758 
759  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
760 
761  NonlinearFactorGraph graph;
762  graph.push_back(smartFactor1);
763  graph.push_back(smartFactor2);
764  graph.push_back(smartFactor3);
765  graph.addPrior(x1, level_pose, noisePrior);
766  graph.addPrior(x2, pose_right, noisePrior);
767 
768  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
769  Point3(0.1, 0.1, 0.1)); // smaller noise
770  Values values;
771  values.insert(x1, level_pose);
772  values.insert(x2, pose_right);
773  // initialize third pose with some noise, we expect it to move back to
774  // original pose_above
775  values.insert(x3, pose_above * noise_pose);
776 
777  // Optimization should correct 3rd pose
778  Values result;
779  LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
780  result = optimizer.optimize();
781  EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-6));
782 }
783 
784 /* *************************************************************************/
785 TEST(SmartProjectionPoseFactorRollingShutter,
786  optimization_3poses_landmarkDistance) {
787  using namespace vanillaPoseRS;
788  Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3;
789 
790  // Project three landmarks into three cameras
791  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1);
792  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2);
793  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3);
794 
795  // create inputs
796  std::vector<std::pair<Key, Key>> key_pairs;
797  key_pairs.push_back(std::make_pair(x1, x2));
798  key_pairs.push_back(std::make_pair(x2, x3));
799  key_pairs.push_back(std::make_pair(x3, x1));
800 
801  std::vector<double> interp_factors;
802  interp_factors.push_back(interp_factor1);
803  interp_factors.push_back(interp_factor2);
804  interp_factors.push_back(interp_factor3);
805 
806  double excludeLandmarksFutherThanDist = 2;
807  SmartProjectionParams params;
808  params.setRankTolerance(1.0);
809  params.setLinearizationMode(gtsam::HESSIAN);
810  // params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); // this would give an
811  // exception as expected
812  params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY);
813  params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist);
814  params.setEnableEPI(false);
815 
816  std::shared_ptr<Cameras> cameraRig(new Cameras());
817  cameraRig->push_back(Camera(Pose3::Identity(), sharedK));
818 
819  SmartFactorRS smartFactor1(model, cameraRig, params);
820  smartFactor1.add(measurements_lmk1, key_pairs, interp_factors);
821 
822  SmartFactorRS smartFactor2(model, cameraRig, params);
823  smartFactor2.add(measurements_lmk2, key_pairs, interp_factors);
824 
825  SmartFactorRS smartFactor3(model, cameraRig, params);
826  smartFactor3.add(measurements_lmk3, key_pairs, interp_factors);
827 
828  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
829 
830  NonlinearFactorGraph graph;
831  graph.push_back(smartFactor1);
832  graph.push_back(smartFactor2);
833  graph.push_back(smartFactor3);
834  graph.addPrior(x1, level_pose, noisePrior);
835  graph.addPrior(x2, pose_right, noisePrior);
836 
837  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
838  Point3(0.1, 0.1, 0.1)); // smaller noise
839  Values values;
840  values.insert(x1, level_pose);
841  values.insert(x2, pose_right);
842  // initialize third pose with some noise, we expect it to move back to
843  // original pose_above
844  values.insert(x3, pose_above * noise_pose);
845 
846  // All factors are disabled (due to the distance threshold) and pose should
847  // remain where it is
848  Values result;
849  LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
850  result = optimizer.optimize();
851  EXPECT(assert_equal(values.at<Pose3>(x3), result.at<Pose3>(x3)));
852 }
853 
854 /* *************************************************************************/
855 TEST(SmartProjectionPoseFactorRollingShutter,
856  optimization_3poses_dynamicOutlierRejection) {
857  using namespace vanillaPoseRS;
858  // add fourth landmark
859  Point3 landmark4(5, -0.5, 1);
860 
861  Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3,
862  measurements_lmk4;
863  // Project 4 landmarks into cameras
864  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1);
865  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2);
866  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3);
867  projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_lmk4);
868  measurements_lmk4.at(0) =
869  measurements_lmk4.at(0) + Point2(10, 10); // add outlier
870 
871  // create inputs
872  std::vector<std::pair<Key, Key>> key_pairs;
873  key_pairs.push_back(std::make_pair(x1, x2));
874  key_pairs.push_back(std::make_pair(x2, x3));
875  key_pairs.push_back(std::make_pair(x3, x1));
876 
877  std::vector<double> interp_factors;
878  interp_factors.push_back(interp_factor1);
879  interp_factors.push_back(interp_factor2);
880  interp_factors.push_back(interp_factor3);
881 
882  double excludeLandmarksFutherThanDist = 1e10;
883  double dynamicOutlierRejectionThreshold =
884  3; // max 3 pixel of average reprojection error
885 
886  SmartProjectionParams params;
887  params.setRankTolerance(1.0);
888  params.setLinearizationMode(gtsam::HESSIAN);
889  params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY);
890  params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist);
891  params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold);
892  params.setEnableEPI(false);
893 
894  std::shared_ptr<Cameras> cameraRig(new Cameras());
895  cameraRig->push_back(Camera(Pose3::Identity(), sharedK));
896 
897  SmartFactorRS::shared_ptr smartFactor1(
898  new SmartFactorRS(model, cameraRig, params));
899  smartFactor1->add(measurements_lmk1, key_pairs, interp_factors);
900 
901  SmartFactorRS::shared_ptr smartFactor2(
902  new SmartFactorRS(model, cameraRig, params));
903  smartFactor2->add(measurements_lmk2, key_pairs, interp_factors);
904 
905  SmartFactorRS::shared_ptr smartFactor3(
906  new SmartFactorRS(model, cameraRig, params));
907  smartFactor3->add(measurements_lmk3, key_pairs, interp_factors);
908 
909  SmartFactorRS::shared_ptr smartFactor4(
910  new SmartFactorRS(model, cameraRig, params));
911  smartFactor4->add(measurements_lmk4, key_pairs, interp_factors);
912 
913  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
914 
915  NonlinearFactorGraph graph;
916  graph.push_back(smartFactor1);
917  graph.push_back(smartFactor2);
918  graph.push_back(smartFactor3);
919  graph.push_back(smartFactor4);
920  graph.addPrior(x1, level_pose, noisePrior);
921  graph.addPrior(x2, pose_right, noisePrior);
922 
923  Pose3 noise_pose = Pose3(
924  Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
925  Point3(0.01, 0.01,
926  0.01)); // smaller noise, otherwise outlier rejection will kick in
927  Values values;
928  values.insert(x1, level_pose);
929  values.insert(x2, pose_right);
930  // initialize third pose with some noise, we expect it to move back to
931  // original pose_above
932  values.insert(x3, pose_above * noise_pose);
933 
934  // Optimization should correct 3rd pose
935  Values result;
936  LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
937  result = optimizer.optimize();
938  EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-6));
939 }
940 
941 /* *************************************************************************/
942 TEST(SmartProjectionPoseFactorRollingShutter,
943  hessianComparedToProjFactorsRollingShutter) {
944  using namespace vanillaPoseRS;
945  Point2Vector measurements_lmk1;
946 
947  // Project three landmarks into three cameras
948  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1);
949 
950  // create inputs
951  std::vector<std::pair<Key, Key>> key_pairs;
952  key_pairs.push_back(std::make_pair(x1, x2));
953  key_pairs.push_back(std::make_pair(x2, x3));
954  key_pairs.push_back(std::make_pair(x3, x1));
955 
956  std::vector<double> interp_factors;
957  interp_factors.push_back(interp_factor1);
958  interp_factors.push_back(interp_factor2);
959  interp_factors.push_back(interp_factor3);
960 
961  std::shared_ptr<Cameras> cameraRig(new Cameras());
962  cameraRig->push_back(Camera(Pose3::Identity(), sharedK));
963 
964  SmartFactorRS::shared_ptr smartFactor1(
965  new SmartFactorRS(model, cameraRig, params));
966  smartFactor1->add(measurements_lmk1, key_pairs, interp_factors);
967 
968  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
969  Point3(0.1, 0.1, 0.1)); // smaller noise
970  Values values;
971  values.insert(x1, level_pose);
972  values.insert(x2, pose_right);
973  // initialize third pose with some noise to get a nontrivial linearization
974  // point
975  values.insert(x3, pose_above * noise_pose);
976  EXPECT( // check that the pose is actually noisy
977  assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656,
978  -0.0313952598, -0.000986635786, 0.0314107591,
979  -0.999013364, -0.0313952598),
980  Point3(0.1, -0.1, 1.9)),
981  values.at<Pose3>(x3)));
982 
983  // linearization point for the poses
984  Pose3 pose1 = level_pose;
985  Pose3 pose2 = pose_right;
986  Pose3 pose3 = pose_above * noise_pose;
987 
988  // ==== check Hessian of smartFactor1 =====
989  // -- compute actual Hessian
990  std::shared_ptr<GaussianFactor> linearfactor1 =
991  smartFactor1->linearize(values);
992  Matrix actualHessian = linearfactor1->information();
993 
994  // -- compute expected Hessian from manual Schur complement from Jacobians
995  // linearization point for the 3D point
996  smartFactor1->triangulateSafe(smartFactor1->cameras(values));
997  TriangulationResult point = smartFactor1->point();
998  EXPECT(point.valid()); // check triangulated point is valid
999 
1000  // Use the factor to calculate the Jacobians
1001  Matrix F = Matrix::Zero(2 * 3, 6 * 3);
1002  Matrix E = Matrix::Zero(2 * 3, 3);
1003  Vector b = Vector::Zero(6);
1004 
1005  // create projection factors rolling shutter
1006  ProjectionFactorRollingShutter factor11(measurements_lmk1[0], interp_factor1,
1007  model, x1, x2, l0, sharedK);
1008  Matrix H1Actual, H2Actual, H3Actual;
1009  // note: b is minus the reprojection error, cf the smart factor jacobian
1010  // computation
1011  b.segment<2>(0) = -factor11.evaluateError(pose1, pose2, *point, H1Actual,
1012  H2Actual, H3Actual);
1013  F.block<2, 6>(0, 0) = H1Actual;
1014  F.block<2, 6>(0, 6) = H2Actual;
1015  E.block<2, 3>(0, 0) = H3Actual;
1016 
1017  ProjectionFactorRollingShutter factor12(measurements_lmk1[1], interp_factor2,
1018  model, x2, x3, l0, sharedK);
1019  b.segment<2>(2) = -factor12.evaluateError(pose2, pose3, *point, H1Actual,
1020  H2Actual, H3Actual);
1021  F.block<2, 6>(2, 6) = H1Actual;
1022  F.block<2, 6>(2, 12) = H2Actual;
1023  E.block<2, 3>(2, 0) = H3Actual;
1024 
1025  ProjectionFactorRollingShutter factor13(measurements_lmk1[2], interp_factor3,
1026  model, x3, x1, l0, sharedK);
1027  b.segment<2>(4) = -factor13.evaluateError(pose3, pose1, *point, H1Actual,
1028  H2Actual, H3Actual);
1029  F.block<2, 6>(4, 12) = H1Actual;
1030  F.block<2, 6>(4, 0) = H2Actual;
1031  E.block<2, 3>(4, 0) = H3Actual;
1032 
1033  // whiten
1034  F = (1 / sigma) * F;
1035  E = (1 / sigma) * E;
1036  b = (1 / sigma) * b;
1037  //* G = F' * F - F' * E * P * E' * F
1038  Matrix P = (E.transpose() * E).inverse();
1039  Matrix expectedHessian =
1040  F.transpose() * F - (F.transpose() * E * P * E.transpose() * F);
1041  EXPECT(assert_equal(expectedHessian, actualHessian, 1e-6));
1042 
1043  // ==== check Information vector of smartFactor1 =====
1044  GaussianFactorGraph gfg;
1045  gfg.add(linearfactor1);
1046  Matrix actualHessian_v2 = gfg.hessian().first;
1047  EXPECT(assert_equal(actualHessian_v2, actualHessian,
1048  1e-6)); // sanity check on hessian
1049 
1050  // -- compute actual information vector
1051  Vector actualInfoVector = gfg.hessian().second;
1052 
1053  // -- compute expected information vector from manual Schur complement from
1054  // Jacobians
1055  //* g = F' * (b - E * P * E' * b)
1056  Vector expectedInfoVector = F.transpose() * (b - E * P * E.transpose() * b);
1057  EXPECT(assert_equal(expectedInfoVector, actualInfoVector, 1e-6));
1058 
1059  // ==== check error of smartFactor1 (again) =====
1060  NonlinearFactorGraph nfg_projFactorsRS;
1061  nfg_projFactorsRS.add(factor11);
1062  nfg_projFactorsRS.add(factor12);
1063  nfg_projFactorsRS.add(factor13);
1064  values.insert(l0, *point);
1065 
1066  double actualError = smartFactor1->error(values);
1067  double expectedError = nfg_projFactorsRS.error(values);
1068  EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7);
1069 }
1070 
1071 /* *************************************************************************/
1072 TEST(SmartProjectionPoseFactorRollingShutter,
1073  hessianComparedToProjFactorsRollingShutter_measurementsFromSamePose) {
1074  // in this test we make sure the fact works even if we have multiple pixel
1075  // measurements of the same landmark at a single pose, a setup that occurs in
1076  // multi-camera systems
1077 
1078  using namespace vanillaPoseRS;
1079  Point2Vector measurements_lmk1;
1080 
1081  // Project three landmarks into three cameras
1082  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1);
1083 
1084  // create redundant measurements:
1085  Camera::MeasurementVector measurements_lmk1_redundant = measurements_lmk1;
1086  measurements_lmk1_redundant.push_back(
1087  measurements_lmk1.at(0)); // we readd the first measurement
1088 
1089  // create inputs
1090  std::vector<std::pair<Key, Key>> key_pairs;
1091  key_pairs.push_back(std::make_pair(x1, x2));
1092  key_pairs.push_back(std::make_pair(x2, x3));
1093  key_pairs.push_back(std::make_pair(x3, x1));
1094  key_pairs.push_back(std::make_pair(x1, x2));
1095 
1096  std::vector<double> interp_factors;
1097  interp_factors.push_back(interp_factor1);
1098  interp_factors.push_back(interp_factor2);
1099  interp_factors.push_back(interp_factor3);
1100  interp_factors.push_back(interp_factor1);
1101 
1102  std::shared_ptr<Cameras> cameraRig(new Cameras());
1103  cameraRig->push_back(Camera(Pose3::Identity(), sharedK));
1104 
1105  SmartFactorRS::shared_ptr smartFactor1(
1106  new SmartFactorRS(model, cameraRig, params));
1107  smartFactor1->add(measurements_lmk1_redundant, key_pairs, interp_factors);
1108 
1109  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
1110  Point3(0.1, 0.1, 0.1)); // smaller noise
1111  Values values;
1112  values.insert(x1, level_pose);
1113  values.insert(x2, pose_right);
1114  // initialize third pose with some noise to get a nontrivial linearization
1115  // point
1116  values.insert(x3, pose_above * noise_pose);
1117  EXPECT( // check that the pose is actually noisy
1118  assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656,
1119  -0.0313952598, -0.000986635786, 0.0314107591,
1120  -0.999013364, -0.0313952598),
1121  Point3(0.1, -0.1, 1.9)),
1122  values.at<Pose3>(x3)));
1123 
1124  // linearization point for the poses
1125  Pose3 pose1 = level_pose;
1126  Pose3 pose2 = pose_right;
1127  Pose3 pose3 = pose_above * noise_pose;
1128 
1129  // ==== check Hessian of smartFactor1 =====
1130  // -- compute actual Hessian
1131  std::shared_ptr<GaussianFactor> linearfactor1 =
1132  smartFactor1->linearize(values);
1133  Matrix actualHessian = linearfactor1->information();
1134 
1135  // -- compute expected Hessian from manual Schur complement from Jacobians
1136  // linearization point for the 3D point
1137  smartFactor1->triangulateSafe(smartFactor1->cameras(values));
1138  TriangulationResult point = smartFactor1->point();
1139  EXPECT(point.valid()); // check triangulated point is valid
1140 
1141  // Use standard ProjectionFactorRollingShutter factor to calculate the
1142  // Jacobians
1143  Matrix F = Matrix::Zero(2 * 4, 6 * 3);
1144  Matrix E = Matrix::Zero(2 * 4, 3);
1145  Vector b = Vector::Zero(8);
1146 
1147  // create projection factors rolling shutter
1148  ProjectionFactorRollingShutter factor11(measurements_lmk1_redundant[0],
1149  interp_factor1, model, x1, x2, l0,
1150  sharedK);
1151  Matrix H1Actual, H2Actual, H3Actual;
1152  // note: b is minus the reprojection error, cf the smart factor jacobian
1153  // computation
1154  b.segment<2>(0) = -factor11.evaluateError(pose1, pose2, *point, H1Actual,
1155  H2Actual, H3Actual);
1156  F.block<2, 6>(0, 0) = H1Actual;
1157  F.block<2, 6>(0, 6) = H2Actual;
1158  E.block<2, 3>(0, 0) = H3Actual;
1159 
1160  ProjectionFactorRollingShutter factor12(measurements_lmk1_redundant[1],
1161  interp_factor2, model, x2, x3, l0,
1162  sharedK);
1163  b.segment<2>(2) = -factor12.evaluateError(pose2, pose3, *point, H1Actual,
1164  H2Actual, H3Actual);
1165  F.block<2, 6>(2, 6) = H1Actual;
1166  F.block<2, 6>(2, 12) = H2Actual;
1167  E.block<2, 3>(2, 0) = H3Actual;
1168 
1169  ProjectionFactorRollingShutter factor13(measurements_lmk1_redundant[2],
1170  interp_factor3, model, x3, x1, l0,
1171  sharedK);
1172  b.segment<2>(4) = -factor13.evaluateError(pose3, pose1, *point, H1Actual,
1173  H2Actual, H3Actual);
1174  F.block<2, 6>(4, 12) = H1Actual;
1175  F.block<2, 6>(4, 0) = H2Actual;
1176  E.block<2, 3>(4, 0) = H3Actual;
1177 
1178  ProjectionFactorRollingShutter factor14(measurements_lmk1_redundant[3],
1179  interp_factor1, model, x1, x2, l0,
1180  sharedK);
1181  b.segment<2>(6) = -factor11.evaluateError(pose1, pose2, *point, H1Actual,
1182  H2Actual, H3Actual);
1183  F.block<2, 6>(6, 0) = H1Actual;
1184  F.block<2, 6>(6, 6) = H2Actual;
1185  E.block<2, 3>(6, 0) = H3Actual;
1186 
1187  // whiten
1188  F = (1 / sigma) * F;
1189  E = (1 / sigma) * E;
1190  b = (1 / sigma) * b;
1191  //* G = F' * F - F' * E * P * E' * F
1192  Matrix P = (E.transpose() * E).inverse();
1193  Matrix expectedHessian =
1194  F.transpose() * F - (F.transpose() * E * P * E.transpose() * F);
1195  EXPECT(assert_equal(expectedHessian, actualHessian, 1e-6));
1196 
1197  // ==== check Information vector of smartFactor1 =====
1198  GaussianFactorGraph gfg;
1199  gfg.add(linearfactor1);
1200  Matrix actualHessian_v2 = gfg.hessian().first;
1201  EXPECT(assert_equal(actualHessian_v2, actualHessian,
1202  1e-6)); // sanity check on hessian
1203 
1204  // -- compute actual information vector
1205  Vector actualInfoVector = gfg.hessian().second;
1206 
1207  // -- compute expected information vector from manual Schur complement from
1208  // Jacobians
1209  //* g = F' * (b - E * P * E' * b)
1210  Vector expectedInfoVector = F.transpose() * (b - E * P * E.transpose() * b);
1211  EXPECT(assert_equal(expectedInfoVector, actualInfoVector, 1e-6));
1212 
1213  // ==== check error of smartFactor1 (again) =====
1214  NonlinearFactorGraph nfg_projFactorsRS;
1215  nfg_projFactorsRS.add(factor11);
1216  nfg_projFactorsRS.add(factor12);
1217  nfg_projFactorsRS.add(factor13);
1218  nfg_projFactorsRS.add(factor14);
1219  values.insert(l0, *point);
1220 
1221  double actualError = smartFactor1->error(values);
1222  double expectedError = nfg_projFactorsRS.error(values);
1223  EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7);
1224 }
1225 
1226 /* *************************************************************************/
1227 TEST(SmartProjectionPoseFactorRollingShutter,
1228  optimization_3poses_measurementsFromSamePose) {
1229  using namespace vanillaPoseRS;
1230  Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3;
1231 
1232  // Project three landmarks into three cameras
1233  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1);
1234  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2);
1235  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3);
1236 
1237  // create inputs
1238  std::vector<std::pair<Key, Key>> key_pairs;
1239  key_pairs.push_back(std::make_pair(x1, x2));
1240  key_pairs.push_back(std::make_pair(x2, x3));
1241  key_pairs.push_back(std::make_pair(x3, x1));
1242 
1243  std::vector<double> interp_factors;
1244  interp_factors.push_back(interp_factor1);
1245  interp_factors.push_back(interp_factor2);
1246  interp_factors.push_back(interp_factor3);
1247 
1248  // For first factor, we create redundant measurement (taken by the same keys
1249  // as factor 1, to make sure the redundancy in the keys does not create
1250  // problems)
1251  Camera::MeasurementVector& measurements_lmk1_redundant = measurements_lmk1;
1252  measurements_lmk1_redundant.push_back(
1253  measurements_lmk1.at(0)); // we readd the first measurement
1254  std::vector<std::pair<Key, Key>> key_pairs_redundant = key_pairs;
1255  key_pairs_redundant.push_back(
1256  key_pairs.at(0)); // we readd the first pair of keys
1257  std::vector<double> interp_factors_redundant = interp_factors;
1258  interp_factors_redundant.push_back(
1259  interp_factors.at(0)); // we readd the first interp factor
1260 
1261  std::shared_ptr<Cameras> cameraRig(new Cameras());
1262  cameraRig->push_back(Camera(Pose3::Identity(), sharedK));
1263 
1264  SmartFactorRS::shared_ptr smartFactor1(
1265  new SmartFactorRS(model, cameraRig, params));
1266  smartFactor1->add(measurements_lmk1_redundant, key_pairs_redundant,
1267  interp_factors_redundant);
1268 
1269  SmartFactorRS::shared_ptr smartFactor2(
1270  new SmartFactorRS(model, cameraRig, params));
1271  smartFactor2->add(measurements_lmk2, key_pairs, interp_factors);
1272 
1273  SmartFactorRS::shared_ptr smartFactor3(
1274  new SmartFactorRS(model, cameraRig, params));
1275  smartFactor3->add(measurements_lmk3, key_pairs, interp_factors);
1276 
1277  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
1278 
1279  NonlinearFactorGraph graph;
1280  graph.push_back(smartFactor1);
1281  graph.push_back(smartFactor2);
1282  graph.push_back(smartFactor3);
1283  graph.addPrior(x1, level_pose, noisePrior);
1284  graph.addPrior(x2, pose_right, noisePrior);
1285 
1286  Values groundTruth;
1287  groundTruth.insert(x1, level_pose);
1288  groundTruth.insert(x2, pose_right);
1289  groundTruth.insert(x3, pose_above);
1290  DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9);
1291 
1292  // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10),
1293  // Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
1294  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
1295  Point3(0.1, 0.1, 0.1)); // smaller noise
1296  Values values;
1297  values.insert(x1, level_pose);
1298  values.insert(x2, pose_right);
1299  // initialize third pose with some noise, we expect it to move back to
1300  // original pose_above
1301  values.insert(x3, pose_above * noise_pose);
1302  EXPECT( // check that the pose is actually noisy
1303  assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656,
1304  -0.0313952598, -0.000986635786, 0.0314107591,
1305  -0.999013364, -0.0313952598),
1306  Point3(0.1, -0.1, 1.9)),
1307  values.at<Pose3>(x3)));
1308 
1309  Values result;
1310  LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
1311  result = optimizer.optimize();
1312  EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-5));
1313 }
1314 
1315 #ifndef DISABLE_TIMING
1316 #include <gtsam/base/timing.h>
1317 //-Total: 0 CPU (0 times, 0 wall, 0.21 children, min: 0 max: 0)
1318 //| -SF RS LINEARIZE: 0.14 CPU
1319 //(10000 times, 0.131202 wall, 0.14 children, min: 0 max: 0)
1320 //| -RS LINEARIZE: 0.06 CPU
1321 //(10000 times, 0.066951 wall, 0.06 children, min: 0 max: 0)
1322 /* *************************************************************************/
1323 TEST(SmartProjectionPoseFactorRollingShutter, timing) {
1324  using namespace vanillaPose;
1325 
1326  // Default cameras for simple derivatives
1327  static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0));
1328  SmartProjectionParams params(
1329  gtsam::HESSIAN,
1330  gtsam::ZERO_ON_DEGENERACY); // only config that works with RS factors
1331 
1332  Rot3 R = Rot3::Identity();
1333  Pose3 pose1 = Pose3(R, Point3(0, 0, 0));
1334  Pose3 pose2 = Pose3(R, Point3(1, 0, 0));
1335  Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple);
1336  Pose3 body_P_sensorId = Pose3::Identity();
1337 
1338  // one landmarks 1m in front of camera
1339  Point3 landmark1(0, 0, 10);
1340 
1341  Point2Vector measurements_lmk1;
1342 
1343  // Project 2 landmarks into 2 cameras
1344  measurements_lmk1.push_back(cam1.project(landmark1));
1345  measurements_lmk1.push_back(cam2.project(landmark1));
1346 
1347  size_t nrTests = 10000;
1348 
1349  for (size_t i = 0; i < nrTests; i++) {
1350  std::shared_ptr<Cameras> cameraRig(new Cameras());
1351  cameraRig->push_back(Camera(body_P_sensorId, sharedKSimple));
1352 
1353  SmartFactorRS::shared_ptr smartFactorRS(new SmartFactorRS(
1354  model, cameraRig, params));
1355  double interp_factor = 0; // equivalent to measurement taken at pose 1
1356  smartFactorRS->add(measurements_lmk1[0], x1, x2, interp_factor);
1357  interp_factor = 1; // equivalent to measurement taken at pose 2
1358  smartFactorRS->add(measurements_lmk1[1], x1, x2, interp_factor);
1359 
1360  Values values;
1361  values.insert(x1, pose1);
1362  values.insert(x2, pose2);
1363  gttic_(SF_RS_LINEARIZE);
1364  smartFactorRS->linearize(values);
1365  gttoc_(SF_RS_LINEARIZE);
1366  }
1367 
1368  for (size_t i = 0; i < nrTests; i++) {
1369  SmartFactor::shared_ptr smartFactor(
1370  new SmartFactor(model, sharedKSimple, params));
1371  smartFactor->add(measurements_lmk1[0], x1);
1372  smartFactor->add(measurements_lmk1[1], x2);
1373 
1374  Values values;
1375  values.insert(x1, pose1);
1376  values.insert(x2, pose2);
1377  gttic_(RS_LINEARIZE);
1378  smartFactor->linearize(values);
1379  gttoc_(RS_LINEARIZE);
1380  }
1381  tictoc_print_();
1382 }
1383 #endif
1384 
1385 #include <gtsam/geometry/SphericalCamera.h>
1386 /* ************************************************************************* */
1387 // spherical Camera with rolling shutter effect
1388 namespace sphericalCameraRS {
1389 typedef SphericalCamera Camera;
1390 typedef CameraSet<Camera> Cameras;
1391 typedef SmartProjectionPoseFactorRollingShutter<Camera> SmartFactorRS_spherical;
1392 Pose3 interp_pose1 = interpolate<Pose3>(level_pose, pose_right, interp_factor1);
1393 Pose3 interp_pose2 = interpolate<Pose3>(pose_right, pose_above, interp_factor2);
1394 Pose3 interp_pose3 = interpolate<Pose3>(pose_above, level_pose, interp_factor3);
1395 static EmptyCal::shared_ptr emptyK(new EmptyCal());
1396 Camera cam1(interp_pose1, emptyK);
1397 Camera cam2(interp_pose2, emptyK);
1398 Camera cam3(interp_pose3, emptyK);
1399 } // namespace sphericalCameraRS
1400 
1401 /* *************************************************************************/
1402 TEST(SmartProjectionPoseFactorRollingShutter,
1403  optimization_3poses_sphericalCameras) {
1404  using namespace sphericalCameraRS;
1405  std::vector<Unit3> measurements_lmk1, measurements_lmk2, measurements_lmk3;
1406 
1407  // Project three landmarks into three cameras
1408  projectToMultipleCameras<Camera>(cam1, cam2, cam3, landmark1,
1409  measurements_lmk1);
1410  projectToMultipleCameras<Camera>(cam1, cam2, cam3, landmark2,
1411  measurements_lmk2);
1412  projectToMultipleCameras<Camera>(cam1, cam2, cam3, landmark3,
1413  measurements_lmk3);
1414 
1415  // create inputs
1416  std::vector<std::pair<Key, Key>> key_pairs;
1417  key_pairs.push_back(std::make_pair(x1, x2));
1418  key_pairs.push_back(std::make_pair(x2, x3));
1419  key_pairs.push_back(std::make_pair(x3, x1));
1420 
1421  std::vector<double> interp_factors;
1422  interp_factors.push_back(interp_factor1);
1423  interp_factors.push_back(interp_factor2);
1424  interp_factors.push_back(interp_factor3);
1425 
1426  SmartProjectionParams params(
1427  gtsam::HESSIAN,
1428  gtsam::ZERO_ON_DEGENERACY); // only config that works with RS factors
1429  params.setRankTolerance(0.1);
1430 
1431  std::shared_ptr<Cameras> cameraRig(new Cameras());
1432  cameraRig->push_back(Camera(Pose3::Identity(), emptyK));
1433 
1434  SmartFactorRS_spherical::shared_ptr smartFactor1(
1435  new SmartFactorRS_spherical(model, cameraRig, params));
1436  smartFactor1->add(measurements_lmk1, key_pairs, interp_factors);
1437 
1438  SmartFactorRS_spherical::shared_ptr smartFactor2(
1439  new SmartFactorRS_spherical(model, cameraRig, params));
1440  smartFactor2->add(measurements_lmk2, key_pairs, interp_factors);
1441 
1442  SmartFactorRS_spherical::shared_ptr smartFactor3(
1443  new SmartFactorRS_spherical(model, cameraRig, params));
1444  smartFactor3->add(measurements_lmk3, key_pairs, interp_factors);
1445 
1446  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
1447 
1448  NonlinearFactorGraph graph;
1449  graph.push_back(smartFactor1);
1450  graph.push_back(smartFactor2);
1451  graph.push_back(smartFactor3);
1452  graph.addPrior(x1, level_pose, noisePrior);
1453  graph.addPrior(x2, pose_right, noisePrior);
1454 
1455  Values groundTruth;
1456  groundTruth.insert(x1, level_pose);
1457  groundTruth.insert(x2, pose_right);
1458  groundTruth.insert(x3, pose_above);
1459  DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9);
1460 
1461  // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10),
1462  // Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
1463  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
1464  Point3(0.1, 0.1, 0.1)); // smaller noise
1465  Values values;
1466  values.insert(x1, level_pose);
1467  values.insert(x2, pose_right);
1468  // initialize third pose with some noise, we expect it to move back to
1469  // original pose_above
1470  values.insert(x3, pose_above * noise_pose);
1471  EXPECT( // check that the pose is actually noisy
1472  assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656,
1473  -0.0313952598, -0.000986635786, 0.0314107591,
1474  -0.999013364, -0.0313952598),
1475  Point3(0.1, -0.1, 1.9)),
1476  values.at<Pose3>(x3)));
1477 
1478  Values result;
1479  LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
1480  result = optimizer.optimize();
1481  EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-6));
1482 }
1483 
1484 /* ************************************************************************* */
1485 int main() {
1486  TestResult tr;
1487  return TestRegistry::runAllTests(tr);
1488 }
1489 /* ************************************************************************* */
ZDim
static const int ZDim
Measurement dimension (Point2)
Definition: testSmartProjectionPoseFactorRollingShutter.cpp:218
sampling::A1
static Matrix A1
Definition: testGaussianBayesNet.cpp:167
simple_graph::factor1
auto factor1
Definition: testJacobianFactor.cpp:187
x3
static Symbol x3('X', 3)
gtsam::Rot3::Ypr
static Rot3 Ypr(double y, double p, double r, OptionalJacobian< 3, 1 > Hy={}, OptionalJacobian< 3, 1 > Hp={}, OptionalJacobian< 3, 1 > Hr={})
Definition: Rot3.h:196
simple_graph::factor2
auto factor2
Definition: testJacobianFactor.cpp:194
cam1
static PinholeCamera< Cal3_S2 > cam1(pose3, cal1)
gtsam::symbol_shorthand::F
Key F(std::uint64_t j)
Definition: inference/Symbol.h:153
gtsam::NonlinearOptimizer::optimize
virtual const Values & optimize()
Definition: NonlinearOptimizer.h:98
block
m m block(1, 0, 2, 2)<< 4
gttic_
#define gttic_(label)
Definition: timing.h:245
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam_unstable.tests.test_ProjectionFactorRollingShutter.point
point
Definition: test_ProjectionFactorRollingShutter.py:25
l0
static Symbol l0('L', 0)
gtsam::Values::at
const ValueType at(Key j) const
Definition: Values-inl.h:204
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
cam2
static StereoCamera cam2(pose3, cal4ptr)
DOUBLES_EQUAL
#define DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:141
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
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
gtsam::FastVector
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
Definition: FastVector.h:34
gtsam::Camera
PinholeCamera< Cal3Bundler0 > Camera
Definition: testAdaptAutoDiff.cpp:52
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
M_PI
#define M_PI
Definition: main.h:106
R
Rot2 R(Rot2::fromAngle(0.1))
leaf::values
leaf::MyValues values
gtsam::Rot3::Identity
static Rot3 Identity()
identity rotation for group operation
Definition: Rot3.h:300
measurement3
static Point2 measurement3(320.0, 10.0)
gtsam::Pose3::Identity
static Pose3 Identity()
identity for group operation
Definition: Pose3.h:106
sphericalCameraRS::cam3
Camera cam3(interp_pose3, emptyK)
gtsam::SharedIsotropic
noiseModel::Isotropic::shared_ptr SharedIsotropic
Definition: NoiseModel.h:745
SphericalCamera.h
Calibrated camera with spherical projection.
numericalDerivative.h
Some functions to compute numerical derivatives.
gtsam::VectorValues::insert
iterator insert(const std::pair< Key, Vector > &key_value)
Definition: VectorValues.cpp:81
gtsam::symbol_shorthand::X
Key X(std::uint64_t j)
Definition: inference/Symbol.h:171
gtsam::SmartProjectionPoseFactorRollingShutter::computeJacobiansWithTriangulatedPoint
void computeJacobiansWithTriangulatedPoint(FBlocks &Fs, Matrix &E, Vector &b, const Values &values) const
Definition: SmartProjectionPoseFactorRollingShutter.h:307
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
inverse
EIGEN_DEVICE_FUNC const InverseReturnType inverse() const
Definition: ArrayCwiseUnaryOps.h:411
ProjectionFactorRollingShutter.h
Basic projection factor for rolling shutter cameras.
gtsam::FactorGraph::add
IsDerived< DERIVEDFACTOR > add(std::shared_ptr< DERIVEDFACTOR > factor)
add is a synonym for push_back.
Definition: FactorGraph.h:214
camera3
static const CalibratedCamera camera3(pose1)
gtsam::NonlinearFactorGraph::addPrior
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
Definition: NonlinearFactorGraph.h:199
gtsam::SmartProjectionParams::setRankTolerance
void setRankTolerance(double rankTol)
Definition: SmartFactorParams.h:106
PoseTranslationPrior.h
Implements a prior on the translation component of a pose.
gtsam::GaussianFactorGraph::add
void add(const GaussianFactor &factor)
Definition: GaussianFactorGraph.h:125
landmark2
Point2 landmark2(7.0, 1.5)
gtsam::SmartProjectionPoseFactorRollingShutter::add
void add(const MEASUREMENT &measured, const Key &world_P_body_key1, const Key &world_P_body_key2, const double &alpha, const size_t &cameraId=0)
Definition: SmartProjectionPoseFactorRollingShutter.h:128
gtsam::Cal3_S2::shared_ptr
std::shared_ptr< Cal3_S2 > shared_ptr
Definition: Cal3_S2.h:39
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
result
Values result
Definition: OdometryOptimize.cpp:8
gtsam::SmartProjectionFactor::triangulateSafe
TriangulationResult triangulateSafe(const Cameras &cameras) const
Call gtsam::triangulateSafe iff we need to re-triangulate.
Definition: SmartProjectionFactor.h:173
SmartFactor
SmartProjectionPoseFactor< Cal3_S2 > SmartFactor
Definition: ISAM2Example_SmartFactor.cpp:22
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
pose3
static Pose3 pose3(rt3, pt3)
camera2
static const Camera2 camera2(pose1, K2)
MatrixZD
Eigen::Matrix< double, ZDim, DimBlock > MatrixZD
Definition: testSmartProjectionPoseFactorRollingShutter.cpp:220
sphericalCameraRS::SmartFactorRS_spherical
SmartProjectionPoseFactorRollingShutter< Camera > SmartFactorRS_spherical
Definition: testSmartProjectionPoseFactorRollingShutter.cpp:1391
level_pose
Pose3 level_pose
Definition: testInvDepthCamera3.cpp:21
vanillaPoseRS::params
SmartProjectionParams params(gtsam::HESSIAN, gtsam::ZERO_ON_DEGENERACY)
gtsam::HessianFactor::information
Matrix information() const override
Definition: HessianFactor.cpp:287
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
x4
static Symbol x4('X', 4)
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:218
LevenbergMarquardtOptimizer.h
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
gtsam::tictoc_print_
void tictoc_print_()
Definition: timing.h:268
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
sigma
static const double sigma
Definition: testSmartProjectionPoseFactorRollingShutter.cpp:38
simple_graph::factor3
auto factor3
Definition: testJacobianFactor.cpp:201
gtsam::Cal3_S2
The most common 5DOF 3D->2D calibration.
Definition: Cal3_S2.h:34
vanillaPoseRS::sharedK
static Cal3_S2::shared_ptr sharedK(new Cal3_S2(fov, w, h))
gtsam::StereoCamera::project
StereoPoint2 project(const Point3 &point) const
Project 3D point to StereoPoint2 (uL,uR,v)
Definition: StereoCamera.cpp:32
gtsam::NonlinearFactorGraph::error
double error(const Values &values) const
Definition: NonlinearFactorGraph.cpp:170
ProjectionFactor.h
Reprojection of a LANDMARK to a 2D point.
gtsam::b
const G & b
Definition: Group.h:86
w
RowVector3d w
Definition: Matrix_resize_int.cpp:3
pose1
static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5))
SmartFactorRS
SmartProjectionPoseFactorRollingShutter< PinholePose< Cal3_S2 > > SmartFactorRS
Definition: testSmartProjectionPoseFactorRollingShutter.cpp:83
gtsam::GaussianFactorGraph::hessian
std::pair< Matrix, Vector > hessian(const Ordering &ordering) const
Definition: GaussianFactorGraph.cpp:264
gtsam::SharedDiagonal
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:743
gtsam
traits
Definition: chartTesting.h:28
gtsam::SmartProjectionPoseFactorRollingShutter::cameras
Base::Cameras cameras(const Values &values) const override
Definition: SmartProjectionPoseFactorRollingShutter.h:266
FBlocks
std::vector< MatrixZD, Eigen::aligned_allocator< MatrixZD > > FBlocks
Definition: testSmartProjectionPoseFactorRollingShutter.cpp:222
h
const double h
Definition: testSimpleHelicopter.cpp:19
E
DiscreteKey E(5, 2)
gtsam::SmartProjectionPoseFactorRollingShutter::shared_ptr
std::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
Definition: SmartProjectionPoseFactorRollingShutter.h:88
gtsam::SmartProjectionFactor::point
TriangulationResult point() const
Definition: SmartProjectionFactor.h:442
gtsam::SmartProjectionParams::setDegeneracyMode
void setDegeneracyMode(DegeneracyMode degMode)
Definition: SmartFactorParams.h:100
rankTol
static const double rankTol
Definition: testSmartProjectionPoseFactorRollingShutter.cpp:36
gtsam::Point2Vector
std::vector< Point2, Eigen::aligned_allocator< Point2 > > Point2Vector
Definition: Point2.h:49
gtsam::symbol_shorthand::L
Key L(std::uint64_t j)
Definition: inference/Symbol.h:159
g1
Pose3 g1(Rot3(), Point3(100.0, 0.0, 300.0))
p
float * p
Definition: Tutorial_Map_using.cpp:9
vanilla::level_uv_right
static const Point2 level_uv_right
Definition: smartFactorScenarios.h:64
simple::pose2
static Pose3 pose2
Definition: testInitializePose3.cpp:58
add
graph add(PriorFactor< Pose2 >(1, priorMean, priorNoise))
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::ProjectionFactorRollingShutter::evaluateError
Vector evaluateError(const Pose3 &pose_a, const Pose3 &pose_b, const Point3 &point, OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3) const override
Evaluate error h(x)-z and optionally derivatives.
Definition: ProjectionFactorRollingShutter.cpp:22
gtsam::EmptyCal::shared_ptr
std::shared_ptr< EmptyCal > shared_ptr
Definition: SphericalCamera.h:47
sphericalCameraRS::emptyK
static EmptyCal::shared_ptr emptyK(new EmptyCal())
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
gtsam::TEST
TEST(SmartFactorBase, Pinhole)
Definition: testSmartFactorBase.cpp:38
Eigen::Matrix
The matrix class, also used for vectors and row-vectors.
Definition: 3rdparty/Eigen/Eigen/src/Core/Matrix.h:178
gtsam::PinholeBase::MeasurementVector
Point2Vector MeasurementVector
Definition: CalibratedCamera.h:67
x1
static Symbol x1('X', 1)
gttoc_
#define gttoc_(label)
Definition: timing.h:250
gtsam::SmartProjectionPoseFactor::shared_ptr
std::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
Definition: SmartProjectionPoseFactor.h:59
g2
Pose3 g2(g1.expmap(h *V1_g1))
SmartProjectionPoseFactorRollingShutter.h
Smart projection factor on poses modeling rolling shutter effect with given readout time...
camera1
PinholeCamera< Cal3_S2 > camera1(pose1, *sharedCal)
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
gtsam::SmartProjectionParams::setDynamicOutlierRejectionThreshold
void setDynamicOutlierRejectionThreshold(double dynOutRejectionThreshold)
Definition: SmartFactorParams.h:115
x2
static Symbol x2('X', 2)
vanilla::level_uv
static const Point2 level_uv
Definition: smartFactorScenarios.h:63
fov
static double fov
Definition: testProjectionFactor.cpp:33
lmParams
LevenbergMarquardtParams lmParams
Definition: testSmartProjectionPoseFactorRollingShutter.cpp:81
measurement1
static Point2 measurement1(323.0, 240.0)
timing.h
Timing utilities.
model
static SharedIsotropic model(noiseModel::Isotropic::Sigma(2, sigma))
gtsam::noiseModel::Isotropic::Sigma
static shared_ptr Sigma(size_t dim, double sigma, bool smart=true)
Definition: NoiseModel.cpp:594

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