Merged in feature/heterogeneousSmartFactorNoise (pull request #271)

Feature/heterogeneoussmartfactornoise

Approved-by: Chris Beall
Approved-by: Jing Dong

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/geometry/StereoCamera.cpp

@@ -39,7 +39,10 @@ namespace gtsam {
 
     const Point3 q = leftCamPose_.transform_to(point);
 
-    if ( q.z() <= 0 ) throw StereoCheiralityException();
+#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
+    if (q.z() <= 0)
+      throw StereoCheiralityException();
+#endif
 
     // get calibration
     const Cal3_S2Stereo& K = *K_;

gtsam/geometry/tests/testStereoCamera.cpp

@@ -104,6 +104,23 @@ TEST( StereoCamera, Dproject)
   CHECK(assert_equal(expected2,actual2,1e-7));
 }
 
+/* ************************************************************************* */
+TEST( StereoCamera, projectCheirality)
+{
+  // create a Stereo camera
+  Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5));
+  StereoCamera stereoCam(Pose3(), K);
+
+  // point behind the camera
+  Point3 p(0, 0, -5);
+#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
+  CHECK_EXCEPTION(stereoCam.project2(p), StereoCheiralityException);
+#else // otherwise project should not throw the exception
+  StereoPoint2 expected = StereoPoint2(320, 470, 240);
+  CHECK(assert_equal(expected,stereoCam.project2(p),1e-7));
+#endif
+}
+
 /* ************************************************************************* */
 TEST( StereoCamera, backproject_case1)
 {

gtsam/slam/SmartFactorBase.h

@@ -202,7 +202,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 +210,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/SmartFactorParams.h

@@ -0,0 +1,134 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file   SmartFactorParams
+ * @brief  Collect common parameters for SmartProjection and SmartStereoProjection factors
+ * @author Luca Carlone
+ * @author Zsolt Kira
+ * @author Frank Dellaert
+ */
+
+#pragma once
+
+#include <gtsam/geometry/triangulation.h>
+
+namespace gtsam {
+
+/**
+ * SmartFactorParams: parameters and (linearization/degeneracy) modes for SmartProjection and SmartStereoProjection factors
+ */
+/// 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
+ */
+struct GTSAM_EXPORT SmartProjectionParams {
+
+  LinearizationMode linearizationMode; ///< How to linearize the factor
+  DegeneracyMode degeneracyMode; ///< How to linearize the factor
+
+  /// @name Parameters governing the triangulation
+  /// @{
+  TriangulationParameters triangulation;
+  double retriangulationThreshold; ///< threshold to decide whether to re-triangulate
+  /// @}
+
+  /// @name Parameters governing how triangulation result is treated
+  /// @{
+  bool throwCheirality; ///< If true, re-throws Cheirality exceptions (default: false)
+  bool verboseCheirality; ///< If true, prints text for Cheirality exceptions (default: false)
+  /// @}
+
+  // Constructor
+  SmartProjectionParams(LinearizationMode linMode = HESSIAN,
+      DegeneracyMode degMode = IGNORE_DEGENERACY, bool throwCheirality = false,
+      bool verboseCheirality = false, double retriangulationTh = 1e-5) :
+        linearizationMode(linMode), degeneracyMode(degMode), retriangulationThreshold(
+            retriangulationTh), throwCheirality(throwCheirality), verboseCheirality(
+                verboseCheirality) {
+  }
+
+  virtual ~SmartProjectionParams() {
+  }
+
+  void print(const std::string& str = "") const {
+    std::cout << "linearizationMode: " << linearizationMode << "\n";
+    std::cout << "   degeneracyMode: " << degeneracyMode << "\n";
+    std::cout << triangulation << std::endl;
+  }
+
+  // get class variables
+  LinearizationMode getLinearizationMode() const {
+    return linearizationMode;
+  }
+  DegeneracyMode getDegeneracyMode() const {
+    return degeneracyMode;
+  }
+  TriangulationParameters getTriangulationParameters() const {
+    return triangulation;
+  }
+  bool getVerboseCheirality() const {
+    return verboseCheirality;
+  }
+  bool getThrowCheirality() const {
+    return throwCheirality;
+  }
+  double getRetriangulationThreshold() const {
+    return retriangulationThreshold;
+  }
+  // set class variables
+  void setLinearizationMode(LinearizationMode linMode) {
+    linearizationMode = linMode;
+  }
+  void setDegeneracyMode(DegeneracyMode degMode) {
+    degeneracyMode = degMode;
+  }
+  void setRetriangulationThreshold(double retriangulationTh) {
+    retriangulationThreshold = retriangulationTh;
+  }
+  void setRankTolerance(double rankTol) {
+    triangulation.rankTolerance = rankTol;
+  }
+  void setEnableEPI(bool enableEPI) {
+    triangulation.enableEPI = enableEPI;
+  }
+  void setLandmarkDistanceThreshold(double landmarkDistanceThreshold) {
+    triangulation.landmarkDistanceThreshold = landmarkDistanceThreshold;
+  }
+  void setDynamicOutlierRejectionThreshold(double dynOutRejectionThreshold) {
+    triangulation.dynamicOutlierRejectionThreshold = dynOutRejectionThreshold;
+  }
+
+private:
+
+  /// Serialization function
+  friend class boost::serialization::access;
+  template<class ARCHIVE>
+  void serialize(ARCHIVE & ar, const unsigned int version) {
+    ar & BOOST_SERIALIZATION_NVP(linearizationMode);
+    ar & BOOST_SERIALIZATION_NVP(degeneracyMode);
+    ar & BOOST_SERIALIZATION_NVP(triangulation);
+    ar & BOOST_SERIALIZATION_NVP(retriangulationThreshold);
+    ar & BOOST_SERIALIZATION_NVP(throwCheirality);
+    ar & BOOST_SERIALIZATION_NVP(verboseCheirality);
+  }
+};
+
+} // \ namespace gtsam

gtsam/slam/SmartProjectionFactor.h

@@ -20,6 +20,7 @@
 #pragma once
 
 #include <gtsam/slam/SmartFactorBase.h>
+#include <gtsam/slam/SmartFactorParams.h>
 
 #include <gtsam/geometry/triangulation.h>
 #include <gtsam/inference/Symbol.h>
@@ -31,109 +32,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
- */
-struct GTSAM_EXPORT SmartProjectionParams {
-
-  LinearizationMode linearizationMode; ///< How to linearize the factor
-  DegeneracyMode degeneracyMode; ///< How to linearize the factor
-
-  /// @name Parameters governing the triangulation
-  /// @{
-  TriangulationParameters triangulation;
-  double retriangulationThreshold; ///< threshold to decide whether to re-triangulate
-  /// @}
-
-  /// @name Parameters governing how triangulation result is treated
-  /// @{
-  bool throwCheirality; ///< If true, re-throws Cheirality exceptions (default: false)
-  bool verboseCheirality; ///< If true, prints text for Cheirality exceptions (default: false)
-  /// @}
-
-  // Constructor
-  SmartProjectionParams(LinearizationMode linMode = HESSIAN,
-      DegeneracyMode degMode = IGNORE_DEGENERACY, bool throwCheirality = false,
-      bool verboseCheirality = false, double retriangulationTh = 1e-5) :
-      linearizationMode(linMode), degeneracyMode(degMode), retriangulationThreshold(
-    		  retriangulationTh), throwCheirality(throwCheirality), verboseCheirality(
-          verboseCheirality) {
-  }
-
-  virtual ~SmartProjectionParams() {
-  }
-
-  void print(const std::string& str) const {
-    std::cout << "linearizationMode: " << linearizationMode << "\n";
-    std::cout << "   degeneracyMode: " << degeneracyMode << "\n";
-    std::cout << triangulation << std::endl;
-  }
-
-  LinearizationMode getLinearizationMode() const {
-    return linearizationMode;
-  }
-  DegeneracyMode getDegeneracyMode() const {
-    return degeneracyMode;
-  }
-  TriangulationParameters getTriangulationParameters() const {
-    return triangulation;
-  }
-  bool getVerboseCheirality() const {
-    return verboseCheirality;
-  }
-  bool getThrowCheirality() const {
-    return throwCheirality;
-  }
-  double getRetriangulationThreshold() const {
-	return retriangulationThreshold;
-  }
-  void setLinearizationMode(LinearizationMode linMode) {
-    linearizationMode = linMode;
-  }
-  void setRetriangulationThreshold(double retriangulationTh) {
-	retriangulationThreshold = retriangulationTh;
-  }
-  void setDegeneracyMode(DegeneracyMode degMode) {
-    degeneracyMode = degMode;
-  }
-  void setRankTolerance(double rankTol) {
-    triangulation.rankTolerance = rankTol;
-  }
-  void setEnableEPI(bool enableEPI) {
-    triangulation.enableEPI = enableEPI;
-  }
-  void setLandmarkDistanceThreshold(double landmarkDistanceThreshold) {
-    triangulation.landmarkDistanceThreshold = landmarkDistanceThreshold;
-  }
-  void setDynamicOutlierRejectionThreshold(double dynOutRejectionThreshold) {
-    triangulation.dynamicOutlierRejectionThreshold = dynOutRejectionThreshold;
-  }
-
-private:
-
-  /// Serialization function
-  friend class boost::serialization::access;
-  template<class ARCHIVE>
-  void serialize(ARCHIVE & ar, const unsigned int version) {
-    ar & BOOST_SERIALIZATION_NVP(linearizationMode);
-    ar & BOOST_SERIALIZATION_NVP(degeneracyMode);
-    ar & BOOST_SERIALIZATION_NVP(triangulation);
-    ar & BOOST_SERIALIZATION_NVP(retriangulationThreshold);
-    ar & BOOST_SERIALIZATION_NVP(throwCheirality);
-    ar & BOOST_SERIALIZATION_NVP(verboseCheirality);
-  }
-};
-
 /**
  * SmartProjectionFactor: triangulates point and keeps an estimate of it around.
  * This factor operates with monocular cameras, where a camera is expected to

gtsam/slam/tests/testSmartProjectionPoseFactor.cpp

@@ -1011,6 +1011,7 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac
 /* *************************************************************************/
 TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) {
 
+  // this test considers a condition in which the cheirality constraint is triggered
   using namespace vanillaPose;
 
   vector<Key> views;
@@ -1083,8 +1084,14 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor )
 
   // Since we do not do anything on degenerate instances (ZERO_ON_DEGENERACY)
   // rotation remains the same as the initial guess, but position is fixed by PoseTranslationPrior
+#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
   EXPECT(assert_equal(Pose3(values.at<Pose3>(x3).rotation(),
       Point3(0,0,1)), result.at<Pose3>(x3)));
+#else
+  // if the check is disabled, no cheirality exception if thrown and the pose converges to the right rotation
+  // with modest accuracy since the configuration is essentially degenerate without the translation due to noise (noise_pose)
+  EXPECT(assert_equal(pose3, result.at<Pose3>(x3),1e-3));
+#endif
 }
 
 /* *************************************************************************/

gtsam_unstable/slam/SmartStereoProjectionFactor.h

@@ -21,6 +21,7 @@
 #pragma once
 
 #include <gtsam/slam/SmartFactorBase.h>
+#include <gtsam/slam/SmartFactorParams.h>
 
 #include <gtsam/geometry/triangulation.h>
 #include <gtsam/geometry/Pose3.h>
@@ -35,91 +36,10 @@
 
 namespace gtsam {
 
-/// Linearization mode: what factor to linearize to
+/*
- enum LinearizationMode {
+ *  Parameters for the smart stereo projection factors (identical to the SmartProjectionParams)
-   HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD
+ */
- };
+typedef SmartProjectionParams SmartStereoProjectionParams;
-
-/// How to manage degeneracy
-enum DegeneracyMode {
-   IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY
- };
-
- /*
-  *  Parameters for the smart stereo projection factors
-  */
- struct GTSAM_EXPORT SmartStereoProjectionParams {
-
-   LinearizationMode linearizationMode; ///< How to linearize the factor
-   DegeneracyMode degeneracyMode; ///< How to linearize the factor
-
-   /// @name Parameters governing the triangulation
-   /// @{
-   TriangulationParameters triangulation;
-   double retriangulationThreshold; ///< threshold to decide whether to re-triangulate
-   /// @}
-
-   /// @name Parameters governing how triangulation result is treated
-   /// @{
-   bool throwCheirality; ///< If true, re-throws Cheirality exceptions (default: false)
-   bool verboseCheirality; ///< If true, prints text for Cheirality exceptions (default: false)
-   /// @}
-
-
-   /// Constructor
-   SmartStereoProjectionParams(LinearizationMode linMode = HESSIAN,
-       DegeneracyMode degMode = IGNORE_DEGENERACY, bool throwCheirality = false,
-       bool verboseCheirality = false) :
-       linearizationMode(linMode), degeneracyMode(degMode), retriangulationThreshold(
-           1e-5), throwCheirality(throwCheirality), verboseCheirality(
-           verboseCheirality) {
-   }
-
-   virtual ~SmartStereoProjectionParams() {
-   }
-
-   void print(const std::string& str) const {
-     std::cout << "linearizationMode: " << linearizationMode << "\n";
-     std::cout << "   degeneracyMode: " << degeneracyMode << "\n";
-     std::cout << triangulation << std::endl;
-   }
-
-   LinearizationMode getLinearizationMode() const {
-     return linearizationMode;
-   }
-   DegeneracyMode getDegeneracyMode() const {
-     return degeneracyMode;
-   }
-   TriangulationParameters getTriangulationParameters() const {
-     return triangulation;
-   }
-   bool getVerboseCheirality() const {
-     return verboseCheirality;
-   }
-   bool getThrowCheirality() const {
-     return throwCheirality;
-   }
-   void setLinearizationMode(LinearizationMode linMode) {
-     linearizationMode = linMode;
-   }
-   void setDegeneracyMode(DegeneracyMode degMode) {
-     degeneracyMode = degMode;
-   }
-   void setRankTolerance(double rankTol) {
-     triangulation.rankTolerance = rankTol;
-   }
-   void setEnableEPI(bool enableEPI) {
-     triangulation.enableEPI = enableEPI;
-   }
-   void setLandmarkDistanceThreshold(double landmarkDistanceThreshold) {
-     triangulation.landmarkDistanceThreshold = landmarkDistanceThreshold;
-   }
-   void setDynamicOutlierRejectionThreshold(double dynOutRejectionThreshold) {
-     triangulation.dynamicOutlierRejectionThreshold = dynOutRejectionThreshold;
-   }
- };
-
-
 
 /**
  * SmartStereoProjectionFactor: triangulates point and keeps an estimate of it around.
@@ -155,14 +75,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,75 +165,28 @@ 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;
+    for(size_t i = 0; i < m; i++) {
-
+      const Pose3 leftPose = cameras[i].pose();
-    if (retriangulate) {
+      const Cal3_S2 monoCal = cameras[i].calibration().calibration();
-//      std::cout << "Retriangulate " << std::endl;
+      const MonoCamera leftCamera_i(leftPose,monoCal);
-      std::vector<Point3> reprojections;
+      const Pose3 left_Pose_right = Pose3(Rot3(),Point3(cameras[i].baseline(),0.0,0.0));
-      reprojections.reserve(m);
+      const Pose3 rightPose = leftPose.compose( left_Pose_right );
-      for(size_t i = 0; i < m; i++) {
+      const MonoCamera rightCamera_i(rightPose,monoCal);
-        reprojections.push_back(cameras[i].backproject(measured_[i]));
+      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);
+    if (retriangulate)
-      for(const Point3& pw: reprojections) {
+      result_ = gtsam::triangulateSafe(monoCameras, monoMeasured,
-        pw_sum = pw_sum + pw;
+          params_.triangulation);
-      }
-      // average reprojected landmark
-      Point3 pw_avg = pw_sum / double(m);
-
-      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
@@ -570,6 +448,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) {
 
@@ -79,6 +79,35 @@ vector<StereoPoint2> stereo_projectToMultipleCameras(const StereoCamera& cam1,
 
 LevenbergMarquardtParams lm_params;
 
+/* ************************************************************************* */
+TEST( SmartStereoProjectionPoseFactor, params) {
+  SmartStereoProjectionParams p;
+
+  // check default values and "get"
+  EXPECT(p.getLinearizationMode() == HESSIAN);
+  EXPECT(p.getDegeneracyMode() == IGNORE_DEGENERACY);
+  EXPECT_DOUBLES_EQUAL(p.getRetriangulationThreshold(), 1e-5, 1e-9);
+  EXPECT(p.getVerboseCheirality() == false);
+  EXPECT(p.getThrowCheirality() == false);
+
+  // check "set"
+  p.setLinearizationMode(JACOBIAN_SVD);
+  p.setDegeneracyMode(ZERO_ON_DEGENERACY);
+  p.setRankTolerance(100);
+  p.setEnableEPI(true);
+  p.setLandmarkDistanceThreshold(200);
+  p.setDynamicOutlierRejectionThreshold(3);
+  p.setRetriangulationThreshold(1e-2);
+
+  EXPECT(p.getLinearizationMode() == JACOBIAN_SVD);
+  EXPECT(p.getDegeneracyMode() == ZERO_ON_DEGENERACY);
+  EXPECT_DOUBLES_EQUAL(p.getTriangulationParameters().rankTolerance, 100, 1e-5);
+  EXPECT(p.getTriangulationParameters().enableEPI == true);
+  EXPECT_DOUBLES_EQUAL(p.getTriangulationParameters().landmarkDistanceThreshold, 200, 1e-5);
+  EXPECT_DOUBLES_EQUAL(p.getTriangulationParameters().dynamicOutlierRejectionThreshold, 3, 1e-5);
+  EXPECT_DOUBLES_EQUAL(p.getRetriangulationThreshold(), 1e-2, 1e-5);
+}
+
 /* ************************************************************************* */
 TEST( SmartStereoProjectionPoseFactor, Constructor) {
   SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor(model));
@@ -151,6 +180,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 +331,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 +354,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 +416,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 +425,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 +677,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 +900,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 +1377,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) {
 }
 
 /* *************************************************************************/
-TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) {
+TEST( SmartStereoProjectionPoseFactor, HessianWithRotationNonDegenerate ) {
 
   vector<Key> views;
   views.push_back(x1);
@@ -1072,6 +1410,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 +1423,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 +1442,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
 }
 
 /* ************************************************************************* */