Merged from branch 'trunk'

gtsam_unstable/slam/SmartProjectionFactor.h

@@ -42,6 +42,7 @@ namespace gtsam {
     ///< (important that the order is the same as the keys that we use to create the factor)
     boost::shared_ptr<CALIBRATION> K_;  ///< shared pointer to calibration object
     const SharedNoiseModel noise_;   ///< noise model used
+    boost::optional<Point3> point_;
     boost::optional<POSE> body_P_sensor_; ///< The pose of the sensor in the body frame
 
     // verbosity handling for Cheirality Exceptions
@@ -73,8 +74,9 @@ namespace gtsam {
      */
     SmartProjectionFactor(const std::vector<Point2> measured, const SharedNoiseModel& model,
         std::vector<Key> poseKeys, const boost::shared_ptr<CALIBRATION>& K,
+        boost::optional<LANDMARK> point = boost::none,
         boost::optional<POSE> body_P_sensor = boost::none) :
-          measured_(measured),  K_(K), noise_(model), body_P_sensor_(body_P_sensor),
+          measured_(measured),  K_(K), noise_(model), point_(point), body_P_sensor_(body_P_sensor),
           throwCheirality_(false), verboseCheirality_(false) {
       keys_.assign(poseKeys.begin(), poseKeys.end());
     }
@@ -93,8 +95,9 @@ namespace gtsam {
     SmartProjectionFactor(const std::vector<Point2> measured, const SharedNoiseModel& model,
         std::vector<Key> poseKeys, const boost::shared_ptr<CALIBRATION>& K,
         bool throwCheirality, bool verboseCheirality,
+        boost::optional<LANDMARK> point = boost::none,
         boost::optional<POSE> body_P_sensor = boost::none) :
-          measured_(measured),  K_(K),  noise_(model), body_P_sensor_(body_P_sensor),
+          measured_(measured),  K_(K),  noise_(model), point_(point), body_P_sensor_(body_P_sensor),
           throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) {}
 
     /** Virtual destructor */
@@ -159,8 +162,23 @@ namespace gtsam {
       }
 
       // We triangulate the 3D position of the landmark
-      boost::optional<Point3> point = triangulatePoint3(cameraPoses, measured_, *K_);
+      if (debug) {
-
+        BOOST_FOREACH(const Pose3& pose, cameraPoses) {
+          std::cout << "Pose: " << pose << std::endl;
+        }
+        BOOST_FOREACH(const Point2& point, measured_) {
+          std::cout << "Point: " << point << std::endl;
+        }
+      }
+      boost::optional<Point3> point;
+      if (point_) {
+      	point = point_;
+        //std::cout << "Using existing point " << *point << std::endl;
+      } else {
+        //std::cout << "Triangulating in linearize " << std::endl;
+        point = triangulatePoint3(cameraPoses, measured_, *K_);
+      }
+      if (debug) std::cout << "Result: " << *point << std::endl;
 
 
       if (debug) {
@@ -179,6 +197,7 @@ namespace gtsam {
 
       // point is behind one of the cameras, turn factor off by setting everything to 0
       if (!point) {
+        std::cout << "WARNING: Could not triangulate during linearize" << std::endl;
         BOOST_FOREACH(gtsam::Matrix& m, Gs) m = zeros(6,6);
         BOOST_FOREACH(Vector& v, gs) v = zero(6);
         return HessianFactor::shared_ptr(new HessianFactor(js, Gs, gs, f));
@@ -365,6 +384,7 @@ namespace gtsam {
      * to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5.
      */
     virtual double error(const Values& values) const {
+      bool debug = false;
       if (this->active(values)) {
         double overallError=0;
 
@@ -379,7 +399,23 @@ namespace gtsam {
         }
 
         // We triangulate the 3D position of the landmark
-        boost::optional<Point3> point = triangulatePoint3(cameraPoses, measured_, *K_);
+        if (debug) {
+          BOOST_FOREACH(const Pose3& pose, cameraPoses) {
+            std::cout << "Pose: " << pose << std::endl;
+          }
+          BOOST_FOREACH(const Point2& point, measured_) {
+            std::cout << "Point: " << point << std::endl;
+          }
+        }
+        boost::optional<Point3> point;
+        if (point_) {
+        	point = point_;
+          //std::cout << "Using existing point " << *point << std::endl;
+        } else {
+          //std::cout << "Triangulate during error calc" << std::endl;
+          point = triangulatePoint3(cameraPoses, measured_, *K_);
+        }
+        if (debug) std::cout << "Result: " << *point << std::endl;
 
         if(point)
         { // triangulation produced a good estimate of landmark position
@@ -392,7 +428,8 @@ namespace gtsam {
             overallError += noise_->distance( reprojectionError.vector() );
           }
           return std::sqrt(overallError);
-        }else{ // triangulation failed: we deactivate the factor, then the error should not contribute to the overall error
+        } else{ // triangulation failed: we deactivate the factor, then the error should not contribute to the overall error
+          std::cout << "WARNING: Could not triangulate during error calc" << std::endl;
           return 0.0;
         }
       } else {
@@ -429,5 +466,6 @@ namespace gtsam {
       ar & BOOST_SERIALIZATION_NVP(throwCheirality_);
       ar & BOOST_SERIALIZATION_NVP(verboseCheirality_);
     }
+
   };
 } // \ namespace gtsam

gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp

@@ -32,6 +32,7 @@ TEST(SmartProjectionFactor, disabled)
 #include <gtsam_unstable/slam/SmartProjectionFactor.h>
 #include <gtsam/nonlinear/ISAM2.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/LinearContainerFactor.h>
 #include <gtsam/nonlinear/Values.h>
@@ -89,7 +90,7 @@ TEST( SmartProjectionFactor, ConstructorWithTransform) {
   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);
+  TestSmartProjectionFactor factor(measurements, model, views, K, boost::none, body_P_sensor);
 }
 
 /* ************************************************************************* */
@@ -115,8 +116,8 @@ TEST( SmartProjectionFactor, EqualsWithTransform ) {
 
   std::vector<Key> views;
   views += X(1);
-  TestSmartProjectionFactor factor1(measurements, model, views, K, body_P_sensor);
+  TestSmartProjectionFactor factor1(measurements, model, views, K, boost::none, body_P_sensor);
-  TestSmartProjectionFactor factor2(measurements, model, views, K, body_P_sensor);
+  TestSmartProjectionFactor factor2(measurements, model, views, K, boost::none, body_P_sensor);
 
   CHECK(assert_equal(factor1, factor2));
 }
@@ -171,6 +172,145 @@ TEST( SmartProjectionFactor, noisy ){
   // DOUBLES_EQUAL(expectedError, actualError, 1e-7);
 }
 
+/* ************************************************************************* */
+TEST( SmartProjectionFactor, 3poses_1iteration_projection_factor_comparison ){
+  cout << " ************************ SmartProjectionFactor: 3 cams + 3 landmarks, 1 iteration, comparison b/w Generic and Smart Projection Factors **********************" << endl;
+
+  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;
+
+  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);
+
+  // create second camera 1 meter to the right of first camera
+  Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0));
+  SimpleCamera cam2(pose2, *K);
+
+  // create third camera 1 meter above the first camera
+  Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0));
+  SimpleCamera cam3(pose3, *K);
+
+  // three landmarks ~5 meters infront of camera
+  Point3 landmark1(5, 0.5, 1.2);
+  Point3 landmark2(5, -0.5, 1.2);
+  Point3 landmark3(3, 0, 3.0);
+
+  vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;
+
+  // 1. Project three landmarks into three cameras and triangulate
+  Point2 cam1_uv1 = cam1.project(landmark1);
+  Point2 cam2_uv1 = cam2.project(landmark1);
+  Point2 cam3_uv1 = cam3.project(landmark1);
+  measurements_cam1 += cam1_uv1, cam2_uv1, cam3_uv1;
+
+  //
+  Point2 cam1_uv2 = cam1.project(landmark2);
+  Point2 cam2_uv2 = cam2.project(landmark2);
+  Point2 cam3_uv2 = cam3.project(landmark2);
+  measurements_cam2 += cam1_uv2, cam2_uv2, cam3_uv2;
+
+
+  Point2 cam1_uv3 = cam1.project(landmark3);
+  Point2 cam2_uv3 = cam2.project(landmark3);
+  Point2 cam3_uv3 = cam3.project(landmark3);
+  measurements_cam3 += cam1_uv3, cam2_uv3, cam3_uv3;
+
+  typedef SmartProjectionFactor<Pose3, Point3, Cal3_S2> SmartFactor;
+  typedef GenericProjectionFactor<Pose3, Point3> ProjectionFactor;
+
+  SmartFactor::shared_ptr smartFactor1(new SmartFactor(measurements_cam1, noiseProjection, views, K, boost::make_optional<Point3>(landmark1) ));
+  SmartFactor::shared_ptr smartFactor2(new SmartFactor(measurements_cam2, noiseProjection, views, K, boost::make_optional<Point3>(landmark2) ));
+  SmartFactor::shared_ptr smartFactor3(new SmartFactor(measurements_cam3, noiseProjection, views, K, boost::make_optional<Point3>(landmark3) ));
+
+  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
+
+  NonlinearFactorGraph graphWithOriginalFactor;
+  graphWithOriginalFactor.add(ProjectionFactor(cam1.project(landmark1), noiseProjection, x1, L(1), K));
+  graphWithOriginalFactor.add(ProjectionFactor(cam2.project(landmark1), noiseProjection, x2, L(1), K));
+  graphWithOriginalFactor.add(ProjectionFactor(cam3.project(landmark1), noiseProjection, x3, L(1), K));
+
+  graphWithOriginalFactor.add(ProjectionFactor(cam1.project(landmark2), noiseProjection, x1, L(2), K));
+  graphWithOriginalFactor.add(ProjectionFactor(cam2.project(landmark2), noiseProjection, x2, L(2), K));
+  graphWithOriginalFactor.add(ProjectionFactor(cam3.project(landmark2), noiseProjection, x3, L(2), K));
+
+  graphWithOriginalFactor.add(ProjectionFactor(cam1.project(landmark3), noiseProjection, x1, L(3), K));
+  graphWithOriginalFactor.add(ProjectionFactor(cam2.project(landmark3), noiseProjection, x2, L(3), K));
+  graphWithOriginalFactor.add(ProjectionFactor(cam3.project(landmark3), noiseProjection, x3, L(3), K));
+
+  graphWithOriginalFactor.add(PriorFactor<Pose3>(x1, pose1, noisePrior));
+  graphWithOriginalFactor.add(PriorFactor<Pose3>(x2, pose2, noisePrior));
+
+  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3));
+  Values valuesOriginalFactor;
+  valuesOriginalFactor.insert(x1, pose1);
+  valuesOriginalFactor.insert(x2, pose2);
+  valuesOriginalFactor.insert(x3, pose3* noise_pose);
+  valuesOriginalFactor.insert(L(1), landmark1);
+  valuesOriginalFactor.insert(L(2), landmark2);
+  valuesOriginalFactor.insert(L(3), landmark3);
+
+  NonlinearFactorGraph graphWithSmartFactor;
+  graphWithSmartFactor.push_back(smartFactor1);
+  graphWithSmartFactor.push_back(smartFactor2);
+  graphWithSmartFactor.push_back(smartFactor3);
+  graphWithSmartFactor.add(PriorFactor<Pose3>(x1, pose1, noisePrior));
+  graphWithSmartFactor.add(PriorFactor<Pose3>(x2, pose2, noisePrior));
+
+  Values valuesSmartFactor;
+  valuesSmartFactor.insert(x1, pose1);
+  valuesSmartFactor.insert(x2, pose2);
+  // initialize third pose with some noise, we expect it to move back to original pose3
+  valuesSmartFactor.insert(x3, pose3*noise_pose);
+  valuesSmartFactor.at<Pose3>(x3).print("Pose3 before optimization: ");
+  pose3.print("Pose3 ground truth: ");
+
+  LevenbergMarquardtParams params;
+  params.maxIterations = 1;
+  params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
+  params.verbosity = NonlinearOptimizerParams::ERROR;
+
+  Values resultWithOriginalFactor;
+  std::cout << "\n=========================================" << std::endl;
+  std::cout << "Optimizing GenericProjectionFactor" << std::endl;
+  LevenbergMarquardtOptimizer optimizerForOriginalFactor(graphWithOriginalFactor, valuesOriginalFactor, params);
+  resultWithOriginalFactor = optimizerForOriginalFactor.optimize();
+
+  Values resultWithSmartFactor;
+  std::cout << "\n=========================================" << std::endl;
+  std::cout << "Optimizing SmartProjectionfactor" << std::endl;
+  LevenbergMarquardtOptimizer optimizerForSmartFactor(graphWithSmartFactor, valuesSmartFactor, params);
+  resultWithSmartFactor = optimizerForSmartFactor.optimize();
+
+  std::cout << "\n=========================================" << std::endl;
+  // result.print("results of 3 camera, 3 landmark optimization \n");
+  resultWithOriginalFactor.at<Pose3>(x3).print("Original: Pose3 after optimization: ");
+  resultWithSmartFactor.at<Pose3>(x3).print("\nSmart: Pose3 after optimization: ");
+  EXPECT(assert_equal(resultWithOriginalFactor.at<Pose3>(x3),resultWithSmartFactor.at<Pose3>(x3)));
+
+  GaussNewtonParams params2;
+  params2.maxIterations = 1;
+  Values resultWithOriginalFactor2;
+  GaussNewtonOptimizer optimizerForOriginalFactor2(graphWithOriginalFactor, valuesOriginalFactor, params2);
+  resultWithOriginalFactor2 = optimizerForOriginalFactor2.optimize();
+
+  Values resultWithSmartFactor2;
+  GaussNewtonOptimizer optimizerForSmartFactor2(graphWithSmartFactor, valuesSmartFactor, params2);
+  resultWithSmartFactor2 = optimizerForSmartFactor2.optimize();
+
+  resultWithOriginalFactor2.at<Pose3>(x3).print("Original: Pose3 after optimization (GaussNewton): ");
+  resultWithSmartFactor2.at<Pose3>(x3).print("\nSmart: Pose3 after optimization (GaussNewton): ");
+
+}
+
 
 /* ************************************************************************* */
 TEST( SmartProjectionFactor, 3poses_smart_projection_factor ){
@@ -341,6 +481,7 @@ TEST( SmartProjectionFactor, 3poses_projection_factor ){
 }
 
 
+
 /* ************************************************************************* */
 TEST( SmartProjectionFactor, Hessian ){
   cout << " ************************ SmartProjectionFactor: Hessian **********************" << endl;