From a82262cf998492579de7cc19550369d5a74b406b Mon Sep 17 00:00:00 2001
From: Luca Carlone <lucacar85@gmail.com>
Date: Thu, 8 Aug 2013 14:00:26 +0000
Subject: [PATCH] Included noise model and f term in SmartProjectionFactor

---
 gtsam_unstable/slam/SmartProjectionFactor.h   | 10 +++-
 .../slam/tests/testSmartProjectionFactor.cpp  | 56 +++++++++++++++++++
 2 files changed, 64 insertions(+), 2 deletions(-)

diff --git a/gtsam_unstable/slam/SmartProjectionFactor.h b/gtsam_unstable/slam/SmartProjectionFactor.h
index 386f8b349..9faf32cd1 100644
--- a/gtsam_unstable/slam/SmartProjectionFactor.h
+++ b/gtsam_unstable/slam/SmartProjectionFactor.h
@@ -168,12 +168,12 @@ namespace gtsam {
         std::cout << "point " << *point << std::endl;
       }
 
+      std::vector<Index> js;
       std::vector<Matrix> Gs(keys_.size()*(keys_.size()+1)/2);
       std::vector<Vector> gs(keys_.size());
+      double f=0;
 
       // fill in the keys
-      double f = 0;
-      std::vector<Index> js;
       BOOST_FOREACH(const Key& k, keys_) {
         js += ordering[k];
       }
@@ -192,6 +192,8 @@ namespace gtsam {
           std::cout << "pose " << pose << std::endl;
           PinholeCamera<CALIBRATION> camera(pose, *K_);
           b.at(i) = ( camera.project(*point,Hx.at(i),Hl.at(i)) - measured_.at(i) ).vector();
+          noise_-> WhitenSystem(Hx.at(i), Hl.at(i), b.at(i));
+          f += b.at(i).squaredNorm();
         }
 
         // Shur complement trick
@@ -276,6 +278,10 @@ namespace gtsam {
           PinholeCamera<CALIBRATION> camera(pose, *K_);
           Matrix Hxi, Hli;
            Vector bi = ( camera.project(*point,Hxi,Hli) - measured_.at(i) ).vector();
+
+           noise_-> WhitenSystem(Hxi, Hli, bi);
+           f += bi.squaredNorm();
+
            Hx2.block( 2*i, 6*i, 2, 6 ) = Hxi;
            Hl2.block( 2*i, 0, 2, 3  ) = Hli;
 
diff --git a/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp b/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp
index 5394e7f1d..15c682505 100644
--- a/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp
+++ b/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp
@@ -326,6 +326,62 @@ TEST( SmartProjectionFactor, 3poses_projection_factor ){
 
   result.print("Regular Projection Factor: results of 3 camera, 3 landmark optimization \n");
 
+}
+
+
+/* ************************************************************************* */
+TEST( SmartProjectionFactor, Hessian ){
+  cout << " ************************ Normal ProjectionFactor: Hessian **********************" << endl;
+
+  Symbol x1('X',  1);
+  Symbol x2('X',  2);
+
+  const SharedDiagonal noiseProjection = noiseModel::Isotropic::Sigma(2, 1);
+
+  std::vector<Key> views;
+  views += x1, x2;
+
+  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);
+
+  // three landmarks ~5 meters infront of camera
+  Point3 landmark1(5, 0.5, 1.2);
+
+  // 1. Project three landmarks into three cameras and triangulate
+  Point2 cam1_uv1 = cam1.project(landmark1);
+  Point2 cam2_uv1 = cam2.project(landmark1);
+  vector<Point2> measurements_cam1;
+  measurements_cam1 += cam1_uv1, cam2_uv1;
+
+  SmartProjectionFactor<Pose3, Point3, Cal3_S2> smartFactor(measurements_cam1, noiseProjection, views, K);
+
+  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3));
+  Values values;
+  values.insert(x1, pose1);
+  values.insert(x2, pose2);
+  //  values.insert(L(1), landmark1);
+
+  Ordering ordering;
+  ordering.push_back(x1);
+  ordering.push_back(x2);
+
+  boost::shared_ptr<GaussianFactor> hessianFactor = smartFactor.linearize(values, ordering);
+  hessianFactor->print("Hessian factor \n");
+
+  // compute triangulation from linearization point
+  // compute reprojection errors
+  // compare with hessianFactor.info(): the bottom right element is the squared sum of the reprojection errors (normalized by the covariance)
+
+
+
+
+
 }