SmartProjectionFactor: More cleanup, added more tests, added some timing

gtsam_unstable/slam/SmartProjectionFactor.h

@@ -146,7 +146,8 @@ namespace gtsam {
     /// linearize returns a Hessianfactor that is an approximation of error(p)
     virtual boost::shared_ptr<GaussianFactor> linearize(const Values& values,  const Ordering& ordering) const {
 
-      bool debug = true;
+      bool debug = false;
+      bool blockwise = true;
 
       // Collect all poses (Cameras)
       std::vector<Pose3> cameraPoses;
@@ -177,7 +178,6 @@ namespace gtsam {
         js += ordering[k];
       }
 
-      bool blockwise = false;
       // For debug only
       std::vector<Matrix> Gs1;
       std::vector<Vector> gs1;
@@ -212,8 +212,7 @@ namespace gtsam {
           for(size_t i2 = 0; i2 < keys_.size(); i2++) {
             // we only need the upper triangular entries
             Hxl[i1][i2] = Hx.at(i1).transpose() * Hl.at(i1) * C * Hl.at(i2).transpose();
-            if (i1==0 & i2==0){
+            if (i1==0 && i2==0){
-
               if (debug) {
                 std::cout << "Hoff"<< i1 << i2 << "=[" << Hx.at(i1).transpose() * Hl.at(i1) * C * Hl.at(i2).transpose() << "];" << std::endl;
                 std::cout << "Hxoff"<< "=[" << Hx.at(i1) << "];" << std::endl;
@@ -373,17 +372,12 @@ namespace gtsam {
         if(point)
         { // triangulation produced a good estimate of landmark position
 
-//          std::cout << "point " << *point << std::endl;
-
           for(size_t i = 0; i < measured_.size(); i++) {
             Pose3 pose = cameraPoses.at(i);
             PinholeCamera<CALIBRATION> camera(pose, *K_);
-//            std::cout << "pose.compose(*body_P_sensor_) " << pose << std::endl;
 
             Point2 reprojectionError(camera.project(*point) - measured_.at(i));
-//            std::cout << "reprojectionError " << reprojectionError << std::endl;
             overallError += noise_->distance( reprojectionError.vector() );
-//            std::cout << "noise_->distance( reprojectionError.vector() ) " << noise_->distance( reprojectionError.vector() ) << std::endl;
           }
           return sqrt(overallError);
         }else{ // triangulation failed: we deactivate the factor, then the error should not contribute to the overall error

gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp

@@ -10,7 +10,7 @@
  * -------------------------------------------------------------------------- */
 
 /**
- *  @file  testProjectionFactor.cpp
+ *  @file  TestSmartProjectionFactor.cpp
  *  @brief Unit tests for ProjectionFactor Class
  *  @author Frank Dellaert
  *  @date Nov 2009
@@ -56,63 +56,71 @@ static SharedNoiseModel model(noiseModel::Unit::Create(2));
 using symbol_shorthand::X;
 using symbol_shorthand::L;
 
-//typedef GenericProjectionFactor<Pose3, Point3> TestProjectionFactor;
+typedef SmartProjectionFactor<Pose3, Point3> TestSmartProjectionFactor;
 
-
+/* ************************************************************************* */
-/* ************************************************************************* *
+TEST( SmartProjectionFactor, Constructor) {
-TEST( MultiProjectionFactor, noiseless ){
+  Key poseKey(X(1));
-  cout << " ************************ MultiProjectionFactor: noiseless ****************************" << endl;
-  Values theta;
-  NonlinearFactorGraph graph;
-
-  Symbol x1('X',  1);
-  Symbol x2('X',  2);
-//  Symbol x3('X',  3);
-
-  const SharedDiagonal noiseProjection = noiseModel::Isotropic::Sigma(2, 1);
 
   std::vector<Key> views;
-  views += x1, x2; //, x3;
+  views += poseKey;
 
-  Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480));
+  std::vector<Point2> measurements;
-  // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
+  measurements.push_back(Point2(323.0, 240.0));
-  Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
-  SimpleCamera level_camera(level_pose, *K);
 
-  // create second camera 1 meter to the right of first camera
+  TestSmartProjectionFactor factor(measurements, model, views, K);
-  Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1,0,0));
-  SimpleCamera level_camera_right(level_pose_right, *K);
-
-  // landmark ~5 meters infront of camera
-  Point3 landmark(5, 0.5, 1.2);
-
-  // 1. Project two landmarks into two cameras and triangulate
-  Point2 level_uv = level_camera.project(landmark);
-  Point2 level_uv_right = level_camera_right.project(landmark);
-
-  Values value;
-  value.insert(x1, level_pose);
-  value.insert(x2, level_pose_right);
-
-//  poses += level_pose, level_pose_right;
-  vector<Point2> measurements;
-  measurements += level_uv, level_uv_right;
-
-  SmartProjectionFactor<Pose3, Point3, Cal3_S2> smartFactor(measurements, noiseProjection, views, K);
-
-  double actualError = smartFactor.error(value);
-  double expectedError = 0.0;
-
-  DOUBLES_EQUAL(expectedError, actualError, 1e-7);
 }
 
-/* ************************************************************************* *
+/* ************************************************************************* */
-TEST( MultiProjectionFactor, noisy ){
+TEST( SmartProjectionFactor, ConstructorWithTransform) {
+  Key poseKey(X(1));
+
+  std::vector<Key> views;
+  views += poseKey;
+
+  std::vector<Point2> measurements;
+  measurements.push_back(Point2(323.0, 240.0));
+  Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
+
+  TestSmartProjectionFactor factor(measurements, model, views, K, body_P_sensor);
+}
+
+/* ************************************************************************* */
+TEST( SmartProjectionFactor, Equals ) {
+  // Create two identical factors and make sure they're equal
+  std::vector<Point2> measurements;
+  measurements.push_back(Point2(323.0, 240.0));
+
+  std::vector<Key> views;
+  views += X(1);
+  TestSmartProjectionFactor factor1(measurements, model, views, K);
+  TestSmartProjectionFactor factor2(measurements, model, views, K);
+
+  CHECK(assert_equal(factor1, factor2));
+}
+
+/* ************************************************************************* */
+TEST( SmartProjectionFactor, EqualsWithTransform ) {
+  // Create two identical factors and make sure they're equal
+  std::vector<Point2> measurements;
+  measurements.push_back(Point2(323.0, 240.0));
+  Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
+
+  std::vector<Key> views;
+  views += X(1);
+  TestSmartProjectionFactor factor1(measurements, model, views, K, body_P_sensor);
+  TestSmartProjectionFactor factor2(measurements, model, views, K, body_P_sensor);
+
+  CHECK(assert_equal(factor1, factor2));
+}
+
+
+/* ************************************************************************* */
+TEST( SmartProjectionFactor, noisy ){
   cout << " ************************ MultiProjectionFactor: noisy ****************************" << endl;
 
   Symbol x1('X',  1);
   Symbol x2('X',  2);
-//  Symbol x3('X',  3);
 
   const SharedDiagonal noiseProjection = noiseModel::Isotropic::Sigma(2, 1);
 
@@ -120,6 +128,7 @@ TEST( MultiProjectionFactor, noisy ){
   views += x1, x2; //, x3;
 
   Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480));
+
   // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
   Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
   SimpleCamera level_camera(level_pose, *K);
@@ -141,7 +150,6 @@ TEST( MultiProjectionFactor, noisy ){
   Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3));
   values.insert(x2, level_pose_right.compose(noise_pose));
 
-//  poses += level_pose, level_pose_right;
   vector<Point2> measurements;
   measurements += level_uv, level_uv_right;
 
@@ -149,16 +157,16 @@ TEST( MultiProjectionFactor, noisy ){
     smartFactor(new SmartProjectionFactor<Pose3, Point3, Cal3_S2>(measurements, noiseProjection, views, K));
 
   double actualError = smartFactor->error(values);
-  double expectedError = sqrt(0.08);
+  std::cout << "Error: " << actualError << std::endl;
 
   // we do not expect to be able to predict the error, since the error on the pixel will change
   // the triangulation of the landmark which is internal to the factor.
-  //  DOUBLES_EQUAL(expectedError, actualError, 1e-7);
+  // DOUBLES_EQUAL(expectedError, actualError, 1e-7);
 }
 
 
 /* ************************************************************************* */
-TEST( MultiProjectionFactor, 3poses ){
+TEST( SmartProjectionFactor, 3poses ){
   cout << " ************************ MultiProjectionFactor: 3 cams + 3 landmarks **********************" << endl;
 
   Symbol x1('X',  1);
@@ -171,6 +179,7 @@ TEST( MultiProjectionFactor, 3poses ){
   views += x1, x2, x3;
 
   Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480));
+
   // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
   Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
   SimpleCamera cam1(pose1, *K);
@@ -214,9 +223,6 @@ TEST( MultiProjectionFactor, 3poses ){
   SmartFactor::shared_ptr smartFactor2(new SmartFactor(measurements_cam2, noiseProjection, views, K));
   SmartFactor::shared_ptr smartFactor3(new SmartFactor(measurements_cam3, noiseProjection, views, K));
 
-//  double actualError = smartFactor->error(values);
-//  double expectedError = sqrt(0.08);
-
   const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
 
   NonlinearFactorGraph graph;
@@ -226,9 +232,6 @@ TEST( MultiProjectionFactor, 3poses ){
   graph.add(PriorFactor<Pose3>(x1, pose1, noisePrior));
   graph.add(PriorFactor<Pose3>(x2, pose2, noisePrior));
 
-//  smartFactor1->print("smartFactor1");
-
-
   Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1));
   Values values;
   values.insert(x1, pose1);
@@ -238,16 +241,23 @@ TEST( MultiProjectionFactor, 3poses ){
   LevenbergMarquardtParams params;
   params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
   params.verbosity = NonlinearOptimizerParams::ERROR;
+
+
+  Values result;
+  gttic_(SmartProjectionFactor);
   LevenbergMarquardtOptimizer optimizer(graph, values, params);
-  Values result = optimizer.optimize();
+  result = optimizer.optimize();
+  gttoc_(SmartProjectionFactor);
+  tictoc_finishedIteration_();
 
   result.print("results of 3 camera, 3 landmark optimization \n");
+  tictoc_print_();
 
 }
 
 
-/* *************************************************************************
+/* ************************************************************************* */
-TEST( MultiProjectionFactor, 3poses_projection_factor ){
+TEST( SmartProjectionFactor, 3poses_projection_factor ){
   cout << " ************************ Normal ProjectionFactor: 3 cams + 3 landmarks **********************" << endl;
 
   Symbol x1('X',  1);