Modifications to SmartProjectionFactor and unit test: work in progress

gtsam_unstable/slam/SmartProjectionFactor.h

@@ -181,7 +181,46 @@ namespace gtsam {
     }
 
     /// linearize returns a Hessianfactor that is an approximation of error(p)
-    virtual boost::shared_ptr<GaussianFactor> linearize(const Values& x,  const Ordering& ordering) const {
+    virtual boost::shared_ptr<GaussianFactor> linearize(const Values& values,  const Ordering& ordering) const {
+
+      std::vector<Matrix> Hx(keys_.size());
+      std::vector<Matrix> Hl(keys_.size());
+      std::vector<Vector> b(keys_.size());
+
+      // Collect all poses (Cameras)
+      std::vector<Pose3> cameraPoses;
+
+      BOOST_FOREACH(const Key& k, keys_) {
+        if(body_P_sensor_)
+          cameraPoses.push_back(values.at<Pose3>(k).compose(*body_P_sensor_));
+        else
+          cameraPoses.push_back(values.at<Pose3>(k));
+      }
+
+      // We triangulate the 3D position of the landmark
+      boost::optional<Point3> point = triangulatePoint3(cameraPoses, measured_, *K_);
+
+      if(point){
+        for(size_t i = 0; i < measured_.size(); i++) {
+          Pose3 pose = cameraPoses.at(i);
+          PinholeCamera<CALIBRATION> camera(pose, *K_);
+          b.at(i) = ( camera.project(*point,Hx.at(i),Hl.at(i)) - measured_.at(i) ).vector();
+        }
+      }
+      else{
+        return HessianFactor::shared_ptr(new HessianFactor());
+      }
+
+      // Allocate m^2 matrix blocks
+      std::vector< std::vector<Matrix> > Hxl(keys_.size(), std::vector<Matrix>( keys_.size()));
+
+      // Allocate inv(Hl'Hl)
+      Matrix3 C;
+      for(size_t i1 = 0; i1 < keys_.size(); i1++) {
+         C += Hl.at(i1).transpose() * Hl.at(i1);
+      }
+      C = C.inverse();
+
 
       // fill in the keys
       std::vector<Index> js;
@@ -189,25 +228,32 @@ namespace gtsam {
         js += ordering[k];
       }
 
+      // Calculate sub blocks
+      for(size_t i1 = 0; i1 < keys_.size(); i1++) {
+        for(size_t i2 = 0; i2 < keys_.size(); i2++) {
+          Hxl[i1][i2] = Hx.at(i1).transpose() * Hl.at(i1) * C * Hl.at(i2).transpose();
+        }
+      }
 
-      std::vector<Matrix> Gs;
-
-      std::vector<Vector> gs;
       // Shur complement trick
 
-//      double e = u + b - z , e2 = e * e;
+      // Populate Gs and gs
-//      double c = 2 * logSqrt2PI - log(p) + e2 * p;
+      std::vector<Matrix> Gs(keys_.size()*(keys_.size()+1)/2);
-//      Vector g1 = Vector_(1, -e * p);
+      std::vector<Vector> gs(keys_.size());
-//      Vector g2 = Vector_(1, 0.5 / p - 0.5 * e2);
-//      Vector g3 = Vector_(1, -e * p);
-//      Matrix G11 = Matrix_(1, 1, p);
-//      Matrix G12 = Matrix_(1, 1, e);
-//      Matrix G13 = Matrix_(1, 1, p);
-//      Matrix G22 = Matrix_(1, 1, 0.5 / (p * p));
-//      Matrix G23 = Matrix_(1, 1, e);
-//      Matrix G33 = Matrix_(1, 1, p);
-
       double f = 0;
+      int GsCount = 0;
+      for(size_t i1 = 0; i1 < keys_.size(); i1++) {
+        gs.at(i1) = Hx.at(i1).transpose() * b.at(i1);
+
+        for(size_t i2 = 0; i2 < keys_.size(); i2++) {
+          gs.at(i1) += Hxl[i1][i2] * b.at(i2);
+
+          if (i2 >= i1) {
+            Gs.at(GsCount) = Hx.at(i1).transpose() * Hx.at(i1) - Hxl[i1][i2] * Hx.at(i2);
+            GsCount++;
+          }
+        }
+      }
 
       return HessianFactor::shared_ptr(new HessianFactor(js, Gs, gs, f));
     }

gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp

@@ -110,9 +110,6 @@ TEST( MultiProjectionFactor, noiseless ){
 TEST( MultiProjectionFactor, noisy ){
   cout << " ************************ MultiProjectionFactor: noisy ****************************" << endl;
 
-  Values theta;
-  NonlinearFactorGraph graph;
-
   Symbol x1('X',  1);
   Symbol x2('X',  2);
 //  Symbol x3('X',  3);
@@ -139,24 +136,98 @@ TEST( MultiProjectionFactor, noisy ){
   Point2 level_uv = level_camera.project(landmark) + pixelError;
   Point2 level_uv_right = level_camera_right.project(landmark);
 
-  Values value;
+  Values values;
-  value.insert(x1, level_pose);
+  values.insert(x1, level_pose);
-  value.insert(x2, level_pose_right);
+  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;
 
-  SmartProjectionFactor<Pose3, Point3, Cal3_S2> smartFactor(measurements, noiseProjection, views, K);
+  SmartProjectionFactor<Pose3, Point3, Cal3_S2>::shared_ptr
+    smartFactor(new SmartProjectionFactor<Pose3, Point3, Cal3_S2>(measurements, noiseProjection, views, K));
 
-  double actualError = smartFactor.error(value);
+  double actualError = smartFactor->error(values);
   double expectedError = sqrt(0.08);
 
+  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
+
+  NonlinearFactorGraph graph;
+  graph.push_back(smartFactor);
+  graph.add(PriorFactor<Pose3>(x1, level_pose, noisePrior));
+
+  LevenbergMarquardtOptimizer optimizer(graph, values);
+  Values result = optimizer.optimize();
+
+  result.print("results of the optimization \n");
   // 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);
 }
 
+
+///* ************************************************************************* */
+TEST( MultiProjectionFactor, 3poses ){
+  cout << " ************************ MultiProjectionFactor: noisy ****************************" << 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 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
+  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 pixelError(0.2,0.2);
+  Point2 level_uv = level_camera.project(landmark) + pixelError;
+  Point2 level_uv_right = level_camera_right.project(landmark);
+
+  Values values;
+  values.insert(x1, level_pose);
+  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;
+
+  SmartProjectionFactor<Pose3, Point3, Cal3_S2>::shared_ptr
+    smartFactor(new SmartProjectionFactor<Pose3, Point3, Cal3_S2>(measurements, noiseProjection, views, K));
+
+  double actualError = smartFactor->error(values);
+  double expectedError = sqrt(0.08);
+
+  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
+
+  NonlinearFactorGraph graph;
+  graph.push_back(smartFactor);
+  graph.add(PriorFactor<Pose3>(x1, level_pose, noisePrior));
+
+  LevenbergMarquardtOptimizer optimizer(graph, values);
+  Values result = optimizer.optimize();
+
+  result.print("results of the optimization \n");
+  // 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);
+}
+
+
 ///* ************************************************************************* */
 //TEST( ProjectionFactor, nonStandard ) {
 //  GenericProjectionFactor<Pose3, Point3, Cal3DS2> f;