passing tough test - nice!

gtsam/slam/tests/testSmartProjectionFactorP.cpp

@@ -120,7 +120,8 @@ TEST( SmartProjectionFactorP, noiseless ) {
 
   // Calculate expected derivative for point (easiest to check)
   std::function<Vector(Point3)> f =  //
-      std::bind(&SmartFactorP::whitenedError<Point3>, factor, cameras, std::placeholders::_1);
+      std::bind(&SmartFactorP::whitenedError<Point3>, factor, cameras,
+                std::placeholders::_1);
 
   // Calculate using computeEP
   Matrix actualE;
@@ -194,7 +195,8 @@ TEST(SmartProjectionFactorP, smartFactorWithSensorBodyTransform) {
   using namespace vanillaPose;
 
   // create arbitrary body_T_sensor (transforms from sensor to body)
-  Pose3 body_T_sensor = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(1, 1, 1));
+  Pose3 body_T_sensor = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2),
+                              Point3(1, 1, 1));
 
   // These are the poses we want to estimate, from camera measurements
   const Pose3 sensor_T_body = body_T_sensor.inverse();
@@ -237,7 +239,8 @@ TEST(SmartProjectionFactorP, smartFactorWithSensorBodyTransform) {
   smartFactor2.add(measurements_cam2, views, sharedKs, body_T_sensors);
 
   SmartFactorP smartFactor3(model, params);
-  smartFactor3.add(measurements_cam3, views, sharedKs, body_T_sensors);;
+  smartFactor3.add(measurements_cam3, views, sharedKs, body_T_sensors);
+  ;
 
   const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
 
@@ -333,7 +336,8 @@ TEST( SmartProjectionFactorP, 3poses_smart_projection_factor ) {
           Pose3(
               Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598,
                    -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598),
-              Point3(0.1, -0.1, 1.9)), values.at<Pose3>(x3)));
+              Point3(0.1, -0.1, 1.9)),
+          values.at<Pose3>(x3)));
 
   Values result;
   LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
@@ -368,7 +372,8 @@ TEST( SmartProjectionFactorP, Factors ) {
   sharedKs.push_back(sharedK);
   sharedKs.push_back(sharedK);
 
-  SmartFactorP::shared_ptr smartFactor1 = boost::make_shared<SmartFactorP>(model);
+  SmartFactorP::shared_ptr smartFactor1 = boost::make_shared < SmartFactorP
+      > (model);
   smartFactor1->add(measurements_cam1, views, sharedKs);
 
   SmartFactorP::Cameras cameras;
@@ -422,8 +427,8 @@ TEST( SmartProjectionFactorP, Factors ) {
     values.insert(x1, cam1.pose());
     values.insert(x2, cam2.pose());
 
-    boost::shared_ptr<RegularHessianFactor<6> > actual =
+    boost::shared_ptr < RegularHessianFactor<6> > actual = smartFactor1
-        smartFactor1->createHessianFactor(values, 0.0);
+        ->createHessianFactor(values, 0.0);
     EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6));
     EXPECT(assert_equal(expected, *actual, 1e-6));
     EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6);
@@ -481,7 +486,8 @@ TEST( SmartProjectionFactorP, 3poses_iterative_smart_projection_factor ) {
           Pose3(
               Rot3(1.11022302e-16, -0.0314107591, 0.99950656, -0.99950656,
                    -0.0313952598, -0.000986635786, 0.0314107591, -0.999013364,
-                  -0.0313952598), Point3(0.1, -0.1, 1.9)),
+                   -0.0313952598),
+              Point3(0.1, -0.1, 1.9)),
           values.at<Pose3>(x3)));
 
   Values result;
@@ -518,16 +524,13 @@ TEST( SmartProjectionFactorP, landmarkDistance ) {
   params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist);
   params.setEnableEPI(false);
 
-  SmartFactorP::shared_ptr smartFactor1(
+  SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, params));
-      new SmartFactorP(model, params));
   smartFactor1->add(measurements_cam1, views, sharedKs);
 
-  SmartFactorP::shared_ptr smartFactor2(
+  SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, params));
-      new SmartFactorP(model, params));
   smartFactor2->add(measurements_cam2, views, sharedKs);
 
-  SmartFactorP::shared_ptr smartFactor3(
+  SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, params));
-      new SmartFactorP(model, params));
   smartFactor3->add(measurements_cam3, views, sharedKs);
 
   const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@@ -588,20 +591,16 @@ TEST( SmartProjectionFactorP, dynamicOutlierRejection ) {
   params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist);
   params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold);
 
-  SmartFactorP::shared_ptr smartFactor1(
+  SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, params));
-      new SmartFactorP(model, params));
   smartFactor1->add(measurements_cam1, views, sharedKs);
 
-  SmartFactorP::shared_ptr smartFactor2(
+  SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, params));
-      new SmartFactorP(model, params));
   smartFactor2->add(measurements_cam2, views, sharedKs);
 
-  SmartFactorP::shared_ptr smartFactor3(
+  SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, params));
-      new SmartFactorP(model, params));
   smartFactor3->add(measurements_cam3, views, sharedKs);
 
-  SmartFactorP::shared_ptr smartFactor4(
+  SmartFactorP::shared_ptr smartFactor4(new SmartFactorP(model, params));
-      new SmartFactorP(model, params));
   smartFactor4->add(measurements_cam4, views, sharedKs);
 
   const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@@ -656,16 +655,13 @@ TEST( SmartProjectionFactorP, CheckHessian) {
   params.setRankTolerance(10);
   params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY);
 
-  SmartFactorP::shared_ptr smartFactor1(
+  SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, params));  // HESSIAN, by default
-      new SmartFactorP(model, params)); // HESSIAN, by default
   smartFactor1->add(measurements_cam1, views, sharedKs);
 
-  SmartFactorP::shared_ptr smartFactor2(
+  SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, params));  // HESSIAN, by default
-      new SmartFactorP(model, params)); // HESSIAN, by default
   smartFactor2->add(measurements_cam2, views, sharedKs);
 
-  SmartFactorP::shared_ptr smartFactor3(
+  SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, params));  // HESSIAN, by default
-      new SmartFactorP(model, params)); // HESSIAN, by default
   smartFactor3->add(measurements_cam3, views, sharedKs);
 
   NonlinearFactorGraph graph;
@@ -811,7 +807,8 @@ TEST( SmartProjectionFactorP, Cal3Bundler ) {
           Pose3(
               Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598,
                    -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598),
-              Point3(0.1, -0.1, 1.9)), values.at<Pose3>(x3)));
+              Point3(0.1, -0.1, 1.9)),
+          values.at<Pose3>(x3)));
 
   Values result;
   LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
@@ -819,6 +816,279 @@ TEST( SmartProjectionFactorP, Cal3Bundler ) {
   EXPECT(assert_equal(cam3.pose(), result.at<Pose3>(x3), 1e-6));
 }
 
+#include <gtsam/slam/ProjectionFactor.h>
+typedef GenericProjectionFactor<Pose3, Point3> TestProjectionFactor;
+static Symbol l0('L', 0);
+/* *************************************************************************/
+TEST( SmartProjectionFactorP, hessianComparedToProjFactors_measurementsFromSamePose) {
+  // in this test we make sure the fact works even if we have multiple pixel measurements of the same landmark
+  // at a single pose, a setup that occurs in multi-camera systems
+
+  using namespace vanillaPose;
+  Point2Vector measurements_lmk1;
+
+  // Project three landmarks into three cameras
+  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1);
+
+  // create redundant measurements:
+  Camera::MeasurementVector measurements_lmk1_redundant = measurements_lmk1;
+  measurements_lmk1_redundant.push_back(measurements_lmk1.at(0));  // we readd the first measurement
+
+  // create inputs
+  std::vector<Key> keys;
+  keys.push_back(x1);
+  keys.push_back(x2);
+  keys.push_back(x3);
+  keys.push_back(x1);
+
+  std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs;
+  sharedKs.push_back(sharedK);
+  sharedKs.push_back(sharedK);
+  sharedKs.push_back(sharedK);
+  sharedKs.push_back(sharedK);
+
+  SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model));
+  smartFactor1->add(measurements_lmk1_redundant, keys, sharedKs);
+
+  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
+                           Point3(0.1, 0.1, 0.1));  // smaller noise
+  Values values;
+  values.insert(x1, level_pose);
+  values.insert(x2, pose_right);
+  // initialize third pose with some noise to get a nontrivial linearization point
+  values.insert(x3, pose_above * noise_pose);
+  EXPECT(  // check that the pose is actually noisy
+      assert_equal( Pose3( Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), Point3(0.1, -0.1, 1.9)), values.at<Pose3>(x3)));
+
+  // linearization point for the poses
+  Pose3 pose1 = level_pose;
+  Pose3 pose2 = pose_right;
+  Pose3 pose3 = pose_above * noise_pose;
+
+  // ==== check Hessian of smartFactor1 =====
+  // -- compute actual Hessian
+  boost::shared_ptr<GaussianFactor> linearfactor1 = smartFactor1->linearize(
+      values);
+  Matrix actualHessian = linearfactor1->information();
+
+  // -- compute expected Hessian from manual Schur complement from Jacobians
+  // linearization point for the 3D point
+  smartFactor1->triangulateSafe(smartFactor1->cameras(values));
+  TriangulationResult point = smartFactor1->point();
+  EXPECT(point.valid());  // check triangulated point is valid
+
+  // Use standard ProjectionFactor factor to calculate the Jacobians
+  Matrix F = Matrix::Zero(2 * 4, 6 * 3);
+  Matrix E = Matrix::Zero(2 * 4, 3);
+  Vector b = Vector::Zero(2 * 4);
+
+  // create projection factors rolling shutter
+  TestProjectionFactor factor11(measurements_lmk1_redundant[0], model, x1, l0,
+                                sharedK);
+  Matrix HPoseActual, HEActual;
+  // note: b is minus the reprojection error, cf the smart factor jacobian computation
+  b.segment<2>(0) = -factor11.evaluateError(pose1, *point, HPoseActual,
+                                            HEActual);
+  F.block<2, 6>(0, 0) = HPoseActual;
+  E.block<2, 3>(0, 0) = HEActual;
+
+  TestProjectionFactor factor12(measurements_lmk1_redundant[1], model, x2, l0,
+                                sharedK);
+  b.segment<2>(2) = -factor12.evaluateError(pose2, *point, HPoseActual,
+                                            HEActual);
+  F.block<2, 6>(2, 6) = HPoseActual;
+  E.block<2, 3>(2, 0) = HEActual;
+
+  TestProjectionFactor factor13(measurements_lmk1_redundant[2], model, x3, l0,
+                                sharedK);
+  b.segment<2>(4) = -factor13.evaluateError(pose3, *point, HPoseActual,
+                                            HEActual);
+  F.block<2, 6>(4, 12) = HPoseActual;
+  E.block<2, 3>(4, 0) = HEActual;
+
+  TestProjectionFactor factor14(measurements_lmk1_redundant[3], model, x1, l0,
+                                sharedK);
+  b.segment<2>(6) = -factor11.evaluateError(pose1, *point, HPoseActual,
+                                            HEActual);
+  F.block<2, 6>(6, 0) = HPoseActual;
+  E.block<2, 3>(6, 0) = HEActual;
+
+  // whiten
+  F = (1 / sigma) * F;
+  E = (1 / sigma) * E;
+  b = (1 / sigma) * b;
+  //* G = F' * F - F' * E * P * E' * F
+  Matrix P = (E.transpose() * E).inverse();
+  Matrix expectedHessian = F.transpose() * F
+      - (F.transpose() * E * P * E.transpose() * F);
+  EXPECT(assert_equal(expectedHessian, actualHessian, 1e-6));
+
+  // ==== check Information vector of smartFactor1 =====
+  GaussianFactorGraph gfg;
+  gfg.add(linearfactor1);
+  Matrix actualHessian_v2 = gfg.hessian().first;
+  EXPECT(assert_equal(actualHessian_v2, actualHessian, 1e-6));  // sanity check on hessian
+
+  // -- compute actual information vector
+  Vector actualInfoVector = gfg.hessian().second;
+
+  // -- compute expected information vector from manual Schur complement from Jacobians
+  //* g = F' * (b - E * P * E' * b)
+  Vector expectedInfoVector = F.transpose() * (b - E * P * E.transpose() * b);
+  EXPECT(assert_equal(expectedInfoVector, actualInfoVector, 1e-6));
+
+  // ==== check error of smartFactor1 (again) =====
+  NonlinearFactorGraph nfg_projFactors;
+  nfg_projFactors.add(factor11);
+  nfg_projFactors.add(factor12);
+  nfg_projFactors.add(factor13);
+  nfg_projFactors.add(factor14);
+  values.insert(l0, *point);
+
+  double actualError = smartFactor1->error(values);
+  double expectedError = nfg_projFactors.error(values);
+  EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7);
+}
+
+///* *************************************************************************/
+//TEST( SmartProjectionFactorP, optimization_3poses_measurementsFromSamePose ) {
+//
+//  using namespace vanillaPoseRS;
+//  Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3;
+//
+//  // Project three landmarks into three cameras
+//  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1);
+//  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2);
+//  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3);
+//
+//  // create inputs
+//  std::vector<std::pair<Key,Key>> key_pairs;
+//  key_pairs.push_back(std::make_pair(x1,x2));
+//  key_pairs.push_back(std::make_pair(x2,x3));
+//  key_pairs.push_back(std::make_pair(x3,x1));
+//
+//  std::vector<double> interp_factors;
+//  interp_factors.push_back(interp_factor1);
+//  interp_factors.push_back(interp_factor2);
+//  interp_factors.push_back(interp_factor3);
+//
+//  // For first factor, we create redundant measurement (taken by the same keys as factor 1, to
+//  // make sure the redundancy in the keys does not create problems)
+//  Camera::MeasurementVector& measurements_lmk1_redundant = measurements_lmk1;
+//  measurements_lmk1_redundant.push_back(measurements_lmk1.at(0)); // we readd the first measurement
+//  std::vector<std::pair<Key,Key>> key_pairs_redundant = key_pairs;
+//  key_pairs_redundant.push_back(key_pairs.at(0)); // we readd the first pair of keys
+//  std::vector<double> interp_factors_redundant = interp_factors;
+//  interp_factors_redundant.push_back(interp_factors.at(0));// we readd the first interp factor
+//
+//  SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model));
+//  smartFactor1->add(measurements_lmk1_redundant, key_pairs_redundant, interp_factors_redundant, sharedK);
+//
+//  SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model));
+//  smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, sharedK);
+//
+//  SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model));
+//  smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, sharedK);
+//
+//  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
+//
+//  NonlinearFactorGraph graph;
+//  graph.push_back(smartFactor1);
+//  graph.push_back(smartFactor2);
+//  graph.push_back(smartFactor3);
+//  graph.addPrior(x1, level_pose, noisePrior);
+//  graph.addPrior(x2, pose_right, noisePrior);
+//
+//  Values groundTruth;
+//  groundTruth.insert(x1, level_pose);
+//  groundTruth.insert(x2, pose_right);
+//  groundTruth.insert(x3, pose_above);
+//  DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9);
+//
+//  //  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
+//  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
+//                           Point3(0.1, 0.1, 0.1)); // smaller noise
+//  Values values;
+//  values.insert(x1, level_pose);
+//  values.insert(x2, pose_right);
+//  // initialize third pose with some noise, we expect it to move back to original pose_above
+//  values.insert(x3, pose_above * noise_pose);
+//  EXPECT( // check that the pose is actually noisy
+//      assert_equal(
+//          Pose3(
+//              Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598,
+//                   -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598),
+//                   Point3(0.1, -0.1, 1.9)), values.at<Pose3>(x3)));
+//
+//  Values result;
+//  LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
+//  result = optimizer.optimize();
+//  EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-5));
+//}
+
+//#ifndef DISABLE_TIMING
+//#include <gtsam/base/timing.h>
+//// -Total: 0 CPU (0 times, 0 wall, 0.04 children, min: 0 max: 0)
+////|   -SF RS LINEARIZE: 0.02 CPU (1000 times, 0.017244 wall, 0.02 children, min: 0 max: 0)
+////|   -RS LINEARIZE: 0.02 CPU (1000 times, 0.009035 wall, 0.02 children, min: 0 max: 0)
+///* *************************************************************************/
+//TEST( SmartProjectionPoseFactorRollingShutter, timing ) {
+//
+//  using namespace vanillaPose;
+//
+//  // Default cameras for simple derivatives
+//  static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0));
+//
+//  Rot3 R = Rot3::identity();
+//  Pose3 pose1 = Pose3(R, Point3(0, 0, 0));
+//  Pose3 pose2 = Pose3(R, Point3(1, 0, 0));
+//  Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple);
+//  Pose3 body_P_sensorId = Pose3::identity();
+//
+//  // one landmarks 1m in front of camera
+//  Point3 landmark1(0, 0, 10);
+//
+//  Point2Vector measurements_lmk1;
+//
+//  // Project 2 landmarks into 2 cameras
+//  measurements_lmk1.push_back(cam1.project(landmark1));
+//  measurements_lmk1.push_back(cam2.project(landmark1));
+//
+//  size_t nrTests = 1000;
+//
+//  for(size_t i = 0; i<nrTests; i++){
+//    SmartFactorRS::shared_ptr smartFactorRS(new SmartFactorRS(model));
+//    double interp_factor = 0;  // equivalent to measurement taken at pose 1
+//    smartFactorRS->add(measurements_lmk1[0], x1, x2, interp_factor, sharedKSimple,
+//                       body_P_sensorId);
+//    interp_factor = 1;  // equivalent to measurement taken at pose 2
+//    smartFactorRS->add(measurements_lmk1[1], x1, x2, interp_factor, sharedKSimple,
+//                       body_P_sensorId);
+//
+//    Values values;
+//    values.insert(x1, pose1);
+//    values.insert(x2, pose2);
+//    gttic_(SF_RS_LINEARIZE);
+//    smartFactorRS->linearize(values);
+//    gttoc_(SF_RS_LINEARIZE);
+//  }
+//
+//  for(size_t i = 0; i<nrTests; i++){
+//    SmartFactor::shared_ptr smartFactor(new SmartFactor(model, sharedKSimple));
+//    smartFactor->add(measurements_lmk1[0], x1);
+//    smartFactor->add(measurements_lmk1[1], x2);
+//
+//    Values values;
+//    values.insert(x1, pose1);
+//    values.insert(x2, pose2);
+//    gttic_(RS_LINEARIZE);
+//    smartFactor->linearize(values);
+//    gttoc_(RS_LINEARIZE);
+//  }
+//  tictoc_print_();
+//}
+//#endif
+
 /* ************************************************************************* */
 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained");
 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal");