Merge branch 'feature/heterogeneousSmartFactorNoise' into feature/improvementsIncrementalFilter

.cproject

@@ -2862,6 +2862,14 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="testBatchFixedLagSmoother.run" path="build/gtsam_unstable/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j4</buildArguments>
+				<buildTarget>testBatchFixedLagSmoother.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="testGaussianFactor.run" path="build/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>

gtsam/geometry/CameraSet.h

@@ -56,7 +56,11 @@ protected:
     // Project and fill error vector
     Vector b(ZDim * m);
     for (size_t i = 0, row = 0; i < m; i++, row += ZDim) {
-      b.segment<ZDim>(row) = traits<Z>::Local(measured[i], predicted[i]);
+      Vector bi = traits<Z>::Local(measured[i], predicted[i]);
+      if(ZDim==3 && std::isnan(bi(1))){ // if it is a stereo point and the right pixel is missing (nan)
+        bi(1) = 0;
+      }
+      b.segment<ZDim>(row) = bi;
     }
     return b;
   }

gtsam/slam/SmartFactorBase.h

@@ -35,6 +35,16 @@
 
 namespace gtsam {
 
+/// Linearization mode: what factor to linearize to
+enum LinearizationMode {
+  HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD
+};
+
+/// How to manage degeneracy
+enum DegeneracyMode {
+  IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY
+};
+
 /**
  * @brief  Base class for smart factors
  * This base class has no internal point, but it has a measurement, noise model
@@ -202,7 +212,7 @@ public:
       boost::optional<typename Cameras::FBlocks&> Fs = boost::none, //
       boost::optional<Matrix&> E = boost::none) const {
     Vector ue = cameras.reprojectionError(point, measured_, Fs, E);
-    if(body_P_sensor_){
+    if(body_P_sensor_ && Fs){
       for(size_t i=0; i < Fs->size(); i++){
         Pose3 w_Pose_body = (cameras[i].pose()).compose(body_P_sensor_->inverse());
         Matrix J(6, 6);
@@ -210,9 +220,17 @@ public:
         Fs->at(i) = Fs->at(i) * J;
       }
     }
+    correctForMissingMeasurements(cameras, ue, Fs, E);
     return ue;
   }
 
+  /**
+   * This corrects the Jacobians for the case in which some pixel measurement is missing (nan)
+   * In practice, this does not do anything in the monocular case, but it is implemented in the stereo version
+   */
+  virtual void correctForMissingMeasurements(const Cameras& cameras, Vector& ue, boost::optional<typename Cameras::FBlocks&> Fs = boost::none,
+		  boost::optional<Matrix&> E = boost::none) const {}
+
   /**
    * Calculate vector of re-projection errors [h(x)-z] = [cameras.project(p) - z]
    * Noise model applied

gtsam/slam/SmartProjectionFactor.h

@@ -31,16 +31,6 @@
 
 namespace gtsam {
 
-/// Linearization mode: what factor to linearize to
-enum LinearizationMode {
-  HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD
-};
-
-/// How to manage degeneracy
-enum DegeneracyMode {
-  IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY
-};
-
 /*
  *  Parameters for the smart projection factors
  */

gtsam_unstable/slam/SmartStereoProjectionFactor.h

@@ -35,16 +35,6 @@
 
 namespace gtsam {
 
-/// Linearization mode: what factor to linearize to
- enum LinearizationMode {
-   HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD
- };
-
-/// How to manage degeneracy
-enum DegeneracyMode {
-   IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY
- };
-
   /*
   *  Parameters for the smart stereo projection factors
   */
@@ -119,8 +109,6 @@ enum DegeneracyMode {
    }
  };
 
-
-
 /**
  * SmartStereoProjectionFactor: triangulates point and keeps an estimate of it around.
  * This factor operates with StereoCamera. This factor requires that values
@@ -155,14 +143,19 @@ public:
   /// Vector of cameras
   typedef CameraSet<StereoCamera> Cameras;
 
+  /// Vector of monocular cameras (stereo treated as 2 monocular)
+  typedef PinholeCamera<Cal3_S2> MonoCamera;
+  typedef CameraSet<MonoCamera> MonoCameras;
+  typedef std::vector<Point2> MonoMeasurements;
+
   /**
    * Constructor
    * @param params internal parameters of the smart factors
    */
   SmartStereoProjectionFactor(const SharedNoiseModel& sharedNoiseModel,
-      const SmartStereoProjectionParams& params =
+      const SmartStereoProjectionParams& params = SmartStereoProjectionParams(),
-      SmartStereoProjectionParams()) :
+      const boost::optional<Pose3> body_P_sensor = boost::none) :
-      Base(sharedNoiseModel), //
+      Base(sharedNoiseModel, body_P_sensor), //
       params_(params), //
       result_(TriangulationResult::Degenerate()) {
   }
@@ -240,77 +233,30 @@ public:
     size_t m = cameras.size();
     bool retriangulate = decideIfTriangulate(cameras);
 
-//    if(!retriangulate)
+    // triangulate stereo measurements by treating each stereocamera as a pair of monocular cameras
-//      std::cout << "retriangulate = false" << std::endl;
+    MonoCameras monoCameras;
-//
+    MonoMeasurements monoMeasured;
-//    bool retriangulate = true;
-
-    if (retriangulate) {
-//      std::cout << "Retriangulate " << std::endl;
-      std::vector<Point3> reprojections;
-      reprojections.reserve(m);
     for(size_t i = 0; i < m; i++) {
-        reprojections.push_back(cameras[i].backproject(measured_[i]));
+      const Pose3 leftPose = cameras[i].pose();
+      const Cal3_S2 monoCal = cameras[i].calibration().calibration();
+      const MonoCamera leftCamera_i(leftPose,monoCal);
+      const Pose3 left_Pose_right = Pose3(Rot3(),Point3(cameras[i].baseline(),0.0,0.0));
+      const Pose3 rightPose = leftPose.compose( left_Pose_right );
+      const MonoCamera rightCamera_i(rightPose,monoCal);
+      const StereoPoint2 zi = measured_[i];
+      monoCameras.push_back(leftCamera_i);
+      monoMeasured.push_back(Point2(zi.uL(),zi.v()));
+      if(!std::isnan(zi.uR())){ // if right point is valid
+        monoCameras.push_back(rightCamera_i);
+        monoMeasured.push_back(Point2(zi.uR(),zi.v()));
       }
-
-      Point3 pw_sum(0,0,0);
-      for(const Point3& pw: reprojections) {
-        pw_sum = pw_sum + pw;
     }
-      // average reprojected landmark
+    if (retriangulate)
-      Point3 pw_avg = pw_sum / double(m);
+      result_ = gtsam::triangulateSafe(monoCameras, monoMeasured,
-
+          params_.triangulation);
-      double totalReprojError = 0;
-
-      // check if it lies in front of all cameras
-      for(size_t i = 0; i < m; i++) {
-        const Pose3& pose = cameras[i].pose();
-        const Point3& pl = pose.transform_to(pw_avg);
-        if (pl.z() <= 0) {
-          result_ = TriangulationResult::BehindCamera();
     return result_;
   }
 
-        // check landmark distance
-        if (params_.triangulation.landmarkDistanceThreshold > 0 &&
-            pl.norm() > params_.triangulation.landmarkDistanceThreshold) {
-          result_ = TriangulationResult::FarPoint();
-          return result_;
-        }
-
-        if (params_.triangulation.dynamicOutlierRejectionThreshold > 0) {
-          const StereoPoint2& zi = measured_[i];
-          StereoPoint2 reprojectionError(cameras[i].project(pw_avg) - zi);
-          totalReprojError += reprojectionError.vector().norm();
-        }
-      } // for
-
-      if (params_.triangulation.dynamicOutlierRejectionThreshold > 0
-          && totalReprojError / m > params_.triangulation.dynamicOutlierRejectionThreshold) {
-        result_ = TriangulationResult::Outlier();
-        return result_;
-      }
-
-      if(params_.triangulation.enableEPI) {
-        try {
-         pw_avg = triangulateNonlinear(cameras, measured_, pw_avg);
-        } catch(StereoCheiralityException& e) {
-          if(params_.verboseCheirality)
-            std::cout << "Cheirality Exception in SmartStereoProjectionFactor" << std::endl;
-          if(params_.throwCheirality)
-            throw;
-          result_ = TriangulationResult::BehindCamera();
-          return TriangulationResult::BehindCamera();
-        }
-      }
-
-      result_ = TriangulationResult(pw_avg);
-
-    } // if retriangulate
-    return result_;
-
-  }
-
   /// triangulate
   bool triangulateForLinearize(const Cameras& cameras) const {
     triangulateSafe(cameras); // imperative, might reset result_
@@ -570,6 +516,32 @@ public:
     }
   }
 
+  /**
+   * This corrects the Jacobians and error vector for the case in which the right pixel in the monocular camera is missing (nan)
+   */
+  virtual void correctForMissingMeasurements(const Cameras& cameras, Vector& ue,
+      boost::optional<typename Cameras::FBlocks&> Fs = boost::none,
+      boost::optional<Matrix&> E = boost::none) const
+  {
+    // when using stereo cameras, some of the measurements might be missing:
+    for(size_t i=0; i < cameras.size(); i++){
+      const StereoPoint2& z = measured_.at(i);
+      if(std::isnan(z.uR())) // if the right pixel is invalid
+      {
+        if(Fs){ // delete influence of right point on jacobian Fs
+          MatrixZD& Fi = Fs->at(i);
+          for(size_t ii=0; ii<Dim; ii++)
+            Fi(1,ii) = 0.0;
+        }
+        if(E) // delete influence of right point on jacobian E
+          E->row(ZDim * i + 1) = Matrix::Zero(1, E->cols());
+
+        // set the corresponding entry of vector ue to zero
+        ue(ZDim * i + 1) = 0.0;
+      }
+    }
+  }
+
   /** return the landmark */
     TriangulationResult point() const {
       return result_;

gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h

@@ -66,9 +66,9 @@ public:
    * @param params internal parameters of the smart factors
    */
   SmartStereoProjectionPoseFactor(const SharedNoiseModel& sharedNoiseModel,
-      const SmartStereoProjectionParams& params =
+      const SmartStereoProjectionParams& params = SmartStereoProjectionParams(),
-      SmartStereoProjectionParams()) :
+      const boost::optional<Pose3> body_P_sensor = boost::none) :
-      Base(sharedNoiseModel, params) {
+      Base(sharedNoiseModel, params, body_P_sensor) {
   }
 
   /** Virtual destructor */
@@ -102,7 +102,7 @@ public:
 
   /**
    * Variant of the previous one in which we include a set of measurements with the same noise and calibration
-   * @param mmeasurements vector of the 2m dimensional location of the projection of a single landmark in the m view (the measurement)
+   * @param measurements vector of the 2m dimensional location of the projection of a single landmark in the m view (the measurement)
    * @param poseKeys vector of keys corresponding to the camera observing the same landmark
    * @param K the (known) camera calibration (same for all measurements)
    */
@@ -161,7 +161,11 @@ public:
     Base::Cameras cameras;
     size_t i=0;
     for(const Key& k: this->keys_) {
-      const Pose3& pose = values.at<Pose3>(k);
+      Pose3 pose = values.at<Pose3>(k);
+
+      if (Base::body_P_sensor_)
+    	  pose = pose.compose(*(Base::body_P_sensor_));
+
       StereoCamera camera(pose, K_all_[i++]);
       cameras.push_back(camera);
     }

gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp

@@ -18,7 +18,7 @@
  *  @date   Sept 2013
  */
 
-// TODO #include <gtsam/slam/tests/smartFactorScenarios.h>
+#include <gtsam/slam/tests/smartFactorScenarios.h>
 #include <gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <gtsam/slam/PoseTranslationPrior.h>
@@ -33,8 +33,6 @@ using namespace boost::assign;
 using namespace gtsam;
 
 // make a realistic calibration matrix
-static double fov = 60; // degrees
-static size_t w = 640, h = 480;
 static double b = 1;
 
 static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fov, w, h, b));
@@ -62,6 +60,8 @@ static StereoPoint2 measurement1(323.0, 300.0, 240.0); //potentially use more re
 static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
     Point3(0.25, -0.10, 1.0));
 
+static double missing_uR = std::numeric_limits<double>::quiet_NaN();
+
 vector<StereoPoint2> stereo_projectToMultipleCameras(const StereoCamera& cam1,
     const StereoCamera& cam2, const StereoCamera& cam3, Point3 landmark) {
 
@@ -151,6 +151,60 @@ TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ) {
   //EXPECT(assert_equal(zero(4),actual,1e-8));
 }
 
+/* *************************************************************************/
+TEST( SmartProjectionPoseFactor, noiselessWithMissingMeasurements ) {
+
+  // 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),
+       Point3(0, 0, 1));
+   StereoCamera level_camera(level_pose, K2);
+
+   // create second camera 1 meter to the right of first camera
+   Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1, 0, 0));
+   StereoCamera level_camera_right(level_pose_right, K2);
+
+   // landmark ~5 meters in front of camera
+   Point3 landmark(5, 0.5, 1.2);
+
+   // 1. Project two landmarks into two cameras and triangulate
+   StereoPoint2 level_uv = level_camera.project(landmark);
+   StereoPoint2 level_uv_right = level_camera_right.project(landmark);
+   StereoPoint2 level_uv_right_missing(level_uv_right.uL(),missing_uR,level_uv_right.v());
+
+   Values values;
+   values.insert(x1, level_pose);
+   values.insert(x2, level_pose_right);
+
+   SmartStereoProjectionPoseFactor factor1(model);
+   factor1.add(level_uv, x1, K2);
+   factor1.add(level_uv_right_missing, x2, K2);
+
+   double actualError = factor1.error(values);
+   double expectedError = 0.0;
+   EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7);
+
+   // TEST TRIANGULATION WITH MISSING VALUES: i) right pixel of second camera is missing:
+   SmartStereoProjectionPoseFactor::Cameras cameras = factor1.cameras(values);
+   double actualError2 = factor1.totalReprojectionError(cameras);
+   EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7);
+
+   CameraSet<StereoCamera> cams;
+   cams += level_camera;
+   cams += level_camera_right;
+   TriangulationResult result = factor1.triangulateSafe(cams);
+   CHECK(result);
+   EXPECT(assert_equal(landmark, *result, 1e-7));
+
+   // TEST TRIANGULATION WITH MISSING VALUES: ii) right pixels of both cameras are missing:
+   SmartStereoProjectionPoseFactor factor2(model);
+   StereoPoint2 level_uv_missing(level_uv.uL(),missing_uR,level_uv.v());
+   factor2.add(level_uv_missing, x1, K2);
+   factor2.add(level_uv_right_missing, x2, K2);
+   result = factor2.triangulateSafe(cams);
+   CHECK(result);
+   EXPECT(assert_equal(landmark, *result, 1e-7));
+}
+
 /* *************************************************************************/
 TEST( SmartStereoProjectionPoseFactor, noisy ) {
   // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
@@ -248,8 +302,6 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) {
 
   const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
 
-
-
   NonlinearFactorGraph graph;
   graph.push_back(smartFactor1);
   graph.push_back(smartFactor2);
@@ -273,7 +325,7 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) {
               Point3(0.1, -0.1, 1.9)), values.at<Pose3>(x3)));
 
   //  cout << std::setprecision(10) << "\n----SmartStereoFactor graph initial error: " << graph.error(values) << endl;
-  EXPECT_DOUBLES_EQUAL(797312.95069157204, graph.error(values), 1e-7);
+  EXPECT_DOUBLES_EQUAL(833953.92789459578, graph.error(values), 1e-7); // initial error
 
   // get triangulated landmarks from smart factors
   Point3 landmark1_smart = *smartFactor1->point();
@@ -335,7 +387,7 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) {
   graph2.push_back(ProjectionFactor(measurements_l3[2], model, x3, L(3), K2, false, verboseCheirality));
 
 //  cout << std::setprecision(10) << "\n----StereoFactor graph initial error: " << graph2.error(values) << endl;
-  EXPECT_DOUBLES_EQUAL(797312.95069157204, graph2.error(values), 1e-7);
+  EXPECT_DOUBLES_EQUAL(833953.92789459578, graph2.error(values), 1e-7);
 
   LevenbergMarquardtOptimizer optimizer2(graph2, values, lm_params);
   Values result2 = optimizer2.optimize();
@@ -344,7 +396,192 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) {
 //  cout << std::setprecision(10) << "StereoFactor graph optimized error: " << graph2.error(result2) << endl;
 
 }
+/* *************************************************************************/
+TEST( SmartStereoProjectionPoseFactor, body_P_sensor ) {
 
+  // camera has some displacement
+  Pose3 body_P_sensor = Pose3(Rot3::Ypr(-0.01, 0., -0.05), Point3(0.1, 0, 0.1));
+  // 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), Point3(0, 0, 1));
+  StereoCamera cam1(pose1.compose(body_P_sensor), K2);
+
+  // create second camera 1 meter to the right of first camera
+  Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0));
+  StereoCamera cam2(pose2.compose(body_P_sensor), K2);
+
+  // create third camera 1 meter above the first camera
+  Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0));
+  StereoCamera cam3(pose3.compose(body_P_sensor), K2);
+
+  // 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);
+
+  // 1. Project three landmarks into three cameras and triangulate
+  vector<StereoPoint2> measurements_l1 = stereo_projectToMultipleCameras(cam1,
+      cam2, cam3, landmark1);
+  vector<StereoPoint2> measurements_l2 = stereo_projectToMultipleCameras(cam1,
+      cam2, cam3, landmark2);
+  vector<StereoPoint2> measurements_l3 = stereo_projectToMultipleCameras(cam1,
+      cam2, cam3, landmark3);
+
+  vector<Key> views;
+  views.push_back(x1);
+  views.push_back(x2);
+  views.push_back(x3);
+
+  SmartStereoProjectionParams smart_params;
+  smart_params.triangulation.enableEPI = true;
+  SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(model, smart_params, body_P_sensor));
+  smartFactor1->add(measurements_l1, views, K2);
+
+  SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(model, smart_params, body_P_sensor));
+  smartFactor2->add(measurements_l2, views, K2);
+
+  SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(model, smart_params, body_P_sensor));
+  smartFactor3->add(measurements_l3, views, K2);
+
+  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
+
+  NonlinearFactorGraph graph;
+  graph.push_back(smartFactor1);
+  graph.push_back(smartFactor2);
+  graph.push_back(smartFactor3);
+  graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
+  graph.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));
+
+  //  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, pose1);
+  values.insert(x2, pose2);
+  // initialize third pose with some noise, we expect it to move back to original pose3
+  values.insert(x3, pose3 * noise_pose);
+  EXPECT(
+      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)));
+
+  //  cout << std::setprecision(10) << "\n----SmartStereoFactor graph initial error: " << graph.error(values) << endl;
+  EXPECT_DOUBLES_EQUAL(953392.32838422502, graph.error(values), 1e-7); // initial error
+
+  Values result;
+  gttic_(SmartStereoProjectionPoseFactor);
+  LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
+  result = optimizer.optimize();
+  gttoc_(SmartStereoProjectionPoseFactor);
+  tictoc_finishedIteration_();
+
+  EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-5);
+
+  // result.print("results of 3 camera, 3 landmark optimization \n");
+  EXPECT(assert_equal(pose3, result.at<Pose3>(x3)));
+}
+/* *************************************************************************/
+TEST( SmartStereoProjectionPoseFactor, body_P_sensor_monocular ){
+  // make a realistic calibration matrix
+  double fov = 60; // degrees
+  size_t w=640,h=480;
+
+  Cal3_S2::shared_ptr K(new Cal3_S2(fov,w,h));
+
+  // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
+  Pose3 cameraPose1 = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); // body poses
+  Pose3 cameraPose2 = cameraPose1 * Pose3(Rot3(), Point3(1,0,0));
+  Pose3 cameraPose3 = cameraPose1 * Pose3(Rot3(), Point3(0,-1,0));
+
+  SimpleCamera cam1(cameraPose1, *K); // with camera poses
+  SimpleCamera cam2(cameraPose2, *K);
+  SimpleCamera cam3(cameraPose3, *K);
+
+  // create arbitrary body_Pose_sensor (transforms from sensor to body)
+  Pose3 sensor_to_body =  Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(1, 1, 1)); // Pose3(); //
+
+  // These are the poses we want to estimate, from camera measurements
+  Pose3 bodyPose1 = cameraPose1.compose(sensor_to_body.inverse());
+  Pose3 bodyPose2 = cameraPose2.compose(sensor_to_body.inverse());
+  Pose3 bodyPose3 = cameraPose3.compose(sensor_to_body.inverse());
+
+  // three landmarks ~5 meters infront of camera
+  Point3 landmark1(5, 0.5, 1.2);
+  Point3 landmark2(5, -0.5, 1.2);
+  Point3 landmark3(5, 0, 3.0);
+
+  vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;
+
+  // Project three landmarks into three cameras
+  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
+  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
+  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
+
+  // Create smart factors
+  std::vector<Key> views;
+  views.push_back(x1);
+  views.push_back(x2);
+  views.push_back(x3);
+
+  // convert measurement to (degenerate) stereoPoint2 (with right pixel being NaN)
+  vector<StereoPoint2> measurements_cam1_stereo, measurements_cam2_stereo, measurements_cam3_stereo;
+  for(size_t k=0; k<measurements_cam1.size();k++)
+	  measurements_cam1_stereo.push_back(StereoPoint2(measurements_cam1[k].x() , missing_uR , measurements_cam1[k].y()));
+
+  for(size_t k=0; k<measurements_cam2.size();k++)
+	  measurements_cam2_stereo.push_back(StereoPoint2(measurements_cam2[k].x() , missing_uR , measurements_cam2[k].y()));
+
+  for(size_t k=0; k<measurements_cam3.size();k++)
+	  measurements_cam3_stereo.push_back(StereoPoint2(measurements_cam3[k].x() , missing_uR , measurements_cam3[k].y()));
+
+  SmartStereoProjectionParams params;
+  params.setRankTolerance(1.0);
+  params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY);
+  params.setEnableEPI(false);
+
+  Cal3_S2Stereo::shared_ptr Kmono(new Cal3_S2Stereo(fov,w,h,b));
+  SmartStereoProjectionPoseFactor smartFactor1(model, params, sensor_to_body);
+  smartFactor1.add(measurements_cam1_stereo, views, Kmono);
+
+  SmartStereoProjectionPoseFactor smartFactor2(model, params, sensor_to_body);
+  smartFactor2.add(measurements_cam2_stereo, views, Kmono);
+
+  SmartStereoProjectionPoseFactor smartFactor3(model, params, sensor_to_body);
+  smartFactor3.add(measurements_cam3_stereo, views, Kmono);
+
+  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
+
+  // Put all factors in factor graph, adding priors
+  NonlinearFactorGraph graph;
+  graph.push_back(smartFactor1);
+  graph.push_back(smartFactor2);
+  graph.push_back(smartFactor3);
+  graph.push_back(PriorFactor<Pose3>(x1, bodyPose1, noisePrior));
+  graph.push_back(PriorFactor<Pose3>(x2, bodyPose2, noisePrior));
+
+  // Check errors at ground truth poses
+  Values gtValues;
+  gtValues.insert(x1, bodyPose1);
+  gtValues.insert(x2, bodyPose2);
+  gtValues.insert(x3, bodyPose3);
+  double actualError = graph.error(gtValues);
+  double expectedError = 0.0;
+  DOUBLES_EQUAL(expectedError, actualError, 1e-7)
+
+  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, bodyPose1);
+  values.insert(x2, bodyPose2);
+  // initialize third pose with some noise, we expect it to move back to original pose3
+  values.insert(x3, bodyPose3*noise_pose);
+
+  LevenbergMarquardtParams lmParams;
+  Values result;
+  LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
+  result = optimizer.optimize();
+  EXPECT(assert_equal(bodyPose3,result.at<Pose3>(x3)));
+}
 /* *************************************************************************/
 TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) {
 
@@ -411,6 +648,78 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) {
   EXPECT(assert_equal(pose3, result.at<Pose3>(x3)));
 }
 
+/* *************************************************************************/
+TEST( SmartStereoProjectionPoseFactor, jacobianSVDwithMissingValues ) {
+
+  vector<Key> views;
+  views.push_back(x1);
+  views.push_back(x2);
+  views.push_back(x3);
+
+  // 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), Point3(0, 0, 1));
+  StereoCamera cam1(pose1, K);
+  // create second camera 1 meter to the right of first camera
+  Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0));
+  StereoCamera cam2(pose2, K);
+  // create third camera 1 meter above the first camera
+  Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0));
+  StereoCamera 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);
+
+  // 1. Project three landmarks into three cameras and triangulate
+  vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1,
+      cam2, cam3, landmark1);
+  vector<StereoPoint2> measurements_cam2 = stereo_projectToMultipleCameras(cam1,
+      cam2, cam3, landmark2);
+  vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1,
+      cam2, cam3, landmark3);
+
+  // DELETE SOME MEASUREMENTS
+  StereoPoint2 sp = measurements_cam1[1];
+  measurements_cam1[1] = StereoPoint2(sp.uL(), missing_uR, sp.v());
+  sp = measurements_cam2[2];
+  measurements_cam2[2] = StereoPoint2(sp.uL(), missing_uR, sp.v());
+
+  SmartStereoProjectionParams params;
+  params.setLinearizationMode(JACOBIAN_SVD);
+
+  SmartStereoProjectionPoseFactor::shared_ptr smartFactor1( new SmartStereoProjectionPoseFactor(model, params));
+  smartFactor1->add(measurements_cam1, views, K);
+
+  SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(model, params));
+  smartFactor2->add(measurements_cam2, views, K);
+
+  SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(model, params));
+  smartFactor3->add(measurements_cam3, views, K);
+
+  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
+
+  NonlinearFactorGraph graph;
+  graph.push_back(smartFactor1);
+  graph.push_back(smartFactor2);
+  graph.push_back(smartFactor3);
+  graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
+  graph.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));
+
+  //  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, pose1);
+  values.insert(x2, pose2);
+  values.insert(x3, pose3 * noise_pose);
+
+  Values result;
+  LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
+  result = optimizer.optimize();
+  EXPECT(assert_equal(pose3, result.at<Pose3>(x3),1e-7));
+}
+
 /* *************************************************************************/
 TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) {
 
@@ -562,7 +871,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
   EXPECT_DOUBLES_EQUAL(0, smartFactor4->error(values), 1e-9);
 
   // dynamic outlier rejection is off
-  EXPECT_DOUBLES_EQUAL(6700, smartFactor4b->error(values), 1e-9);
+  EXPECT_DOUBLES_EQUAL(6147.3947317473921, smartFactor4b->error(values), 1e-9);
 
   // Factors 1-3 should have valid point, factor 4 should not
   EXPECT(smartFactor1->point());
@@ -1039,7 +1348,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) {
 }
 
 /* *************************************************************************/
-TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) {
+TEST( SmartStereoProjectionPoseFactor, HessianWithRotationNonDegenerate ) {
 
   vector<Key> views;
   views.push_back(x1);
@@ -1072,6 +1381,9 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) {
   boost::shared_ptr<GaussianFactor> hessianFactor = smartFactor->linearize(
       values);
 
+  // check that it is non degenerate
+  EXPECT(smartFactor->isValid());
+
   Pose3 poseDrift = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0));
 
   Values rotValues;
@@ -1082,6 +1394,9 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) {
   boost::shared_ptr<GaussianFactor> hessianFactorRot = smartFactor->linearize(
       rotValues);
 
+  // check that it is non degenerate
+  EXPECT(smartFactor->isValid());
+
   // Hessian is invariant to rotations in the nondegenerate case
   EXPECT(
       assert_equal(hessianFactor->information(),
@@ -1098,10 +1413,14 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) {
   boost::shared_ptr<GaussianFactor> hessianFactorRotTran =
       smartFactor->linearize(tranValues);
 
-  // Hessian is invariant to rotations and translations in the nondegenerate case
+  // Hessian is invariant to rotations and translations in the degenerate case
   EXPECT(
       assert_equal(hessianFactor->information(),
+#ifdef GTSAM_USE_EIGEN_MKL
+          hessianFactorRotTran->information(), 1e-5));
+#else
           hessianFactorRotTran->information(), 1e-6));
+#endif
 }
 
 /* ************************************************************************* */