Merge branch 'develop' into feature/cleanup_ImuFactor

README.md

@@ -31,6 +31,7 @@ Prerequisites:
 
 - [Boost](http://www.boost.org/users/download/) >= 1.43 (Ubuntu: `sudo apt-get install libboost-all-dev`)
 - [CMake](http://www.cmake.org/cmake/resources/software.html) >= 2.6 (Ubuntu: `sudo apt-get install cmake`)
+- A modern compiler, i.e., at least gcc 4.7.3 on Linux.
 
 Optional prerequisites - used automatically if findable by CMake:
 
@@ -46,6 +47,6 @@ See the [`INSTALL`](INSTALL) file for more detailed installation instructions.
 
 GTSAM is open source under the BSD license, see the [`LICENSE`](LICENSE) and [`LICENSE.BSD`](LICENSE.BSD) files.
 
-Please see the [`examples/`](examples) directory and the [`USAGE`](USAGE.md) file for examples on how to use GTSAM.
+Please see the [`examples/`](examples) directory and the [`USAGE`](USAGE.md) file for examples on how to use GTSAM.
-
+
 GTSAM was developed in the lab of [Frank Dellaert](http://www.cc.gatech.edu/~dellaert) at the [Georgia Institute of Technology](http://www.gatech.edu), with the help of many contributors over the years, see [THANKS](THANKS).

examples/SFMExample_SmartFactor.cpp

@@ -58,7 +58,7 @@ int main(int argc, char* argv[]) {
   for (size_t j = 0; j < points.size(); ++j) {
 
     // every landmark represent a single landmark, we use shared pointer to init the factor, and then insert measurements.
-    SmartFactor::shared_ptr smartfactor(new SmartFactor(K));
+    SmartFactor::shared_ptr smartfactor(new SmartFactor(measurementNoise, K));
 
     for (size_t i = 0; i < poses.size(); ++i) {
 
@@ -71,7 +71,7 @@ int main(int argc, char* argv[]) {
       //    2. the corresponding camera's key
       //    3. camera noise model
       //    4. camera calibration
-      smartfactor->add(measurement, i, measurementNoise);
+      smartfactor->add(measurement, i);
     }
 
     // insert the smart factor in the graph

examples/SFMExample_SmartFactorPCG.cpp

@@ -54,7 +54,7 @@ int main(int argc, char* argv[]) {
   for (size_t j = 0; j < points.size(); ++j) {
 
     // every landmark represent a single landmark, we use shared pointer to init the factor, and then insert measurements.
-    SmartFactor::shared_ptr smartfactor(new SmartFactor(K));
+    SmartFactor::shared_ptr smartfactor(new SmartFactor(measurementNoise, K));
 
     for (size_t i = 0; i < poses.size(); ++i) {
 
@@ -63,7 +63,7 @@ int main(int argc, char* argv[]) {
       Point2 measurement = camera.project(points[j]);
 
       // call add() function to add measurement into a single factor, here we need to add:
-      smartfactor->add(measurement, i, measurementNoise);
+      smartfactor->add(measurement, i);
     }
 
     // insert the smart factor in the graph

gtsam.h

@@ -916,7 +916,7 @@ class StereoCamera {
   // Standard Interface
   gtsam::Pose3 pose() const;
   double baseline() const;
-  gtsam::Cal3_S2Stereo* calibration() const;
+  gtsam::Cal3_S2Stereo calibration() const;
 
   // Manifold
   gtsam::StereoCamera retract(const Vector& d) const;
@@ -2364,15 +2364,17 @@ class SmartProjectionParams {
 template<CALIBRATION>
 virtual class SmartProjectionPoseFactor: gtsam::NonlinearFactor {
 
-  SmartProjectionPoseFactor(const CALIBRATION* K);
+  SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise,
-  SmartProjectionPoseFactor(const CALIBRATION* K,
+      const CALIBRATION* K);
+  SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise,
+      const CALIBRATION* K,
       const gtsam::Pose3& body_P_sensor);
-  SmartProjectionPoseFactor(const CALIBRATION* K,
+  SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise,
+      const CALIBRATION* K,
       const gtsam::Pose3& body_P_sensor,
       const gtsam::SmartProjectionParams& params);
 
-  void add(const gtsam::Point2& measured_i, size_t poseKey_i,
+  void add(const gtsam::Point2& measured_i, size_t poseKey_i);
-      const gtsam::noiseModel::Base* noise_i);
 
   // enabling serialization functionality
   //void serialize() const;

gtsam/geometry/CameraSet.h

@@ -161,7 +161,7 @@ public:
     for (size_t i = 0; i < m; i++) { // for each camera
 
       const MatrixZD& Fi = Fs[i];
-      const Eigen::Matrix<double, 2, N> Ei_P = //
+      const Eigen::Matrix<double, ZDim, N> Ei_P = //
           E.block(ZDim * i, 0, ZDim, N) * P;
 
       // D = (Dx2) * ZDim

gtsam/geometry/StereoCamera.h

@@ -66,8 +66,8 @@ public:
   StereoCamera(const Pose3& leftCamPose, const Cal3_S2Stereo::shared_ptr K);
 
   /// Return shared pointer to calibration
-  const Cal3_S2Stereo::shared_ptr calibration() const {
+  const Cal3_S2Stereo& calibration() const {
-    return K_;
+    return *K_;
   }
 
   /// @}

gtsam/geometry/tests/testTriangulation.cpp

@@ -18,9 +18,15 @@
 
 #include <gtsam/geometry/triangulation.h>
 #include <gtsam/geometry/SimpleCamera.h>
+#include <gtsam/geometry/StereoCamera.h>
+#include <gtsam/geometry/CameraSet.h>
 #include <gtsam/geometry/Cal3Bundler.h>
+#include <gtsam/slam/StereoFactor.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/nonlinear/ExpressionFactor.h>
 #include <CppUnitLite/TestHarness.h>
 
+
 #include <boost/assign.hpp>
 #include <boost/assign/std/vector.hpp>
 
@@ -273,6 +279,106 @@ TEST( triangulation, onePose) {
       TriangulationUnderconstrainedException);
 }
 
+//******************************************************************************
+TEST( triangulation, StereotriangulateNonlinear ) {
+
+  Cal3_S2Stereo::shared_ptr stereoK(new Cal3_S2Stereo(1733.75, 1733.75, 0, 689.645, 508.835, 0.0699612));
+
+  // two camera poses m1, m2
+  Matrix4 m1, m2;
+  m1 << 0.796888717,     0.603404026,   -0.0295271487, 46.6673779,
+      0.592783835,    -0.77156583,    0.230856632,   66.2186159,
+      0.116517574,   -0.201470143,     -0.9725393, -4.28382528,
+      0, 0, 0, 1;
+
+  m2 << -0.955959025,    -0.29288915,   -0.0189328569, 45.7169799,
+      -0.29277519,    0.947083213,    0.131587097, 65.843136,
+     -0.0206094928,   0.131334858,   -0.991123524, -4.3525033,
+     0, 0, 0, 1;
+
+  typedef CameraSet<StereoCamera> Cameras;
+  Cameras cameras;
+  cameras.push_back(StereoCamera(Pose3(m1), stereoK));
+  cameras.push_back(StereoCamera(Pose3(m2), stereoK));
+
+  vector<StereoPoint2> measurements;
+  measurements += StereoPoint2(226.936, 175.212, 424.469);
+  measurements += StereoPoint2(339.571, 285.547, 669.973);
+
+  Point3 initial = Point3(46.0536958, 66.4621179, -6.56285929);  // error: 96.5715555191
+
+  Point3 actual = triangulateNonlinear(cameras, measurements, initial);
+
+  Point3 expected(46.0484569, 66.4710686, -6.55046613); // error: 0.763510644187
+
+  EXPECT(assert_equal(expected, actual, 1e-4));
+
+
+  // regular stereo factor comparison - expect very similar result as above
+  {
+    typedef GenericStereoFactor<Pose3,Point3> StereoFactor;
+
+    Values values;
+    values.insert(Symbol('x', 1), Pose3(m1));
+    values.insert(Symbol('x', 2), Pose3(m2));
+    values.insert(Symbol('l', 1), initial);
+
+    NonlinearFactorGraph graph;
+    static SharedNoiseModel unit(noiseModel::Unit::Create(3));
+    graph.push_back(StereoFactor::shared_ptr(new StereoFactor(measurements[0], unit, Symbol('x',1), Symbol('l',1), stereoK)));
+    graph.push_back(StereoFactor::shared_ptr(new StereoFactor(measurements[1], unit, Symbol('x',2), Symbol('l',1), stereoK)));
+
+    const SharedDiagonal posePrior = noiseModel::Isotropic::Sigma(6, 1e-9);
+    graph.push_back(PriorFactor<Pose3>(Symbol('x',1), Pose3(m1), posePrior));
+    graph.push_back(PriorFactor<Pose3>(Symbol('x',2), Pose3(m2), posePrior));
+
+    LevenbergMarquardtOptimizer optimizer(graph, values);
+    Values result = optimizer.optimize();
+
+    EXPECT(assert_equal(expected, result.at<Point3>(Symbol('l',1)), 1e-4));
+  }
+
+  // use Triangulation Factor directly - expect same result as above
+  {
+    Values values;
+    values.insert(Symbol('l', 1), initial);
+
+    NonlinearFactorGraph graph;
+    static SharedNoiseModel unit(noiseModel::Unit::Create(3));
+
+    graph.push_back(TriangulationFactor<StereoCamera>(cameras[0], measurements[0], unit, Symbol('l',1)));
+    graph.push_back(TriangulationFactor<StereoCamera>(cameras[1], measurements[1], unit, Symbol('l',1)));
+
+    LevenbergMarquardtOptimizer optimizer(graph, values);
+    Values result = optimizer.optimize();
+
+    EXPECT(assert_equal(expected, result.at<Point3>(Symbol('l',1)), 1e-4));
+  }
+
+  // use ExpressionFactor - expect same result as above
+  {
+    Values values;
+    values.insert(Symbol('l', 1), initial);
+
+    NonlinearFactorGraph graph;
+    static SharedNoiseModel unit(noiseModel::Unit::Create(3));
+
+    Expression<Point3> point_(Symbol('l',1));
+    Expression<StereoCamera> camera0_(cameras[0]);
+    Expression<StereoCamera> camera1_(cameras[1]);
+    Expression<StereoPoint2> project0_(camera0_, &StereoCamera::project2, point_);
+    Expression<StereoPoint2> project1_(camera1_, &StereoCamera::project2, point_);
+
+    graph.addExpressionFactor(unit, measurements[0], project0_);
+    graph.addExpressionFactor(unit, measurements[1], project1_);
+
+    LevenbergMarquardtOptimizer optimizer(graph, values);
+    Values result = optimizer.optimize();
+
+    EXPECT(assert_equal(expected, result.at<Point3>(Symbol('l',1)), 1e-4));
+  }
+}
+
 //******************************************************************************
 int main() {
   TestResult tr;

gtsam/geometry/triangulation.h

@@ -68,7 +68,7 @@ GTSAM_EXPORT Point3 triangulateDLT(
 /**
  * Create a factor graph with projection factors from poses and one calibration
  * @param poses Camera poses
- * @param sharedCal shared pointer to single calibration object
+ * @param sharedCal shared pointer to single calibration object (monocular only!)
  * @param measurements 2D measurements
  * @param landmarkKey to refer to landmark
  * @param initialEstimate
@@ -97,7 +97,7 @@ std::pair<NonlinearFactorGraph, Values> triangulationGraph(
 /**
  * Create a factor graph with projection factors from pinhole cameras
  * (each camera has a pose and calibration)
- * @param cameras pinhole cameras
+ * @param cameras pinhole cameras (monocular or stereo)
  * @param measurements 2D measurements
  * @param landmarkKey to refer to landmark
  * @param initialEstimate
@@ -106,22 +106,21 @@ std::pair<NonlinearFactorGraph, Values> triangulationGraph(
 template<class CAMERA>
 std::pair<NonlinearFactorGraph, Values> triangulationGraph(
     const std::vector<CAMERA>& cameras,
-    const std::vector<Point2>& measurements, Key landmarkKey,
+    const std::vector<typename CAMERA::Measurement>& measurements, Key landmarkKey,
     const Point3& initialEstimate) {
   Values values;
   values.insert(landmarkKey, initialEstimate); // Initial landmark value
   NonlinearFactorGraph graph;
-  static SharedNoiseModel unit2(noiseModel::Unit::Create(2));
+  static SharedNoiseModel unit(noiseModel::Unit::Create(CAMERA::Measurement::dimension));
-  static SharedNoiseModel prior_model(noiseModel::Isotropic::Sigma(6, 1e-6));
   for (size_t i = 0; i < measurements.size(); i++) {
     const CAMERA& camera_i = cameras[i];
     graph.push_back(TriangulationFactor<CAMERA> //
-        (camera_i, measurements[i], unit2, landmarkKey));
+        (camera_i, measurements[i], unit, landmarkKey));
   }
   return std::make_pair(graph, values);
 }
 
-/// PinholeCamera specific version
+/// PinholeCamera specific version // TODO: (chris) why does this exist?
 template<class CALIBRATION>
 std::pair<NonlinearFactorGraph, Values> triangulationGraph(
     const std::vector<PinholeCamera<CALIBRATION> >& cameras,
@@ -165,7 +164,7 @@ Point3 triangulateNonlinear(const std::vector<Pose3>& poses,
 
 /**
  * Given an initial estimate , refine a point using measurements in several cameras
- * @param cameras pinhole cameras
+ * @param cameras pinhole cameras (monocular or stereo)
  * @param measurements 2D measurements
  * @param initialEstimate
  * @return refined Point3
@@ -173,7 +172,7 @@ Point3 triangulateNonlinear(const std::vector<Pose3>& poses,
 template<class CAMERA>
 Point3 triangulateNonlinear(
     const std::vector<CAMERA>& cameras,
-    const std::vector<Point2>& measurements, const Point3& initialEstimate) {
+    const std::vector<typename CAMERA::Measurement>& measurements, const Point3& initialEstimate) {
 
   // Create a factor graph and initial values
   Values values;
@@ -184,7 +183,7 @@ Point3 triangulateNonlinear(
   return optimize(graph, values, Symbol('p', 0));
 }
 
-/// PinholeCamera specific version
+/// PinholeCamera specific version  // TODO: (chris) why does this exist?
 template<class CALIBRATION>
 Point3 triangulateNonlinear(
     const std::vector<PinholeCamera<CALIBRATION> >& cameras,

gtsam/slam/SmartFactorBase.h

@@ -15,6 +15,7 @@
  * @author Luca Carlone
  * @author Zsolt Kira
  * @author Frank Dellaert
+ * @author Chris Beall
  */
 
 #pragma once
@@ -79,22 +80,6 @@ protected:
   typedef Eigen::Matrix<double, Dim, 1> VectorD;
   typedef Eigen::Matrix<double, ZDim, ZDim> Matrix2;
 
-  // check that noise model is isotropic and the same
-  // TODO, this is hacky, we should just do this via constructor, not add
-  void maybeSetNoiseModel(const SharedNoiseModel& sharedNoiseModel) {
-    if (!sharedNoiseModel)
-      return;
-    SharedIsotropic sharedIsotropic = boost::dynamic_pointer_cast<
-        noiseModel::Isotropic>(sharedNoiseModel);
-    if (!sharedIsotropic)
-      throw std::runtime_error("SmartFactorBase: needs isotropic");
-    if (!noiseModel_)
-      noiseModel_ = sharedIsotropic;
-    else if (!sharedIsotropic->equals(*noiseModel_))
-      throw std::runtime_error(
-          "SmartFactorBase: cannot add measurements with different noise model");
-  }
-
 public:
 
   // Definitions for blocks of F, externally visible
@@ -109,8 +94,21 @@ public:
   typedef CameraSet<CAMERA> Cameras;
 
   /// Constructor
-  SmartFactorBase(boost::optional<Pose3> body_P_sensor = boost::none) :
+  SmartFactorBase(const SharedNoiseModel& sharedNoiseModel,
-          body_P_sensor_(body_P_sensor){}
+      boost::optional<Pose3> body_P_sensor = boost::none) :
+          body_P_sensor_(body_P_sensor){
+
+    if (!sharedNoiseModel)
+      throw std::runtime_error("SmartFactorBase: sharedNoiseModel is required");
+
+    SharedIsotropic sharedIsotropic = boost::dynamic_pointer_cast<
+        noiseModel::Isotropic>(sharedNoiseModel);
+
+    if (!sharedIsotropic)
+      throw std::runtime_error("SmartFactorBase: needs isotropic");
+
+    noiseModel_ = sharedIsotropic;
+  }
 
   /// Virtual destructor, subclasses from NonlinearFactor
   virtual ~SmartFactorBase() {
@@ -122,34 +120,18 @@ public:
    * @param poseKey is the index corresponding to the camera observing the landmark
    * @param sharedNoiseModel is the measurement noise
    */
-  void add(const Z& measured_i, const Key& cameraKey_i,
+  void add(const Z& measured_i, const Key& cameraKey_i) {
-      const SharedNoiseModel& sharedNoiseModel) {
     this->measured_.push_back(measured_i);
     this->keys_.push_back(cameraKey_i);
-    maybeSetNoiseModel(sharedNoiseModel);
   }
 
   /**
-   * Add a bunch of measurements, together with the camera keys and noises
+   * Add a bunch of measurements, together with the camera keys
    */
-  void add(std::vector<Z>& measurements, std::vector<Key>& cameraKeys,
+  void add(std::vector<Z>& measurements, std::vector<Key>& cameraKeys) {
-      std::vector<SharedNoiseModel>& noises) {
     for (size_t i = 0; i < measurements.size(); i++) {
       this->measured_.push_back(measurements.at(i));
       this->keys_.push_back(cameraKeys.at(i));
-      maybeSetNoiseModel(noises.at(i));
-    }
-  }
-
-  /**
-   * Add a bunch of measurements and use the same noise model for all of them
-   */
-  void add(std::vector<Z>& measurements, std::vector<Key>& cameraKeys,
-      const SharedNoiseModel& noise) {
-    for (size_t i = 0; i < measurements.size(); i++) {
-      this->measured_.push_back(measurements.at(i));
-      this->keys_.push_back(cameraKeys.at(i));
-      maybeSetNoiseModel(noise);
     }
   }
 
@@ -158,11 +140,10 @@ public:
    * The noise is assumed to be the same for all measurements
    */
   template<class SFM_TRACK>
-  void add(const SFM_TRACK& trackToAdd, const SharedNoiseModel& noise) {
+  void add(const SFM_TRACK& trackToAdd) {
     for (size_t k = 0; k < trackToAdd.number_measurements(); k++) {
       this->measured_.push_back(trackToAdd.measurements[k].second);
       this->keys_.push_back(trackToAdd.measurements[k].first);
-      maybeSetNoiseModel(noise);
     }
   }
 
@@ -198,7 +179,7 @@ public:
     }
     if(body_P_sensor_)
       body_P_sensor_->print("body_P_sensor_:\n");
-    print("", keyFormatter);
+    Base::print("", keyFormatter);
   }
 
   /// equals

gtsam/slam/SmartProjectionFactor.h

@@ -44,23 +44,21 @@ enum DegeneracyMode {
 /*
  *  Parameters for the smart projection factors
  */
-class GTSAM_EXPORT SmartProjectionParams {
+struct GTSAM_EXPORT SmartProjectionParams {
-
-public:
 
   LinearizationMode linearizationMode; ///< How to linearize the factor
   DegeneracyMode degeneracyMode; ///< How to linearize the factor
 
   /// @name Parameters governing the triangulation
   /// @{
-  mutable TriangulationParameters triangulation;
+  TriangulationParameters triangulation;
-  const double retriangulationThreshold; ///< threshold to decide whether to re-triangulate
+  double retriangulationThreshold; ///< threshold to decide whether to re-triangulate
   /// @}
 
   /// @name Parameters governing how triangulation result is treated
   /// @{
-  const bool throwCheirality; ///< If true, re-throws Cheirality exceptions (default: false)
+  bool throwCheirality; ///< If true, re-throws Cheirality exceptions (default: false)
-  const bool verboseCheirality; ///< If true, prints text for Cheirality exceptions (default: false)
+  bool verboseCheirality; ///< If true, prints text for Cheirality exceptions (default: false)
   /// @}
 
   // Constructor
@@ -161,10 +159,10 @@ public:
    * @param body_P_sensor pose of the camera in the body frame
    * @param params internal parameters of the smart factors
    */
-  SmartProjectionFactor(
+  SmartProjectionFactor(const SharedNoiseModel& sharedNoiseModel,
       const boost::optional<Pose3> body_P_sensor = boost::none,
       const SmartProjectionParams& params = SmartProjectionParams()) :
-      Base(body_P_sensor), params_(params), //
+      Base(sharedNoiseModel, body_P_sensor), params_(params), //
       result_(TriangulationResult::Degenerate()) {
   }
 

gtsam/slam/SmartProjectionPoseFactor.h

@@ -61,14 +61,16 @@ public:
 
   /**
    * Constructor
+   * @param Isotropic measurement noise
    * @param K (fixed) calibration, assumed to be the same for all cameras
    * @param body_P_sensor pose of the camera in the body frame
    * @param params internal parameters of the smart factors
    */
-  SmartProjectionPoseFactor(const boost::shared_ptr<CALIBRATION> K,
+  SmartProjectionPoseFactor(const SharedNoiseModel& sharedNoiseModel,
+      const boost::shared_ptr<CALIBRATION> K,
       const boost::optional<Pose3> body_P_sensor = boost::none,
       const SmartProjectionParams& params = SmartProjectionParams()) :
-      Base(body_P_sensor, params), K_(K) {
+      Base(sharedNoiseModel, body_P_sensor, params), K_(K) {
   }
 
   /** Virtual destructor */

gtsam/slam/TriangulationFactor.h

@@ -18,6 +18,7 @@
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/geometry/CalibratedCamera.h>
 #include <boost/make_shared.hpp>
+#include <boost/lexical_cast.hpp>
 
 namespace gtsam {
 
@@ -42,9 +43,12 @@ protected:
   /// shorthand for this class
   typedef TriangulationFactor<CAMERA> This;
 
+  /// shorthand for measurement type, e.g. Point2 or StereoPoint2
+  typedef typename CAMERA::Measurement Measurement;
+
   // Keep a copy of measurement and calibration for I/O
   const CAMERA camera_; ///< CAMERA in which this landmark was seen
-  const Point2 measured_; ///< 2D measurement
+  const Measurement measured_; ///< 2D measurement
 
   // verbosity handling for Cheirality Exceptions
   const bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false)
@@ -69,14 +73,17 @@ public:
    * @param throwCheirality determines whether Cheirality exceptions are rethrown
    * @param verboseCheirality determines whether exceptions are printed for Cheirality
    */
-  TriangulationFactor(const CAMERA& camera, const Point2& measured,
+  TriangulationFactor(const CAMERA& camera, const Measurement& measured,
       const SharedNoiseModel& model, Key pointKey, bool throwCheirality = false,
       bool verboseCheirality = false) :
       Base(model, pointKey), camera_(camera), measured_(measured), throwCheirality_(
           throwCheirality), verboseCheirality_(verboseCheirality) {
-    if (model && model->dim() != 2)
+    if (model && model->dim() != Measurement::dimension)
       throw std::invalid_argument(
-          "TriangulationFactor must be created with 2-dimensional noise model.");
+          "TriangulationFactor must be created with "
+              + boost::lexical_cast<std::string>((int) Measurement::dimension)
+              + "-dimensional noise model.");
+
   }
 
   /** Virtual destructor */
@@ -113,18 +120,18 @@ public:
   Vector evaluateError(const Point3& point, boost::optional<Matrix&> H2 =
       boost::none) const {
     try {
-      Point2 error(camera_.project2(point, boost::none, H2) - measured_);
+      Measurement error(camera_.project2(point, boost::none, H2) - measured_);
       return error.vector();
     } catch (CheiralityException& e) {
       if (H2)
-        *H2 = zeros(2, 3);
+        *H2 = zeros(Measurement::dimension, 3);
       if (verboseCheirality_)
         std::cout << e.what() << ": Landmark "
             << DefaultKeyFormatter(this->key()) << " moved behind camera"
             << std::endl;
       if (throwCheirality_)
         throw e;
-      return ones(2) * 2.0 * camera_.calibration().fx();
+      return ones(Measurement::dimension) * 2.0 * camera_.calibration().fx();
     }
   }
 
@@ -146,9 +153,9 @@ public:
     // Allocate memory for Jacobian factor, do only once
     if (Ab.rows() == 0) {
       std::vector<size_t> dimensions(1, 3);
-      Ab = VerticalBlockMatrix(dimensions, 2, true);
+      Ab = VerticalBlockMatrix(dimensions, Measurement::dimension, true);
-      A.resize(2,3);
+      A.resize(Measurement::dimension,3);
-      b.resize(2);
+      b.resize(Measurement::dimension);
     }
 
     // Would be even better if we could pass blocks to project
@@ -164,7 +171,7 @@ public:
   }
 
   /** return the measurement */
-  const Point2& measured() const {
+  const Measurement& measured() const {
     return measured_;
   }
 

gtsam/slam/tests/testSmartFactorBase.cpp

@@ -16,17 +16,24 @@
  *  @date   Feb 2015
  */
 
-#include "../SmartFactorBase.h"
+#include <gtsam/slam/SmartFactorBase.h>
 #include <gtsam/geometry/Pose3.h>
 #include <CppUnitLite/TestHarness.h>
 
 using namespace std;
 using namespace gtsam;
 
+static SharedNoiseModel unit2(noiseModel::Unit::Create(2));
+static SharedNoiseModel unit3(noiseModel::Unit::Create(3));
+
 /* ************************************************************************* */
 #include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/geometry/Cal3Bundler.h>
 class PinholeFactor: public SmartFactorBase<PinholeCamera<Cal3Bundler> > {
+public:
+  typedef SmartFactorBase<PinholeCamera<Cal3Bundler> > Base;
+  PinholeFactor(const SharedNoiseModel& sharedNoiseModel): Base(sharedNoiseModel) {
+  }
   virtual double error(const Values& values) const {
     return 0.0;
   }
@@ -37,15 +44,19 @@ class PinholeFactor: public SmartFactorBase<PinholeCamera<Cal3Bundler> > {
 };
 
 TEST(SmartFactorBase, Pinhole) {
-  PinholeFactor f;
+  PinholeFactor f= PinholeFactor(unit2);
-  f.add(Point2(), 1, SharedNoiseModel());
+  f.add(Point2(), 1);
-  f.add(Point2(), 2, SharedNoiseModel());
+  f.add(Point2(), 2);
   EXPECT_LONGS_EQUAL(2 * 2, f.dim());
 }
 
 /* ************************************************************************* */
 #include <gtsam/geometry/StereoCamera.h>
 class StereoFactor: public SmartFactorBase<StereoCamera> {
+public:
+  typedef SmartFactorBase<StereoCamera> Base;
+  StereoFactor(const SharedNoiseModel& sharedNoiseModel): Base(sharedNoiseModel) {
+  }
   virtual double error(const Values& values) const {
     return 0.0;
   }
@@ -56,9 +67,9 @@ class StereoFactor: public SmartFactorBase<StereoCamera> {
 };
 
 TEST(SmartFactorBase, Stereo) {
-  StereoFactor f;
+  StereoFactor f(unit3);
-  f.add(StereoPoint2(), 1, SharedNoiseModel());
+  f.add(StereoPoint2(), 1);
-  f.add(StereoPoint2(), 2, SharedNoiseModel());
+  f.add(StereoPoint2(), 2);
   EXPECT_LONGS_EQUAL(2 * 3, f.dim());
 }
 

gtsam/slam/tests/testSmartProjectionCameraFactor.cpp

@@ -72,39 +72,39 @@ TEST( SmartProjectionCameraFactor, perturbCameraPose) {
 /* ************************************************************************* */
 TEST( SmartProjectionCameraFactor, Constructor) {
   using namespace vanilla;
-  SmartFactor::shared_ptr factor1(new SmartFactor());
+  SmartFactor::shared_ptr factor1(new SmartFactor(unit2));
 }
 
 /* ************************************************************************* */
 TEST( SmartProjectionCameraFactor, Constructor2) {
   using namespace vanilla;
   params.setRankTolerance(rankTol);
-  SmartFactor factor1(boost::none, params);
+  SmartFactor factor1(unit2, boost::none, params);
 }
 
 /* ************************************************************************* */
 TEST( SmartProjectionCameraFactor, Constructor3) {
   using namespace vanilla;
-  SmartFactor::shared_ptr factor1(new SmartFactor());
+  SmartFactor::shared_ptr factor1(new SmartFactor(unit2));
-  factor1->add(measurement1, x1, unit2);
+  factor1->add(measurement1, x1);
 }
 
 /* ************************************************************************* */
 TEST( SmartProjectionCameraFactor, Constructor4) {
   using namespace vanilla;
   params.setRankTolerance(rankTol);
-  SmartFactor factor1(boost::none, params);
+  SmartFactor factor1(unit2, boost::none, params);
-  factor1.add(measurement1, x1, unit2);
+  factor1.add(measurement1, x1);
 }
 
 /* ************************************************************************* */
 TEST( SmartProjectionCameraFactor, Equals ) {
   using namespace vanilla;
-  SmartFactor::shared_ptr factor1(new SmartFactor());
+  SmartFactor::shared_ptr factor1(new SmartFactor(unit2));
-  factor1->add(measurement1, x1, unit2);
+  factor1->add(measurement1, x1);
 
-  SmartFactor::shared_ptr factor2(new SmartFactor());
+  SmartFactor::shared_ptr factor2(new SmartFactor(unit2));
-  factor2->add(measurement1, x1, unit2);
+  factor2->add(measurement1, x1);
 }
 
 /* *************************************************************************/
@@ -115,9 +115,9 @@ TEST( SmartProjectionCameraFactor, noiseless ) {
   values.insert(c1, level_camera);
   values.insert(c2, level_camera_right);
 
-  SmartFactor::shared_ptr factor1(new SmartFactor());
+  SmartFactor::shared_ptr factor1(new SmartFactor(unit2));
-  factor1->add(level_uv, c1, unit2);
+  factor1->add(level_uv, c1);
-  factor1->add(level_uv_right, c2, unit2);
+  factor1->add(level_uv_right, c2);
 
   double expectedError = 0.0;
   DOUBLES_EQUAL(expectedError, factor1->error(values), 1e-7);
@@ -140,9 +140,9 @@ TEST( SmartProjectionCameraFactor, noisy ) {
   Camera perturbed_level_camera_right = perturbCameraPose(level_camera_right);
   values.insert(c2, perturbed_level_camera_right);
 
-  SmartFactor::shared_ptr factor1(new SmartFactor());
+  SmartFactor::shared_ptr factor1(new SmartFactor(unit2));
-  factor1->add(level_uv, c1, unit2);
+  factor1->add(level_uv, c1);
-  factor1->add(level_uv_right, c2, unit2);
+  factor1->add(level_uv_right, c2);
 
   // Point is now uninitialized before a triangulation event
   EXPECT(!factor1->point());
@@ -164,20 +164,16 @@ TEST( SmartProjectionCameraFactor, noisy ) {
   Vector actual = factor1->whitenedError(cameras1, point1);
   EXPECT(assert_equal(expected, actual, 1));
 
-  SmartFactor::shared_ptr factor2(new SmartFactor());
+  SmartFactor::shared_ptr factor2(new SmartFactor(unit2));
   vector<Point2> measurements;
   measurements.push_back(level_uv);
   measurements.push_back(level_uv_right);
 
-  vector<SharedNoiseModel> noises;
-  noises.push_back(unit2);
-  noises.push_back(unit2);
-
   vector<Key> views;
   views.push_back(c1);
   views.push_back(c2);
 
-  factor2->add(measurements, views, noises);
+  factor2->add(measurements, views);
 
   double actualError2 = factor2->error(values);
   EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1);
@@ -195,16 +191,16 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimize ) {
   projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
 
   // Create and fill smartfactors
-  SmartFactor::shared_ptr smartFactor1(new SmartFactor());
+  SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2));
-  SmartFactor::shared_ptr smartFactor2(new SmartFactor());
+  SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2));
-  SmartFactor::shared_ptr smartFactor3(new SmartFactor());
+  SmartFactor::shared_ptr smartFactor3(new SmartFactor(unit2));
   vector<Key> views;
   views.push_back(c1);
   views.push_back(c2);
   views.push_back(c3);
-  smartFactor1->add(measurements_cam1, views, unit2);
+  smartFactor1->add(measurements_cam1, views);
-  smartFactor2->add(measurements_cam2, views, unit2);
+  smartFactor2->add(measurements_cam2, views);
-  smartFactor3->add(measurements_cam3, views, unit2);
+  smartFactor3->add(measurements_cam3, views);
 
   // Create factor graph and add priors on two cameras
   NonlinearFactorGraph graph;
@@ -308,8 +304,8 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimizeFromSfM_tracks ) {
     measures.second = measurements_cam1.at(i);
     track1.measurements.push_back(measures);
   }
-  SmartFactor::shared_ptr smartFactor1(new SmartFactor());
+  SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2));
-  smartFactor1->add(track1, unit2);
+  smartFactor1->add(track1);
 
   SfM_Track track2;
   for (size_t i = 0; i < 3; ++i) {
@@ -318,11 +314,11 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimizeFromSfM_tracks ) {
     measures.second = measurements_cam2.at(i);
     track2.measurements.push_back(measures);
   }
-  SmartFactor::shared_ptr smartFactor2(new SmartFactor());
+  SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2));
-  smartFactor2->add(track2, unit2);
+  smartFactor2->add(track2);
 
-  SmartFactor::shared_ptr smartFactor3(new SmartFactor());
+  SmartFactor::shared_ptr smartFactor3(new SmartFactor(unit2));
-  smartFactor3->add(measurements_cam3, views, unit2);
+  smartFactor3->add(measurements_cam3, views);
 
   const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6 + 5, 1e-5);
 
@@ -384,20 +380,20 @@ TEST( SmartProjectionCameraFactor, perturbCamerasAndOptimize ) {
   views.push_back(c2);
   views.push_back(c3);
 
-  SmartFactor::shared_ptr smartFactor1(new SmartFactor());
+  SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2));
-  smartFactor1->add(measurements_cam1, views, unit2);
+  smartFactor1->add(measurements_cam1, views);
 
-  SmartFactor::shared_ptr smartFactor2(new SmartFactor());
+  SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2));
-  smartFactor2->add(measurements_cam2, views, unit2);
+  smartFactor2->add(measurements_cam2, views);
 
-  SmartFactor::shared_ptr smartFactor3(new SmartFactor());
+  SmartFactor::shared_ptr smartFactor3(new SmartFactor(unit2));
-  smartFactor3->add(measurements_cam3, views, unit2);
+  smartFactor3->add(measurements_cam3, views);
 
-  SmartFactor::shared_ptr smartFactor4(new SmartFactor());
+  SmartFactor::shared_ptr smartFactor4(new SmartFactor(unit2));
-  smartFactor4->add(measurements_cam4, views, unit2);
+  smartFactor4->add(measurements_cam4, views);
 
-  SmartFactor::shared_ptr smartFactor5(new SmartFactor());
+  SmartFactor::shared_ptr smartFactor5(new SmartFactor(unit2));
-  smartFactor5->add(measurements_cam5, views, unit2);
+  smartFactor5->add(measurements_cam5, views);
 
   const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6 + 5, 1e-5);
 
@@ -464,20 +460,20 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler ) {
   views.push_back(c2);
   views.push_back(c3);
 
-  SmartFactor::shared_ptr smartFactor1(new SmartFactor());
+  SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2));
-  smartFactor1->add(measurements_cam1, views, unit2);
+  smartFactor1->add(measurements_cam1, views);
 
-  SmartFactor::shared_ptr smartFactor2(new SmartFactor());
+  SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2));
-  smartFactor2->add(measurements_cam2, views, unit2);
+  smartFactor2->add(measurements_cam2, views);
 
-  SmartFactor::shared_ptr smartFactor3(new SmartFactor());
+  SmartFactor::shared_ptr smartFactor3(new SmartFactor(unit2));
-  smartFactor3->add(measurements_cam3, views, unit2);
+  smartFactor3->add(measurements_cam3, views);
 
-  SmartFactor::shared_ptr smartFactor4(new SmartFactor());
+  SmartFactor::shared_ptr smartFactor4(new SmartFactor(unit2));
-  smartFactor4->add(measurements_cam4, views, unit2);
+  smartFactor4->add(measurements_cam4, views);
 
-  SmartFactor::shared_ptr smartFactor5(new SmartFactor());
+  SmartFactor::shared_ptr smartFactor5(new SmartFactor(unit2));
-  smartFactor5->add(measurements_cam5, views, unit2);
+  smartFactor5->add(measurements_cam5, views);
 
   const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(9, 1e-6);
 
@@ -540,20 +536,20 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler2 ) {
   views.push_back(c2);
   views.push_back(c3);
 
-  SmartFactor::shared_ptr smartFactor1(new SmartFactor());
+  SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2));
-  smartFactor1->add(measurements_cam1, views, unit2);
+  smartFactor1->add(measurements_cam1, views);
 
-  SmartFactor::shared_ptr smartFactor2(new SmartFactor());
+  SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2));
-  smartFactor2->add(measurements_cam2, views, unit2);
+  smartFactor2->add(measurements_cam2, views);
 
-  SmartFactor::shared_ptr smartFactor3(new SmartFactor());
+  SmartFactor::shared_ptr smartFactor3(new SmartFactor(unit2));
-  smartFactor3->add(measurements_cam3, views, unit2);
+  smartFactor3->add(measurements_cam3, views);
 
-  SmartFactor::shared_ptr smartFactor4(new SmartFactor());
+  SmartFactor::shared_ptr smartFactor4(new SmartFactor(unit2));
-  smartFactor4->add(measurements_cam4, views, unit2);
+  smartFactor4->add(measurements_cam4, views);
 
-  SmartFactor::shared_ptr smartFactor5(new SmartFactor());
+  SmartFactor::shared_ptr smartFactor5(new SmartFactor(unit2));
-  smartFactor5->add(measurements_cam5, views, unit2);
+  smartFactor5->add(measurements_cam5, views);
 
   const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(9, 1e-6);
 
@@ -604,9 +600,9 @@ TEST( SmartProjectionCameraFactor, noiselessBundler ) {
   values.insert(c1, level_camera);
   values.insert(c2, level_camera_right);
 
-  SmartFactor::shared_ptr factor1(new SmartFactor());
+  SmartFactor::shared_ptr factor1(new SmartFactor(unit2));
-  factor1->add(level_uv, c1, unit2);
+  factor1->add(level_uv, c1);
-  factor1->add(level_uv_right, c2, unit2);
+  factor1->add(level_uv_right, c2);
 
   double actualError = factor1->error(values);
 
@@ -633,9 +629,9 @@ TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor ) {
   values.insert(c2, level_camera_right);
 
   NonlinearFactorGraph smartGraph;
-  SmartFactor::shared_ptr factor1(new SmartFactor());
+  SmartFactor::shared_ptr factor1(new SmartFactor(unit2));
-  factor1->add(level_uv, c1, unit2);
+  factor1->add(level_uv, c1);
-  factor1->add(level_uv_right, c2, unit2);
+  factor1->add(level_uv_right, c2);
   smartGraph.push_back(factor1);
   double expectedError = factor1->error(values);
   double expectedErrorGraph = smartGraph.error(values);
@@ -674,9 +670,9 @@ TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor1 ) {
   values.insert(c2, level_camera_right);
 
   NonlinearFactorGraph smartGraph;
-  SmartFactor::shared_ptr factor1(new SmartFactor());
+  SmartFactor::shared_ptr factor1(new SmartFactor(unit2));
-  factor1->add(level_uv, c1, unit2);
+  factor1->add(level_uv, c1);
-  factor1->add(level_uv_right, c2, unit2);
+  factor1->add(level_uv_right, c2);
   smartGraph.push_back(factor1);
   Matrix expectedHessian = smartGraph.linearize(values)->hessian().first;
   Vector expectedInfoVector = smartGraph.linearize(values)->hessian().second;
@@ -765,9 +761,9 @@ TEST( SmartProjectionCameraFactor, computeImplicitJacobian ) {
   values.insert(c1, level_camera);
   values.insert(c2, level_camera_right);
 
-  SmartFactor::shared_ptr factor1(new SmartFactor());
+  SmartFactor::shared_ptr factor1(new SmartFactor(unit2));
-  factor1->add(level_uv, c1, unit2);
+  factor1->add(level_uv, c1);
-  factor1->add(level_uv_right, c2, unit2);
+  factor1->add(level_uv_right, c2);
   Matrix expectedE;
   Vector expectedb;
 
@@ -814,9 +810,9 @@ TEST( SmartProjectionCameraFactor, implicitJacobianFactor ) {
   params.setEnableEPI(useEPI);
 
   SmartFactor::shared_ptr explicitFactor(
-      new SmartFactor(boost::none, params));
+      new SmartFactor(unit2, boost::none, params));
-  explicitFactor->add(level_uv, c1, unit2);
+  explicitFactor->add(level_uv, c1);
-  explicitFactor->add(level_uv_right, c2, unit2);
+  explicitFactor->add(level_uv_right, c2);
 
   GaussianFactor::shared_ptr gaussianHessianFactor = explicitFactor->linearize(
       values);
@@ -826,9 +822,9 @@ TEST( SmartProjectionCameraFactor, implicitJacobianFactor ) {
   // Implicit Schur version
   params.setLinearizationMode(gtsam::IMPLICIT_SCHUR);
   SmartFactor::shared_ptr implicitFactor(
-      new SmartFactor(boost::none, params));
+      new SmartFactor(unit2, boost::none, params));
-  implicitFactor->add(level_uv, c1, unit2);
+  implicitFactor->add(level_uv, c1);
-  implicitFactor->add(level_uv_right, c2, unit2);
+  implicitFactor->add(level_uv_right, c2);
   GaussianFactor::shared_ptr gaussianImplicitSchurFactor =
       implicitFactor->linearize(values);
   CHECK(gaussianImplicitSchurFactor);

gtsam/slam/tests/testSmartProjectionPoseFactor.cpp

@@ -53,7 +53,7 @@ LevenbergMarquardtParams lmParams;
 /* ************************************************************************* */
 TEST( SmartProjectionPoseFactor, Constructor) {
   using namespace vanillaPose;
-  SmartFactor::shared_ptr factor1(new SmartFactor(sharedK));
+  SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK));
 }
 
 /* ************************************************************************* */
@@ -61,14 +61,14 @@ TEST( SmartProjectionPoseFactor, Constructor2) {
   using namespace vanillaPose;
   SmartProjectionParams params;
   params.setRankTolerance(rankTol);
-  SmartFactor factor1(sharedK, boost::none, params);
+  SmartFactor factor1(model, sharedK, boost::none, params);
 }
 
 /* ************************************************************************* */
 TEST( SmartProjectionPoseFactor, Constructor3) {
   using namespace vanillaPose;
-  SmartFactor::shared_ptr factor1(new SmartFactor(sharedK));
+  SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK));
-  factor1->add(measurement1, x1, model);
+  factor1->add(measurement1, x1);
 }
 
 /* ************************************************************************* */
@@ -76,18 +76,18 @@ TEST( SmartProjectionPoseFactor, Constructor4) {
   using namespace vanillaPose;
   SmartProjectionParams params;
   params.setRankTolerance(rankTol);
-  SmartFactor factor1(sharedK, boost::none, params);
+  SmartFactor factor1(model, sharedK, boost::none, params);
-  factor1.add(measurement1, x1, model);
+  factor1.add(measurement1, x1);
 }
 
 /* ************************************************************************* */
 TEST( SmartProjectionPoseFactor, Equals ) {
   using namespace vanillaPose;
-  SmartFactor::shared_ptr factor1(new SmartFactor(sharedK));
+  SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK));
-  factor1->add(measurement1, x1, model);
+  factor1->add(measurement1, x1);
 
-  SmartFactor::shared_ptr factor2(new SmartFactor(sharedK));
+  SmartFactor::shared_ptr factor2(new SmartFactor(model,sharedK));
-  factor2->add(measurement1, x1, model);
+  factor2->add(measurement1, x1);
 
   CHECK(assert_equal(*factor1, *factor2));
 }
@@ -101,9 +101,9 @@ TEST( SmartProjectionPoseFactor, noiseless ) {
   Point2 level_uv = level_camera.project(landmark1);
   Point2 level_uv_right = level_camera_right.project(landmark1);
 
-  SmartFactor factor(sharedK);
+  SmartFactor factor(model, sharedK);
-  factor.add(level_uv, x1, model);
+  factor.add(level_uv, x1);
-  factor.add(level_uv_right, x2, model);
+  factor.add(level_uv_right, x2);
 
   Values values; // it's a pose factor, hence these are poses
   values.insert(x1, cam1.pose());
@@ -166,26 +166,22 @@ TEST( SmartProjectionPoseFactor, noisy ) {
       Point3(0.5, 0.1, 0.3));
   values.insert(x2, pose_right.compose(noise_pose));
 
-  SmartFactor::shared_ptr factor(new SmartFactor((sharedK)));
+  SmartFactor::shared_ptr factor(new SmartFactor(model, sharedK));
-  factor->add(level_uv, x1, model);
+  factor->add(level_uv, x1);
-  factor->add(level_uv_right, x2, model);
+  factor->add(level_uv_right, x2);
 
   double actualError1 = factor->error(values);
 
-  SmartFactor::shared_ptr factor2(new SmartFactor((sharedK)));
+  SmartFactor::shared_ptr factor2(new SmartFactor(model, sharedK));
   vector<Point2> measurements;
   measurements.push_back(level_uv);
   measurements.push_back(level_uv_right);
 
-  vector<SharedNoiseModel> noises;
-  noises.push_back(model);
-  noises.push_back(model);
-
   vector<Key> views;
   views.push_back(x1);
   views.push_back(x2);
 
-  factor2->add(measurements, views, noises);
+  factor2->add(measurements, views);
   double actualError2 = factor2->error(values);
   DOUBLES_EQUAL(actualError1, actualError2, 1e-7);
 }
@@ -238,14 +234,14 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){
   params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY);
   params.setEnableEPI(false);
 
-  SmartProjectionPoseFactor<Cal3_S2> smartFactor1(K, sensor_to_body, params);
+  SmartProjectionPoseFactor<Cal3_S2> smartFactor1(model, K, sensor_to_body, params);
-  smartFactor1.add(measurements_cam1, views, model);
+  smartFactor1.add(measurements_cam1, views);
 
-  SmartProjectionPoseFactor<Cal3_S2> smartFactor2(K, sensor_to_body, params);
+  SmartProjectionPoseFactor<Cal3_S2> smartFactor2(model, K, sensor_to_body, params);
-  smartFactor2.add(measurements_cam2, views, model);
+  smartFactor2.add(measurements_cam2, views);
 
-  SmartProjectionPoseFactor<Cal3_S2> smartFactor3(K, sensor_to_body, params);
+  SmartProjectionPoseFactor<Cal3_S2> smartFactor3(model, K, sensor_to_body, params);
-  smartFactor3.add(measurements_cam3, views, model);
+  smartFactor3.add(measurements_cam3, views);
 
   const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
 
@@ -296,14 +292,14 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) {
   views.push_back(x2);
   views.push_back(x3);
 
-  SmartFactor::shared_ptr smartFactor1(new SmartFactor(sharedK2));
+  SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK2));
-  smartFactor1->add(measurements_cam1, views, model);
+  smartFactor1->add(measurements_cam1, views);
 
-  SmartFactor::shared_ptr smartFactor2(new SmartFactor(sharedK2));
+  SmartFactor::shared_ptr smartFactor2(new SmartFactor(model, sharedK2));
-  smartFactor2->add(measurements_cam2, views, model);
+  smartFactor2->add(measurements_cam2, views);
 
-  SmartFactor::shared_ptr smartFactor3(new SmartFactor(sharedK2));
+  SmartFactor::shared_ptr smartFactor3(new SmartFactor(model, sharedK2));
-  smartFactor3->add(measurements_cam3, views, model);
+  smartFactor3->add(measurements_cam3, views);
 
   const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
 
@@ -366,8 +362,8 @@ TEST( SmartProjectionPoseFactor, Factors ) {
   views.push_back(x1);
   views.push_back(x2);
 
-  SmartFactor::shared_ptr smartFactor1 = boost::make_shared<SmartFactor>(sharedK);
+  SmartFactor::shared_ptr smartFactor1 = boost::make_shared<SmartFactor>(model, sharedK);
-  smartFactor1->add(measurements_cam1, views, model);
+  smartFactor1->add(measurements_cam1, views);
 
   SmartFactor::Cameras cameras;
   cameras.push_back(cam1);
@@ -524,14 +520,14 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) {
   projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
   projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
 
-  SmartFactor::shared_ptr smartFactor1(new SmartFactor(sharedK));
+  SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK));
-  smartFactor1->add(measurements_cam1, views, model);
+  smartFactor1->add(measurements_cam1, views);
 
-  SmartFactor::shared_ptr smartFactor2(new SmartFactor(sharedK));
+  SmartFactor::shared_ptr smartFactor2(new SmartFactor(model, sharedK));
-  smartFactor2->add(measurements_cam2, views, model);
+  smartFactor2->add(measurements_cam2, views);
 
-  SmartFactor::shared_ptr smartFactor3(new SmartFactor(sharedK));
+  SmartFactor::shared_ptr smartFactor3(new SmartFactor(model, sharedK));
-  smartFactor3->add(measurements_cam3, views, model);
+  smartFactor3->add(measurements_cam3, views);
 
   const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
 
@@ -586,19 +582,19 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) {
   params.setLinearizationMode(gtsam::JACOBIAN_SVD);
   params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY);
   params.setEnableEPI(false);
-  SmartFactor factor1(sharedK, boost::none, params);
+  SmartFactor factor1(model, sharedK, boost::none, params);
 
   SmartFactor::shared_ptr smartFactor1(
-      new SmartFactor(sharedK, boost::none, params));
+      new SmartFactor(model, sharedK, boost::none, params));
-  smartFactor1->add(measurements_cam1, views, model);
+  smartFactor1->add(measurements_cam1, views);
 
   SmartFactor::shared_ptr smartFactor2(
-      new SmartFactor(sharedK, boost::none, params));
+      new SmartFactor(model, sharedK, boost::none, params));
-  smartFactor2->add(measurements_cam2, views, model);
+  smartFactor2->add(measurements_cam2, views);
 
   SmartFactor::shared_ptr smartFactor3(
-      new SmartFactor(sharedK, boost::none, params));
+      new SmartFactor(model, sharedK, boost::none, params));
-  smartFactor3->add(measurements_cam3, views, model);
+  smartFactor3->add(measurements_cam3, views);
 
   const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
 
@@ -650,16 +646,16 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) {
   params.setEnableEPI(false);
 
   SmartFactor::shared_ptr smartFactor1(
-      new SmartFactor(sharedK, boost::none, params));
+      new SmartFactor(model, sharedK, boost::none, params));
-  smartFactor1->add(measurements_cam1, views, model);
+  smartFactor1->add(measurements_cam1, views);
 
   SmartFactor::shared_ptr smartFactor2(
-      new SmartFactor(sharedK, boost::none, params));
+      new SmartFactor(model, sharedK, boost::none, params));
-  smartFactor2->add(measurements_cam2, views, model);
+  smartFactor2->add(measurements_cam2, views);
 
   SmartFactor::shared_ptr smartFactor3(
-      new SmartFactor(sharedK, boost::none, params));
+      new SmartFactor(model, sharedK, boost::none, params));
-  smartFactor3->add(measurements_cam3, views, model);
+  smartFactor3->add(measurements_cam3, views);
 
   const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
 
@@ -717,20 +713,20 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) {
   params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold);
 
   SmartFactor::shared_ptr smartFactor1(
-      new SmartFactor(sharedK, boost::none, params));
+      new SmartFactor(model, sharedK, boost::none, params));
-  smartFactor1->add(measurements_cam1, views, model);
+  smartFactor1->add(measurements_cam1, views);
 
   SmartFactor::shared_ptr smartFactor2(
-      new SmartFactor(sharedK, boost::none, params));
+      new SmartFactor(model, sharedK, boost::none, params));
-  smartFactor2->add(measurements_cam2, views, model);
+  smartFactor2->add(measurements_cam2, views);
 
   SmartFactor::shared_ptr smartFactor3(
-      new SmartFactor(sharedK, boost::none, params));
+      new SmartFactor(model, sharedK, boost::none, params));
-  smartFactor3->add(measurements_cam3, views, model);
+  smartFactor3->add(measurements_cam3, views);
 
   SmartFactor::shared_ptr smartFactor4(
-      new SmartFactor(sharedK, boost::none, params));
+      new SmartFactor(model, sharedK, boost::none, params));
-  smartFactor4->add(measurements_cam4, views, model);
+  smartFactor4->add(measurements_cam4, views);
 
   const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
 
@@ -775,16 +771,16 @@ TEST( SmartProjectionPoseFactor, jacobianQ ) {
   params.setLinearizationMode(gtsam::JACOBIAN_Q);
 
   SmartFactor::shared_ptr smartFactor1(
-      new SmartFactor(sharedK, boost::none, params));
+      new SmartFactor(model, sharedK, boost::none, params));
-  smartFactor1->add(measurements_cam1, views, model);
+  smartFactor1->add(measurements_cam1, views);
 
   SmartFactor::shared_ptr smartFactor2(
-      new SmartFactor(sharedK, boost::none, params));
+      new SmartFactor(model, sharedK, boost::none, params));
-  smartFactor2->add(measurements_cam2, views, model);
+  smartFactor2->add(measurements_cam2, views);
 
   SmartFactor::shared_ptr smartFactor3(
-      new SmartFactor(sharedK, boost::none, params));
+      new SmartFactor(model, sharedK, boost::none, params));
-  smartFactor3->add(measurements_cam3, views, model);
+  smartFactor3->add(measurements_cam3, views);
 
   const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
 
@@ -895,16 +891,16 @@ TEST( SmartProjectionPoseFactor, CheckHessian) {
   params.setRankTolerance(10);
 
   SmartFactor::shared_ptr smartFactor1(
-      new SmartFactor(sharedK, boost::none, params)); // HESSIAN, by default
+      new SmartFactor(model, sharedK, boost::none, params)); // HESSIAN, by default
-  smartFactor1->add(measurements_cam1, views, model);
+  smartFactor1->add(measurements_cam1, views);
 
   SmartFactor::shared_ptr smartFactor2(
-      new SmartFactor(sharedK, boost::none, params)); // HESSIAN, by default
+      new SmartFactor(model, sharedK, boost::none, params)); // HESSIAN, by default
-  smartFactor2->add(measurements_cam2, views, model);
+  smartFactor2->add(measurements_cam2, views);
 
   SmartFactor::shared_ptr smartFactor3(
-      new SmartFactor(sharedK, boost::none, params)); // HESSIAN, by default
+      new SmartFactor(model, sharedK, boost::none, params)); // HESSIAN, by default
-  smartFactor3->add(measurements_cam3, views, model);
+  smartFactor3->add(measurements_cam3, views);
 
   NonlinearFactorGraph graph;
   graph.push_back(smartFactor1);
@@ -978,12 +974,12 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac
   params.setDegeneracyMode(gtsam::HANDLE_INFINITY);
 
   SmartFactor::shared_ptr smartFactor1(
-      new SmartFactor(sharedK2, boost::none, params));
+      new SmartFactor(model, sharedK2, boost::none, params));
-  smartFactor1->add(measurements_cam1, views, model);
+  smartFactor1->add(measurements_cam1, views);
 
   SmartFactor::shared_ptr smartFactor2(
-      new SmartFactor(sharedK2, boost::none, params));
+      new SmartFactor(model, sharedK2, boost::none, params));
-  smartFactor2->add(measurements_cam2, views, model);
+  smartFactor2->add(measurements_cam2, views);
 
   const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
   const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10);
@@ -1040,16 +1036,16 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor )
   params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY);
 
   SmartFactor::shared_ptr smartFactor1(
-      new SmartFactor(sharedK, boost::none, params));
+      new SmartFactor(model, sharedK, boost::none, params));
-  smartFactor1->add(measurements_cam1, views, model);
+  smartFactor1->add(measurements_cam1, views);
 
   SmartFactor::shared_ptr smartFactor2(
-      new SmartFactor(sharedK, boost::none, params));
+      new SmartFactor(model, sharedK, boost::none, params));
-  smartFactor2->add(measurements_cam2, views, model);
+  smartFactor2->add(measurements_cam2, views);
 
   SmartFactor::shared_ptr smartFactor3(
-      new SmartFactor(sharedK, boost::none, params));
+      new SmartFactor(model, sharedK, boost::none, params));
-  smartFactor3->add(measurements_cam3, views, model);
+  smartFactor3->add(measurements_cam3, views);
 
   const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
   const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3,
@@ -1108,8 +1104,8 @@ TEST( SmartProjectionPoseFactor, Hessian ) {
   measurements_cam1.push_back(cam1_uv1);
   measurements_cam1.push_back(cam2_uv1);
 
-  SmartFactor::shared_ptr smartFactor1(new SmartFactor(sharedK2));
+  SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK2));
-  smartFactor1->add(measurements_cam1, views, model);
+  smartFactor1->add(measurements_cam1, views);
 
   Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10),
       Point3(0.5, 0.1, 0.3));
@@ -1140,8 +1136,8 @@ TEST( SmartProjectionPoseFactor, HessianWithRotation ) {
 
   projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
 
-  SmartFactor::shared_ptr smartFactorInstance(new SmartFactor(sharedK));
+  SmartFactor::shared_ptr smartFactorInstance(new SmartFactor(model, sharedK));
-  smartFactorInstance->add(measurements_cam1, views, model);
+  smartFactorInstance->add(measurements_cam1, views);
 
   Values values;
   values.insert(x1, cam1.pose());
@@ -1196,8 +1192,8 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) {
   vector<Point2> measurements_cam1;
   projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
 
-  SmartFactor::shared_ptr smartFactor(new SmartFactor(sharedK2));
+  SmartFactor::shared_ptr smartFactor(new SmartFactor(model, sharedK2));
-  smartFactor->add(measurements_cam1, views, model);
+  smartFactor->add(measurements_cam1, views);
 
   Values values;
   values.insert(x1, cam1.pose());
@@ -1239,8 +1235,8 @@ TEST( SmartProjectionPoseFactor, ConstructorWithCal3Bundler) {
   using namespace bundlerPose;
   SmartProjectionParams params;
   params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY);
-  SmartFactor factor(sharedBundlerK, boost::none, params);
+  SmartFactor factor(model, sharedBundlerK, boost::none, params);
-  factor.add(measurement1, x1, model);
+  factor.add(measurement1, x1);
 }
 
 /* *************************************************************************/
@@ -1263,14 +1259,14 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ) {
   views.push_back(x2);
   views.push_back(x3);
 
-  SmartFactor::shared_ptr smartFactor1(new SmartFactor(sharedBundlerK));
+  SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedBundlerK));
-  smartFactor1->add(measurements_cam1, views, model);
+  smartFactor1->add(measurements_cam1, views);
 
-  SmartFactor::shared_ptr smartFactor2(new SmartFactor(sharedBundlerK));
+  SmartFactor::shared_ptr smartFactor2(new SmartFactor(model, sharedBundlerK));
-  smartFactor2->add(measurements_cam2, views, model);
+  smartFactor2->add(measurements_cam2, views);
 
-  SmartFactor::shared_ptr smartFactor3(new SmartFactor(sharedBundlerK));
+  SmartFactor::shared_ptr smartFactor3(new SmartFactor(model, sharedBundlerK));
-  smartFactor3->add(measurements_cam3, views, model);
+  smartFactor3->add(measurements_cam3, views);
 
   const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
 
@@ -1334,16 +1330,16 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) {
   params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY);
 
   SmartFactor::shared_ptr smartFactor1(
-      new SmartFactor(sharedBundlerK, boost::none, params));
+      new SmartFactor(model, sharedBundlerK, boost::none, params));
-  smartFactor1->add(measurements_cam1, views, model);
+  smartFactor1->add(measurements_cam1, views);
 
   SmartFactor::shared_ptr smartFactor2(
-      new SmartFactor(sharedBundlerK, boost::none, params));
+      new SmartFactor(model, sharedBundlerK, boost::none, params));
-  smartFactor2->add(measurements_cam2, views, model);
+  smartFactor2->add(measurements_cam2, views);
 
   SmartFactor::shared_ptr smartFactor3(
-      new SmartFactor(sharedBundlerK, boost::none, params));
+      new SmartFactor(model, sharedBundlerK, boost::none, params));
-  smartFactor3->add(measurements_cam3, views, model);
+  smartFactor3->add(measurements_cam3, views);
 
   const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
   const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3,

gtsam/slam/tests/testTriangulationFactor.cpp

@@ -18,7 +18,10 @@
 
 #include <gtsam/geometry/triangulation.h>
 #include <gtsam/geometry/SimpleCamera.h>
+#include <gtsam/geometry/StereoCamera.h>
 #include <gtsam/geometry/Cal3Bundler.h>
+#include <gtsam/nonlinear/Expression.h>
+#include <gtsam/nonlinear/ExpressionFactor.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <CppUnitLite/TestHarness.h>
 
@@ -46,7 +49,7 @@ Point2 z1 = camera1.project(landmark);
 
 //******************************************************************************
 TEST( triangulation, TriangulationFactor ) {
-  // Create the factor with a measurement that is 3 pixels off in x
+
   Key pointKey(1);
   SharedNoiseModel model;
   typedef TriangulationFactor<SimpleCamera> Factor;
@@ -63,6 +66,46 @@ TEST( triangulation, TriangulationFactor ) {
   CHECK(assert_equal(HExpected, HActual, 1e-3));
 }
 
+//******************************************************************************
+TEST( triangulation, TriangulationFactorStereo ) {
+
+  Key pointKey(1);
+  SharedNoiseModel model=noiseModel::Isotropic::Sigma(3,0.5);
+  Cal3_S2Stereo::shared_ptr stereoCal(new Cal3_S2Stereo(1500, 1200, 0, 640, 480, 0.5));
+  StereoCamera camera2(pose1, stereoCal);
+
+  StereoPoint2 z2 = camera2.project(landmark);
+
+  typedef TriangulationFactor<StereoCamera> Factor;
+  Factor factor(camera2, z2, model, pointKey);
+
+  // Use the factor to calculate the Jacobians
+  Matrix HActual;
+  factor.evaluateError(landmark, HActual);
+
+  Matrix HExpected = numericalDerivative11<Vector,Point3>(
+      boost::bind(&Factor::evaluateError, &factor, _1, boost::none), landmark);
+
+  // Verify the Jacobians are correct
+  CHECK(assert_equal(HExpected, HActual, 1e-3));
+
+  // compare same problem against expression factor
+  Expression<StereoPoint2>::UnaryFunction<Point3>::type f = boost::bind(&StereoCamera::project2, camera2, _1, boost::none, _2);
+  Expression<Point3> point_(pointKey);
+  Expression<StereoPoint2> project2_(f, point_);
+
+  ExpressionFactor<StereoPoint2> eFactor(model, z2, project2_);
+
+  Values values;
+  values.insert(pointKey, landmark);
+
+  vector<Matrix> HActual1(1), HActual2(1);
+  Vector error1 = factor.unwhitenedError(values, HActual1);
+  Vector error2 = eFactor.unwhitenedError(values, HActual2);
+  EXPECT(assert_equal(error1, error2));
+  EXPECT(assert_equal(HActual1[0], HActual2[0]));
+}
+
 //******************************************************************************
 int main() {
   TestResult tr;

gtsam_unstable/examples/SmartProjectionFactorExample.cpp

@@ -87,17 +87,17 @@ int main(int argc, char** argv){
 
   //read stereo measurements and construct smart factors
 
-  SmartFactor::shared_ptr factor(new SmartFactor(K));
+  SmartFactor::shared_ptr factor(new SmartFactor(model, K));
   size_t current_l = 3;   // hardcoded landmark ID from first measurement
 
   while (factor_file >> i >> l >> uL >> uR >> v >> X >> Y >> Z) {
 
     if(current_l != l) {
       graph.push_back(factor);
-      factor = SmartFactor::shared_ptr(new SmartFactor(K));
+      factor = SmartFactor::shared_ptr(new SmartFactor(model, K));
       current_l = l;
     }
-    factor->add(Point2(uL,v), i, model);
+    factor->add(Point2(uL,v), i);
   }
 
   Pose3 firstPose = initial_estimate.at<Pose3>(1);

gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp

@@ -46,7 +46,7 @@ using namespace gtsam;
 
 int main(int argc, char** argv){
 
-  typedef SmartStereoProjectionPoseFactor<Cal3_S2Stereo> SmartFactor;
+  typedef SmartStereoProjectionPoseFactor SmartFactor;
 
   bool output_poses = true;
   string poseOutput("../../../examples/data/optimized_poses.txt");
@@ -109,17 +109,17 @@ int main(int argc, char** argv){
 
   //read stereo measurements and construct smart factors
 
-  SmartFactor::shared_ptr factor(new SmartFactor());
+  SmartFactor::shared_ptr factor(new SmartFactor(model));
   size_t current_l = 3;   // hardcoded landmark ID from first measurement
 
   while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) {
 
     if(current_l != l) {
       graph.push_back(factor);
-      factor = SmartFactor::shared_ptr(new SmartFactor());
+      factor = SmartFactor::shared_ptr(new SmartFactor(model));
       current_l = l;
     }
-    factor->add(StereoPoint2(uL,uR,v), Symbol('x',x), model, K);
+    factor->add(StereoPoint2(uL,uR,v), Symbol('x',x), K);
   }
 
   Pose3 first_pose = initial_estimate.at<Pose3>(Symbol('x',1));

gtsam_unstable/slam/SmartStereoProjectionFactor.h

@@ -11,10 +11,11 @@
 
 /**
  * @file   SmartStereoProjectionFactor.h
- * @brief  Base class to create smart factors on poses or cameras
+ * @brief  Smart stereo factor on StereoCameras (pose + calibration)
  * @author Luca Carlone
  * @author Zsolt Kira
  * @author Frank Dellaert
+ * @author Chris Beall
  */
 
 #pragma once
@@ -24,6 +25,7 @@
 #include <gtsam/geometry/triangulation.h>
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/geometry/StereoCamera.h>
+#include <gtsam/slam/StereoFactor.h>
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/slam/dataset.h>
 
@@ -33,79 +35,136 @@
 
 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
+  */
+ 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
+ * SmartStereoProjectionFactor: triangulates point and keeps an estimate of it around.
- */
+ * This factor operates with StereoCamera. This factor requires that values
-template<class CALIBRATION>
+ * contains the involved StereoCameras. Calibration is assumed to be fixed, as this
+ * is also assumed in StereoCamera.
+ * If you'd like to store poses in values instead of cameras, use
+ * SmartStereoProjectionPoseFactor instead
+*/
 class SmartStereoProjectionFactor: public SmartFactorBase<StereoCamera> {
+private:
+
+  typedef SmartFactorBase<StereoCamera> Base;
+
 protected:
 
+  /// @name Parameters
+  /// @{
+  const SmartStereoProjectionParams params_;
+  /// @}
+
   /// @name Caching triangulation
   /// @{
-  const TriangulationParameters parameters_;
+  mutable TriangulationResult result_; ///< result from triangulateSafe
-  // TODO:
-//  mutable TriangulationResult result_; ///< result from triangulateSafe
-
-  const double retriangulationThreshold_; ///< threshold to decide whether to re-triangulate
   mutable std::vector<Pose3> cameraPosesTriangulation_; ///< current triangulation poses
   /// @}
 
-  const bool manageDegeneracy_; ///< if set to true will use the rotation-only version for degenerate cases
-
-  const double linearizationThreshold_; ///< threshold to decide whether to re-linearize
-  mutable std::vector<Pose3> cameraPosesLinearization_; ///< current linearization poses
-
-  mutable Point3 point_; ///< Current estimate of the 3D point
-
-  mutable bool degenerate_;
-  mutable bool cheiralityException_;
-
-
-  /// shorthand for base class type
-  typedef SmartFactorBase<StereoCamera> Base;
-
-  /// shorthand for this class
-  typedef SmartStereoProjectionFactor<CALIBRATION> This;
-
-  enum {
-    ZDim = 3
-  }; ///< Dimension trait of measurement type
-
-  /// @name Parameters governing how triangulation result is treated
-  /// @{
-   const bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false)
-   const bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false)
-   /// @}
-
 public:
 
   /// shorthand for a smart pointer to a factor
-  typedef boost::shared_ptr<This> shared_ptr;
+  typedef boost::shared_ptr<SmartStereoProjectionFactor> shared_ptr;
-
-  /// shorthand for a StereoCamera // TODO: Get rid of this?
-  typedef StereoCamera Camera;
 
   /// Vector of cameras
-  typedef CameraSet<Camera> Cameras;
+  typedef CameraSet<StereoCamera> Cameras;
 
   /**
    * Constructor
-   * @param rankTol tolerance used to check if point triangulation is degenerate
+   * @param params internal parameters of the smart factors
-   * @param linThreshold threshold on relative pose changes used to decide whether to relinearize (selective relinearization)
-   * @param manageDegeneracy is true, in presence of degenerate triangulation, the factor is converted to a rotation-only constraint,
-   * otherwise the factor is simply neglected
-   * @param enableEPI if set to true linear triangulation is refined with embedded LM iterations
-   * @param body_P_sensor is the transform from body to sensor frame (default identity)
    */
-  SmartStereoProjectionFactor(const double rankTolerance,
+  SmartStereoProjectionFactor(const SharedNoiseModel& sharedNoiseModel,
-      const double linThreshold, const bool manageDegeneracy,
+      const SmartStereoProjectionParams& params =
-      const bool enableEPI, double landmarkDistanceThreshold = 1e10,
+      SmartStereoProjectionParams()) :
-      double dynamicOutlierRejectionThreshold = -1) :
+      Base(sharedNoiseModel), //
-      parameters_(rankTolerance, enableEPI, landmarkDistanceThreshold,
+      params_(params), //
-          dynamicOutlierRejectionThreshold), //
+      result_(TriangulationResult::Degenerate()) {
-      retriangulationThreshold_(1e-5), manageDegeneracy_(manageDegeneracy), linearizationThreshold_(
-          linThreshold), degenerate_(false), cheiralityException_(false), throwCheirality_(
-          false), verboseCheirality_(false) {
   }
 
   /** Virtual destructor */
@@ -120,14 +179,20 @@ public:
   void print(const std::string& s = "", const KeyFormatter& keyFormatter =
       DefaultKeyFormatter) const {
     std::cout << s << "SmartStereoProjectionFactor\n";
-    std::cout << "triangulationParameters:\n" << parameters_ << std::endl;
+    std::cout << "linearizationMode:\n" << params_.linearizationMode << std::endl;
-    std::cout << "degenerate_ = " << degenerate_ << std::endl;
+    std::cout << "triangulationParameters:\n" << params_.triangulation << std::endl;
-    std::cout << "cheiralityException_ = " << cheiralityException_ << std::endl;
+    std::cout << "result:\n" << result_ << std::endl;
-    std::cout << "linearizationThreshold_ = " << linearizationThreshold_
-        << std::endl;
     Base::print("", keyFormatter);
   }
 
+  /// equals
+  virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
+    const SmartStereoProjectionFactor *e =
+        dynamic_cast<const SmartStereoProjectionFactor*>(&p);
+    return e && params_.linearizationMode == e->params_.linearizationMode
+        && Base::equals(p, tol);
+  }
+
   /// Check if the new linearization point_ is the same as the one used for previous triangulation
   bool decideIfTriangulate(const Cameras& cameras) const {
     // several calls to linearize will be done from the same linearization point_, hence it is not needed to re-triangulate
@@ -146,7 +211,7 @@ public:
     if (!retriangulate) {
       for (size_t i = 0; i < cameras.size(); i++) {
         if (!cameras[i].pose().equals(cameraPosesTriangulation_[i],
-            retriangulationThreshold_)) {
+            params_.retriangulationThreshold)) {
           retriangulate = true; // at least two poses are different, hence we retriangulate
           break;
         }
@@ -164,124 +229,99 @@ public:
     return retriangulate; // if we arrive to this point_ all poses are the same and we don't need re-triangulation
   }
 
-  /// triangulateSafe
+//  /// triangulateSafe
-  size_t triangulateSafe(const Values& values) const {
+//  size_t triangulateSafe(const Values& values) const {
-    return triangulateSafe(this->cameras(values));
+//    return triangulateSafe(this->cameras(values));
-  }
+//  }
 
   /// triangulateSafe
-  size_t triangulateSafe(const Cameras& cameras) const {
+  TriangulationResult triangulateSafe(const Cameras& cameras) const {
 
     size_t m = cameras.size();
-    if (m < 2) { // if we have a single pose the corresponding factor is uninformative
-      degenerate_ = true;
-      return m;
-    }
     bool retriangulate = decideIfTriangulate(cameras);
 
+//    if(!retriangulate)
+//      std::cout << "retriangulate = false" << std::endl;
+//
+//    bool retriangulate = true;
+
     if (retriangulate) {
-      // We triangulate the 3D position of the landmark
+//      std::cout << "Retriangulate " << std::endl;
-      try {
+      std::vector<Point3> reprojections;
-        // std::cout << "triangulatePoint3 i \n" << rankTolerance << std::endl;
+      reprojections.reserve(m);
-
+      for(size_t i = 0; i < m; i++) {
-        //TODO: Chris will replace this with something else for stereo
+        reprojections.push_back(cameras[i].backproject(measured_[i]));
-//        point_ = triangulatePoint3<CALIBRATION>(cameras, this->measured_,
-//            rankTolerance_, enableEPI_);
-
-        // // // Temporary hack to use monocular triangulation
-        std::vector<Point2> mono_measurements;
-        BOOST_FOREACH(const StereoPoint2& sp, this->measured_) {
-          mono_measurements.push_back(sp.point2());
-        }
-
-        std::vector<PinholeCamera<Cal3_S2> > mono_cameras;
-        BOOST_FOREACH(const Camera& camera, cameras) {
-          const Pose3& pose = camera.pose();
-          const Cal3_S2& K = camera.calibration()->calibration();
-          mono_cameras.push_back(PinholeCamera<Cal3_S2>(pose, K));
-        }
-        point_ = triangulatePoint3<PinholeCamera<Cal3_S2> >(mono_cameras, mono_measurements,
-            parameters_.rankTolerance, parameters_.enableEPI);
-
-        // // // End temporary hack
-
-        // FIXME: temporary: triangulation using only first camera
-//        const StereoPoint2& z0 = this->measured_.at(0);
-//        point_ = cameras[0].backproject(z0);
-
-        degenerate_ = false;
-        cheiralityException_ = false;
-
-        // Check landmark distance and reprojection errors to avoid outliers
-        double totalReprojError = 0.0;
-        size_t i = 0;
-        BOOST_FOREACH(const Camera& camera, cameras) {
-          Point3 cameraTranslation = camera.pose().translation();
-          // we discard smart factors corresponding to points that are far away
-          if (cameraTranslation.distance(point_) > parameters_.landmarkDistanceThreshold) {
-            degenerate_ = true;
-            break;
-          }
-          const StereoPoint2& zi = this->measured_.at(i);
-          try {
-            StereoPoint2 reprojectionError(camera.project(point_) - zi);
-            totalReprojError += reprojectionError.vector().norm();
-          } catch (CheiralityException) {
-            cheiralityException_ = true;
-          }
-          i += 1;
-        }
-        //std::cout << "totalReprojError error: " << totalReprojError << std::endl;
-        // we discard smart factors that have large reprojection error
-        if (parameters_.dynamicOutlierRejectionThreshold > 0
-            && totalReprojError / m > parameters_.dynamicOutlierRejectionThreshold)
-          degenerate_ = true;
-
-      } catch (TriangulationUnderconstrainedException&) {
-        // if TriangulationUnderconstrainedException can be
-        // 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before
-        // 2) The rank of the matrix used for triangulation is < 3: rotation-only, parallel cameras (or motion towards the landmark)
-        // in the second case we want to use a rotation-only smart factor
-        degenerate_ = true;
-        cheiralityException_ = false;
-      } catch (TriangulationCheiralityException&) {
-        // point is behind one of the cameras: can be the case of close-to-parallel cameras or may depend on outliers
-        // we manage this case by either discarding the smart factor, or imposing a rotation-only constraint
-        cheiralityException_ = true;
       }
-    }
+
-    return m;
+      Point3 pw_sum;
+      BOOST_FOREACH(const Point3& pw, reprojections) {
+        pw_sum = pw_sum + pw;
+      }
+      // 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::Degenerate();
+          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::Degenerate();
+        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_
-    bool isDebug = false;
+    return (result_);
-    size_t nrCameras = this->triangulateSafe(cameras);
-
-    if (nrCameras < 2
-        || (!this->manageDegeneracy_
-            && (this->cheiralityException_ || this->degenerate_))) {
-      if (isDebug) {
-        std::cout << "createImplicitSchurFactor: degenerate configuration"
-            << std::endl;
-      }
-      return false;
-    } else {
-
-      // instead, if we want to manage the exception..
-      if (this->cheiralityException_ || this->degenerate_) { // if we want to manage the exceptions with rotation-only factors
-        this->degenerate_ = true;
-      }
-      return true;
-    }
   }
 
   /// linearize returns a Hessianfactor that is an approximation of error(p)
   boost::shared_ptr<RegularHessianFactor<Base::Dim> > createHessianFactor(
-      const Cameras& cameras, const double lambda = 0.0) const {
+      const Cameras& cameras, const double lambda = 0.0,  bool diagonalDamping =
+          false) const {
 
-    bool isDebug = false;
     size_t numKeys = this->keys_.size();
     // Create structures for Hessian Factors
     std::vector<Key> js;
@@ -290,146 +330,142 @@ public:
 
     if (this->measured_.size() != cameras.size()) {
       std::cout
-          << "SmartProjectionHessianFactor: this->measured_.size() inconsistent with input"
+          << "SmartStereoProjectionHessianFactor: this->measured_.size() inconsistent with input"
           << std::endl;
       exit(1);
     }
 
     triangulateSafe(cameras);
-    if (isDebug)
-      std::cout << "point_ = " << point_ << std::endl;
 
-    if (numKeys < 2
+    if (params_.degeneracyMode == ZERO_ON_DEGENERACY && !result_) {
-        || (!this->manageDegeneracy_
+      // failed: return"empty" Hessian
-            && (this->cheiralityException_ || this->degenerate_))) {
-      if (isDebug)
-        std::cout << "In linearize: exception" << std::endl;
       BOOST_FOREACH(Matrix& m, Gs)
         m = zeros(Base::Dim, Base::Dim);
       BOOST_FOREACH(Vector& v, gs)
         v = zero(Base::Dim);
-      return boost::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_, Gs, gs,
+      return boost::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_,
-          0.0);
+          Gs, gs, 0.0);
     }
 
-    // instead, if we want to manage the exception..
+    // Jacobian could be 3D Point3 OR 2D Unit3, difference is E.cols().
-    if (this->cheiralityException_ || this->degenerate_) { // if we want to manage the exceptions with rotation-only factors
+    std::vector<Base::MatrixZD> Fblocks;
-      this->degenerate_ = true;
-      if (isDebug)
-        std::cout << "degenerate_ = true" << std::endl;
-    }
-
-    if (this->linearizationThreshold_ >= 0) // if we apply selective relinearization and we need to relinearize
-      for (size_t i = 0; i < cameras.size(); i++)
-        this->cameraPosesLinearization_[i] = cameras[i].pose();
-
-    // ==================================================================
-    std::vector<typename Base::MatrixZD> Fblocks;
     Matrix F, E;
     Vector b;
-    computeJacobians(Fblocks, E, b, cameras);
+    computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras);
-    Base::FillDiagonalF(Fblocks, F); // expensive !!!
 
-    // Schur complement trick
+    // Whiten using noise model
-    // Frank says: should be possible to do this more efficiently?
+    Base::whitenJacobians(Fblocks, E, b);
-    // And we care, as in grouped factors this is called repeatedly
-    Matrix H(Base::Dim * numKeys, Base::Dim * numKeys);
-    Vector gs_vector;
 
-    Matrix3 P = Cameras::PointCov(E, lambda);
+    // build augmented hessian
-    H.noalias() = F.transpose() * (F - (E * (P * (E.transpose() * F))));
+    SymmetricBlockMatrix augmentedHessian = //
-    gs_vector.noalias() = F.transpose() * (b - (E * (P * (E.transpose() * b))));
+        Cameras::SchurComplement(Fblocks, E, b, lambda, diagonalDamping);
 
-    if (isDebug)
+    return boost::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_,
-      std::cout << "gs_vector size " << gs_vector.size() << std::endl;
+        augmentedHessian);
-    if (isDebug)
-      std::cout << "H:\n" << H << std::endl;
-
-    // Populate Gs and gs
-    int GsCount2 = 0;
-    for (DenseIndex i1 = 0; i1 < (DenseIndex) numKeys; i1++) { // for each camera
-      DenseIndex i1D = i1 * Base::Dim;
-      gs.at(i1) = gs_vector.segment<Base::Dim>(i1D);
-      for (DenseIndex i2 = 0; i2 < (DenseIndex) numKeys; i2++) {
-        if (i2 >= i1) {
-          Gs.at(GsCount2) = H.block<Base::Dim, Base::Dim>(i1D, i2 * Base::Dim);
-          GsCount2++;
-        }
-      }
-    }
-    // ==================================================================
-    double f = b.squaredNorm();
-    return boost::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_, Gs, gs, f);
   }
 
-//  // create factor
+  // create factor
-//  boost::shared_ptr<ImplicitSchurFactor<Base::Dim> > createImplicitSchurFactor(
+//  boost::shared_ptr<RegularImplicitSchurFactor<StereoCamera> > createRegularImplicitSchurFactor(
 //      const Cameras& cameras, double lambda) const {
 //    if (triangulateForLinearize(cameras))
-//      return Base::createImplicitSchurFactor(cameras, point_, lambda);
+//      return Base::createRegularImplicitSchurFactor(cameras, *result_, lambda);
 //    else
-//      return boost::shared_ptr<ImplicitSchurFactor<Base::Dim> >();
+//      // failed: return empty
+//      return boost::shared_ptr<RegularImplicitSchurFactor<StereoCamera> >();
 //  }
 //
 //  /// create factor
-//  boost::shared_ptr<JacobianFactorQ<Base::Dim> > createJacobianQFactor(
+//  boost::shared_ptr<JacobianFactorQ<Base::Dim, Base::ZDim> > createJacobianQFactor(
 //      const Cameras& cameras, double lambda) const {
 //    if (triangulateForLinearize(cameras))
-//      return Base::createJacobianQFactor(cameras, point_, lambda);
+//      return Base::createJacobianQFactor(cameras, *result_, lambda);
 //    else
-//      return boost::make_shared< JacobianFactorQ<Base::Dim> >(this->keys_);
+//      // failed: return empty
+//      return boost::make_shared<JacobianFactorQ<Base::Dim, Base::ZDim> >(this->keys_);
 //  }
 //
 //  /// Create a factor, takes values
-//  boost::shared_ptr<JacobianFactorQ<Base::Dim> > createJacobianQFactor(
+//  boost::shared_ptr<JacobianFactorQ<Base::Dim, Base::ZDim> > createJacobianQFactor(
 //      const Values& values, double lambda) const {
-//    Cameras cameras;
+//    return createJacobianQFactor(this->cameras(values), lambda);
-//    // TODO triangulate twice ??
-//    bool nonDegenerate = computeCamerasAndTriangulate(values, cameras);
-//    if (nonDegenerate)
-//      return createJacobianQFactor(cameras, lambda);
-//    else
-//      return boost::make_shared< JacobianFactorQ<Base::Dim> >(this->keys_);
 //  }
-//
+
   /// different (faster) way to compute Jacobian factor
   boost::shared_ptr<JacobianFactor> createJacobianSVDFactor(
       const Cameras& cameras, double lambda) const {
     if (triangulateForLinearize(cameras))
-      return Base::createJacobianSVDFactor(cameras, point_, lambda);
+      return Base::createJacobianSVDFactor(cameras, *result_, lambda);
     else
       return boost::make_shared<JacobianFactorSVD<Base::Dim, ZDim> >(this->keys_);
   }
 
-  /// Returns true if nonDegenerate
+//  /// linearize to a Hessianfactor
-  bool computeCamerasAndTriangulate(const Values& values,
+//  virtual boost::shared_ptr<RegularHessianFactor<Base::Dim> > linearizeToHessian(
-      Cameras& cameras) const {
+//      const Values& values, double lambda = 0.0) const {
-    Values valuesFactor;
+//    return createHessianFactor(this->cameras(values), lambda);
+//  }
 
-    // Select only the cameras
+//  /// linearize to an Implicit Schur factor
-    BOOST_FOREACH(const Key key, this->keys_)
+//  virtual boost::shared_ptr<RegularImplicitSchurFactor<StereoCamera> > linearizeToImplicit(
-      valuesFactor.insert(key, values.at(key));
+//      const Values& values, double lambda = 0.0) const {
+//    return createRegularImplicitSchurFactor(this->cameras(values), lambda);
+//  }
+//
+//  /// linearize to a JacobianfactorQ
+//  virtual boost::shared_ptr<JacobianFactorQ<Base::Dim, Base::ZDim> > linearizeToJacobian(
+//      const Values& values, double lambda = 0.0) const {
+//    return createJacobianQFactor(this->cameras(values), lambda);
+//  }
 
-    cameras = this->cameras(valuesFactor);
+  /**
-    size_t nrCameras = this->triangulateSafe(cameras);
+   * Linearize to Gaussian Factor
-
+   * @param values Values structure which must contain camera poses for this factor
-    if (nrCameras < 2
+   * @return a Gaussian factor
-        || (!this->manageDegeneracy_
+   */
-            && (this->cheiralityException_ || this->degenerate_)))
+  boost::shared_ptr<GaussianFactor> linearizeDamped(const Cameras& cameras,
-      return false;
+      const double lambda = 0.0) const {
-
+    // depending on flag set on construction we may linearize to different linear factors
-    // instead, if we want to manage the exception..
+    switch (params_.linearizationMode) {
-    if (this->cheiralityException_ || this->degenerate_) // if we want to manage the exceptions with rotation-only factors
+    case HESSIAN:
-      this->degenerate_ = true;
+      return createHessianFactor(cameras, lambda);
-
+//    case IMPLICIT_SCHUR:
-    if (this->degenerate_) {
+//      return createRegularImplicitSchurFactor(cameras, lambda);
-      std::cout << "SmartStereoProjectionFactor: this is not ready"
+    case JACOBIAN_SVD:
-          << std::endl;
+      return createJacobianSVDFactor(cameras, lambda);
-      std::cout << "this->cheiralityException_ " << this->cheiralityException_
+//    case JACOBIAN_Q:
-          << std::endl;
+//      return createJacobianQFactor(cameras, lambda);
-      std::cout << "this->degenerate_ " << this->degenerate_ << std::endl;
+    default:
+      throw std::runtime_error("SmartStereoFactorlinearize: unknown mode");
     }
-    return true;
+  }
+
+  /**
+   * Linearize to Gaussian Factor
+   * @param values Values structure which must contain camera poses for this factor
+   * @return a Gaussian factor
+   */
+  boost::shared_ptr<GaussianFactor> linearizeDamped(const Values& values,
+      const double lambda = 0.0) const {
+    // depending on flag set on construction we may linearize to different linear factors
+    Cameras cameras = this->cameras(values);
+    return linearizeDamped(cameras, lambda);
+  }
+
+  /// linearize
+  virtual boost::shared_ptr<GaussianFactor> linearize(
+      const Values& values) const {
+    return linearizeDamped(values);
+  }
+
+  /**
+   * Triangulate and compute derivative of error with respect to point
+   * @return whether triangulation worked
+   */
+  bool triangulateAndComputeE(Matrix& E, const Cameras& cameras) const {
+    bool nonDegenerate = triangulateForLinearize(cameras);
+    if (nonDegenerate)
+      cameras.project2(*result_, boost::none, E);
+    return nonDegenerate;
   }
 
   /**
@@ -437,87 +473,62 @@ public:
    * @return whether triangulation worked
    */
   bool triangulateAndComputeE(Matrix& E, const Values& values) const {
-    Cameras cameras;
+    Cameras cameras = this->cameras(values);
-    bool nonDegenerate = computeCamerasAndTriangulate(values, cameras);
+    return triangulateAndComputeE(E, cameras);
-    if (nonDegenerate)
-      cameras.project2(point_, boost::none, E);
-    return nonDegenerate;
   }
 
-  /// Version that takes values, and creates the point
-  bool computeJacobians(std::vector<typename Base::MatrixZD>& Fblocks,
-      Matrix& E, Vector& b, const Values& values) const {
-    Cameras cameras;
-    bool nonDegenerate = computeCamerasAndTriangulate(values, cameras);
-    if (nonDegenerate)
-      computeJacobians(Fblocks, E, b, cameras, 0.0);
-    return nonDegenerate;
-  }
 
   /// Compute F, E only (called below in both vanilla and SVD versions)
   /// Assumes the point has been computed
   /// Note E can be 2m*3 or 2m*2, in case point is degenerate
-  void computeJacobians(std::vector<typename Base::MatrixZD>& Fblocks,
+  void computeJacobiansWithTriangulatedPoint(
-      Matrix& E, Vector& b, const Cameras& cameras) const {
+      std::vector<Base::MatrixZD>& Fblocks, Matrix& E, Vector& b,
-    if (this->degenerate_) {
+      const Cameras& cameras) const {
-      throw("FIXME: computeJacobians degenerate case commented out!");
+
-//      std::cout << "manage degeneracy " << manageDegeneracy_ << std::endl;
+    if (!result_) {
-//      std::cout << "point " << point_ << std::endl;
+      throw ("computeJacobiansWithTriangulatedPoint");
-//      std::cout
+//      // Handle degeneracy
-//          << "SmartStereoProjectionFactor: Management of degeneracy is disabled - not ready to be used"
+//      // TODO check flag whether we should do this
-//          << std::endl;
+//      Unit3 backProjected; /* = cameras[0].backprojectPointAtInfinity(
-//      if (D > 6) {
+//          this->measured_.at(0)); */
-//        std::cout
-//            << "Management of degeneracy is not yet ready when one also optimizes for the calibration "
-//            << std::endl;
-//      }
 //
-//      int numKeys = this->keys_.size();
+//      Base::computeJacobians(Fblocks, E, b, cameras, backProjected);
-//      E = zeros(2 * numKeys, 2);
-//      b = zero(2 * numKeys);
-//      double f = 0;
-//      for (size_t i = 0; i < this->measured_.size(); i++) {
-//        if (i == 0) { // first pose
-//          this->point_ = cameras[i].backprojectPointAtInfinity(
-//              this->measured_.at(i));
-//          // 3D parametrization of point at infinity: [px py 1]
-//        }
-//        Matrix Fi, Ei;
-//        Vector bi = -(cameras[i].projectPointAtInfinity(this->point_, Fi, Ei)
-//            - this->measured_.at(i)).vector();
-//
-//        this->noise_.at(i)->WhitenSystem(Fi, Ei, bi);
-//        f += bi.squaredNorm();
-//        Fblocks.push_back(typename Base::MatrixZD(this->keys_[i], Fi));
-//        E.block < 2, 2 > (2 * i, 0) = Ei;
-//        subInsert(b, bi, 2 * i);
-//      }
-//      return f;
     } else {
-      // nondegenerate: just return Base version
+      // valid result: just return Base version
-      Base::computeJacobians(Fblocks, E, b, cameras, point_);
+      Base::computeJacobians(Fblocks, E, b, cameras, *result_);
-    } // end else
+    }
+  }
+
+  /// Version that takes values, and creates the point
+  bool triangulateAndComputeJacobians(
+      std::vector<Base::MatrixZD>& Fblocks, Matrix& E, Vector& b,
+      const Values& values) const {
+    Cameras cameras = this->cameras(values);
+    bool nonDegenerate = triangulateForLinearize(cameras);
+    if (nonDegenerate)
+      computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras);
+    return nonDegenerate;
   }
 
   /// takes values
   bool triangulateAndComputeJacobiansSVD(
-      std::vector<typename Base::MatrixZD>& Fblocks, Matrix& Enull, Vector& b,
+      std::vector<Base::MatrixZD>& Fblocks, Matrix& Enull, Vector& b,
       const Values& values) const {
-    typename Base::Cameras cameras;
+    Cameras cameras = this->cameras(values);
-    double good = computeCamerasAndTriangulate(values, cameras);
+    bool nonDegenerate = triangulateForLinearize(cameras);
-    if (good)
+    if (nonDegenerate)
-      return Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, point_);
+      Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, *result_);
-    return true;
+    return nonDegenerate;
   }
 
   /// Calculate vector of re-projection errors, before applying noise model
   Vector reprojectionErrorAfterTriangulation(const Values& values) const {
-    Cameras cameras;
+    Cameras cameras = this->cameras(values);
-    bool nonDegenerate = computeCamerasAndTriangulate(values, cameras);
+    bool nonDegenerate = triangulateForLinearize(cameras);
     if (nonDegenerate)
-      return Base::unwhitenedError(cameras, point_);
+      return Base::unwhitenedError(cameras, *result_);
     else
-      return zero(cameras.size() * 3);
+      return zero(cameras.size() * Base::ZDim);
   }
 
   /**
@@ -529,87 +540,61 @@ public:
   double totalReprojectionError(const Cameras& cameras,
       boost::optional<Point3> externalPoint = boost::none) const {
 
-    size_t nrCameras;
+    if (externalPoint)
-    if (externalPoint) {
+      result_ = TriangulationResult(*externalPoint);
-      nrCameras = this->keys_.size();
+    else
-      point_ = *externalPoint;
+      result_ = triangulateSafe(cameras);
-      degenerate_ = false;
-      cheiralityException_ = false;
-    } else {
-      nrCameras = this->triangulateSafe(cameras);
-    }
 
-    if (nrCameras < 2
+    if (result_)
-        || (!this->manageDegeneracy_
+      // All good, just use version in base class
-            && (this->cheiralityException_ || this->degenerate_))) {
+      return Base::totalReprojectionError(cameras, *result_);
+    else if (params_.degeneracyMode == HANDLE_INFINITY) {
+      throw(std::runtime_error("Backproject at infinity not implemented for SmartStereo."));
+//      // Otherwise, manage the exceptions with rotation-only factors
+//      const StereoPoint2& z0 = this->measured_.at(0);
+//      Unit3 backprojected; //= cameras.front().backprojectPointAtInfinity(z0);
+//
+//      return Base::totalReprojectionError(cameras, backprojected);
+    } else {
       // if we don't want to manage the exceptions we discard the factor
-      // std::cout << "In error evaluation: exception" << std::endl;
       return 0.0;
     }
-
-    if (this->cheiralityException_) { // if we want to manage the exceptions with rotation-only factors
-      std::cout
-          << "SmartProjectionHessianFactor: cheirality exception (this should not happen if CheiralityException is disabled)!"
-          << std::endl;
-      this->degenerate_ = true;
-    }
-
-    if (this->degenerate_) {
-      return 0.0; // TODO: this maybe should be zero?
-//      std::cout
-//          << "SmartProjectionHessianFactor: trying to manage degeneracy (this should not happen is manageDegeneracy is disabled)!"
-//          << std::endl;
-//      size_t i = 0;
-//      double overallError = 0;
-//      BOOST_FOREACH(const Camera& camera, cameras) {
-//        const StereoPoint2& zi = this->measured_.at(i);
-//        if (i == 0) // first pose
-//          this->point_ = camera.backprojectPointAtInfinity(zi); // 3D parametrization of point at infinity
-//        StereoPoint2 reprojectionError(
-//            camera.projectPointAtInfinity(this->point_) - zi);
-//        overallError += 0.5
-//            * this->noise_.at(i)->distance(reprojectionError.vector());
-//        i += 1;
-//      }
-//      return overallError;
-    } else {
-      // Just use version in base class
-      return Base::totalReprojectionError(cameras, point_);
-    }
   }
 
-  /// Cameras are computed in derived class
+  /// Calculate total reprojection error
-  virtual Cameras cameras(const Values& values) const = 0;
+  virtual double error(const Values& values) const {
+    if (this->active(values)) {
+      return totalReprojectionError(Base::cameras(values));
+    } else { // else of active flag
+      return 0.0;
+    }
+  }
 
   /** return the landmark */
-  boost::optional<Point3> point() const {
+    TriangulationResult point() const {
-    return point_;
+      return result_;
-  }
+    }
 
-  /** COMPUTE the landmark */
+    /** COMPUTE the landmark */
-  boost::optional<Point3> point(const Values& values) const {
+    TriangulationResult point(const Values& values) const {
-    triangulateSafe(values);
+      Cameras cameras = this->cameras(values);
-    return point_;
+      return triangulateSafe(cameras);
-  }
+    }
 
-  /** return the degenerate state */
+    /// Is result valid?
-  inline bool isDegenerate() const {
+    bool isValid() const {
-    return (cheiralityException_ || degenerate_);
+      return result_;
-  }
+    }
 
-  /** return the cheirality status flag */
+    /** return the degenerate state */
-  inline bool isPointBehindCamera() const {
+    bool isDegenerate() const {
-    return cheiralityException_;
+      return result_.degenerate();
-  }
+    }
-  /** return chirality verbosity */
-  inline bool verboseCheirality() const {
-    return verboseCheirality_;
-  }
 
-  /** return flag for throwing cheirality exceptions */
+    /** return the cheirality status flag */
-  inline bool throwCheirality() const {
+    bool isPointBehindCamera() const {
-    return throwCheirality_;
+      return result_.behindCamera();
-  }
+    }
 
 private:
 
@@ -618,15 +603,15 @@ private:
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
     ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
-    ar & BOOST_SERIALIZATION_NVP(throwCheirality_);
+    ar & BOOST_SERIALIZATION_NVP(params_.throwCheirality);
-    ar & BOOST_SERIALIZATION_NVP(verboseCheirality_);
+    ar & BOOST_SERIALIZATION_NVP(params_.verboseCheirality);
   }
 };
 
 /// traits
-template<class CALIBRATION>
+template<>
-struct traits<SmartStereoProjectionFactor<CALIBRATION> > : public Testable<
+struct traits<SmartStereoProjectionFactor > : public Testable<
-    SmartStereoProjectionFactor<CALIBRATION> > {
+    SmartStereoProjectionFactor> {
 };
 
 } // \ namespace gtsam

gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h

@@ -11,7 +11,7 @@
 
 /**
  * @file   SmartStereoProjectionPoseFactor.h
- * @brief  Produces an Hessian factors on POSES from monocular measurements of a single landmark
+ * @brief  Smart stereo factor on poses, assuming camera calibration is fixed
  * @author Luca Carlone
  * @author Chris Beall
  * @author Zsolt Kira
@@ -35,53 +35,40 @@ namespace gtsam {
  */
 
 /**
- * The calibration is known here. The factor only constraints poses (variable dimension is 6)
+ * This factor assumes that camera calibration is fixed, but each camera
+ * has its own calibration.
+ * The factor only constrains poses (variable dimension is 6).
+ * This factor requires that values contains the involved poses (Pose3).
  * @addtogroup SLAM
  */
-template<class CALIBRATION>
+class SmartStereoProjectionPoseFactor: public SmartStereoProjectionFactor {
-class SmartStereoProjectionPoseFactor: public SmartStereoProjectionFactor<CALIBRATION> {
-
-public:
-  /// Linearization mode: what factor to linearize to
-   enum LinearizationMode {
-     HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD
-   };
 
 protected:
 
-  LinearizationMode linearizeTo_;  ///< How to linearize the factor (HESSIAN, JACOBIAN_SVD, JACOBIAN_Q)
+  std::vector<boost::shared_ptr<Cal3_S2Stereo> > K_all_; ///< shared pointer to calibration object (one for each camera)
-
-  std::vector<boost::shared_ptr<CALIBRATION> > K_all_; ///< shared pointer to calibration object (one for each camera)
 
 public:
 
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
   /// shorthand for base class type
-  typedef SmartStereoProjectionFactor<CALIBRATION> Base;
+  typedef SmartStereoProjectionFactor Base;
 
   /// shorthand for this class
-  typedef SmartStereoProjectionPoseFactor<CALIBRATION> This;
+  typedef SmartStereoProjectionPoseFactor This;
 
   /// shorthand for a smart pointer to a factor
   typedef boost::shared_ptr<This> shared_ptr;
 
   /**
    * Constructor
-   * @param rankTol tolerance used to check if point triangulation is degenerate
+   * @param Isotropic measurement noise
-   * @param linThreshold threshold on relative pose changes used to decide whether to relinearize (selective relinearization)
+   * @param params internal parameters of the smart factors
-   * @param manageDegeneracy is true, in presence of degenerate triangulation, the factor is converted to a rotation-only constraint,
-   * otherwise the factor is simply neglected
-   * @param enableEPI if set to true linear triangulation is refined with embedded LM iterations
    */
-  SmartStereoProjectionPoseFactor(const double rankTol = 1,
+  SmartStereoProjectionPoseFactor(const SharedNoiseModel& sharedNoiseModel,
-      const double linThreshold = -1, const bool manageDegeneracy = false,
+      const SmartStereoProjectionParams& params =
-      const bool enableEPI = false, LinearizationMode linearizeTo = HESSIAN,
+      SmartStereoProjectionParams()) :
-      double landmarkDistanceThreshold = 1e10,
+      Base(sharedNoiseModel, params) {
-      double dynamicOutlierRejectionThreshold = -1) :
-      Base(rankTol, linThreshold, manageDegeneracy, enableEPI,
-          landmarkDistanceThreshold, dynamicOutlierRejectionThreshold), linearizeTo_(
-          linearizeTo) {
   }
 
   /** Virtual destructor */
@@ -91,27 +78,23 @@ public:
    * add a new measurement and pose key
    * @param measured is the 2m dimensional location of the projection of a single landmark in the m view (the measurement)
    * @param poseKey is key corresponding to the camera observing the same landmark
-   * @param noise_i is the measurement noise
+   * @param K is the (fixed) camera calibration
-   * @param K_i is the (known) camera calibration
    */
-  void add(const StereoPoint2 measured_i, const Key poseKey_i,
+  void add(const StereoPoint2 measured, const Key poseKey,
-      const SharedNoiseModel noise_i,
+      const boost::shared_ptr<Cal3_S2Stereo> K) {
-      const boost::shared_ptr<CALIBRATION> K_i) {
+    Base::add(measured, poseKey);
-    Base::add(measured_i, poseKey_i, noise_i);
+    K_all_.push_back(K);
-    K_all_.push_back(K_i);
   }
 
   /**
    *  Variant of the previous one in which we include a set of measurements
    * @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 noises vector of measurement noises
    * @param Ks vector of calibration objects
    */
   void add(std::vector<StereoPoint2> measurements, std::vector<Key> poseKeys,
-      std::vector<SharedNoiseModel> noises,
+      std::vector<boost::shared_ptr<Cal3_S2Stereo> > Ks) {
-      std::vector<boost::shared_ptr<CALIBRATION> > Ks) {
+    Base::add(measurements, poseKeys);
-    Base::add(measurements, poseKeys, noises);
     for (size_t i = 0; i < measurements.size(); i++) {
       K_all_.push_back(Ks.at(i));
     }
@@ -121,13 +104,12 @@ 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 poseKeys vector of keys corresponding to the camera observing the same landmark
-   * @param noise measurement noise (same for all measurements)
    * @param K the (known) camera calibration (same for all measurements)
    */
   void add(std::vector<StereoPoint2> measurements, std::vector<Key> poseKeys,
-      const SharedNoiseModel noise, const boost::shared_ptr<CALIBRATION> K) {
+      const boost::shared_ptr<Cal3_S2Stereo> K) {
     for (size_t i = 0; i < measurements.size(); i++) {
-      Base::add(measurements.at(i), poseKeys.at(i), noise);
+      Base::add(measurements.at(i), poseKeys.at(i));
       K_all_.push_back(K);
     }
   }
@@ -140,57 +122,19 @@ public:
   void print(const std::string& s = "", const KeyFormatter& keyFormatter =
       DefaultKeyFormatter) const {
     std::cout << s << "SmartStereoProjectionPoseFactor, z = \n ";
-    BOOST_FOREACH(const boost::shared_ptr<CALIBRATION>& K, K_all_)
+    BOOST_FOREACH(const boost::shared_ptr<Cal3_S2Stereo>& K, K_all_)
     K->print("calibration = ");
     Base::print("", keyFormatter);
   }
 
   /// equals
   virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
-    const This *e = dynamic_cast<const This*>(&p);
+    const SmartStereoProjectionPoseFactor *e =
+        dynamic_cast<const SmartStereoProjectionPoseFactor*>(&p);
 
     return e && Base::equals(p, tol);
   }
 
-  /**
-   * Collect all cameras involved in this factor
-   * @param values Values structure which must contain camera poses corresponding
-   * to keys involved in this factor
-   * @return vector of Values
-   */
-  typename Base::Cameras cameras(const Values& values) const {
-    typename Base::Cameras cameras;
-    size_t i=0;
-    BOOST_FOREACH(const Key& k, this->keys_) {
-      Pose3 pose = values.at<Pose3>(k);
-      typename Base::Camera camera(pose, K_all_[i++]);
-      cameras.push_back(camera);
-    }
-    return cameras;
-  }
-
-  /**
-   * Linearize to Gaussian Factor
-   * @param values Values structure which must contain camera poses for this factor
-   * @return
-   */
-  virtual boost::shared_ptr<GaussianFactor> linearize(
-      const Values& values) const {
-    // depending on flag set on construction we may linearize to different linear factors
-    switch(linearizeTo_){
-    case JACOBIAN_SVD :
-      return this->createJacobianSVDFactor(cameras(values), 0.0);
-      break;
-    case JACOBIAN_Q :
-      throw("JacobianQ not working yet!");
-//      return this->createJacobianQFactor(cameras(values), 0.0);
-      break;
-    default:
-      return this->createHessianFactor(cameras(values));
-      break;
-    }
-  }
-
   /**
    * error calculates the error of the factor.
    */
@@ -203,10 +147,27 @@ public:
   }
 
   /** return the calibration object */
-  inline const std::vector<boost::shared_ptr<CALIBRATION> > calibration() const {
+  inline const std::vector<boost::shared_ptr<Cal3_S2Stereo> > calibration() const {
     return K_all_;
   }
 
+  /**
+   * Collect all cameras involved in this factor
+   * @param values Values structure which must contain camera poses corresponding
+   * to keys involved in this factor
+   * @return vector of Values
+   */
+   Base::Cameras cameras(const Values& values) const {
+    Base::Cameras cameras;
+    size_t i=0;
+    BOOST_FOREACH(const Key& k, this->keys_) {
+      const Pose3& pose = values.at<Pose3>(k);
+      StereoCamera camera(pose, K_all_[i++]);
+      cameras.push_back(camera);
+    }
+    return cameras;
+  }
+
 private:
 
   /// Serialization function
@@ -220,9 +181,9 @@ private:
 }; // end of class declaration
 
 /// traits
-template<class CALIBRATION>
+template<>
-struct traits<SmartStereoProjectionPoseFactor<CALIBRATION> > : public Testable<
+struct traits<SmartStereoProjectionPoseFactor> : public Testable<
-    SmartStereoProjectionPoseFactor<CALIBRATION> > {
+    SmartStereoProjectionPoseFactor> {
 };
 
 } // \ namespace gtsam

gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp

@@ -40,11 +40,10 @@ static double b = 1;
 static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fov, w, h, b));
 static Cal3_S2Stereo::shared_ptr K2(
     new Cal3_S2Stereo(1500, 1200, 0, 640, 480, b));
-static boost::shared_ptr<Cal3Bundler> Kbundler(
-    new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000));
 
-static double rankTol = 1.0;
+
-static double linThreshold = -1.0;
+static SmartStereoProjectionParams params;
+
 // static bool manageDegeneracy = true;
 // Create a noise model for the pixel error
 static SharedNoiseModel model(noiseModel::Isotropic::Sigma(3, 0.1));
@@ -63,8 +62,6 @@ 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));
 
-typedef SmartStereoProjectionPoseFactor<Cal3_S2Stereo> SmartFactor;
-
 vector<StereoPoint2> stereo_projectToMultipleCameras(const StereoCamera& cam1,
     const StereoCamera& cam2, const StereoCamera& cam3, Point3 landmark) {
 
@@ -80,37 +77,37 @@ vector<StereoPoint2> stereo_projectToMultipleCameras(const StereoCamera& cam1,
   return measurements_cam;
 }
 
-LevenbergMarquardtParams params;
+LevenbergMarquardtParams lm_params;
 
 /* ************************************************************************* */
 TEST( SmartStereoProjectionPoseFactor, Constructor) {
-  SmartFactor::shared_ptr factor1(new SmartFactor());
+  SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor(model));
 }
 
 /* ************************************************************************* */
 TEST( SmartStereoProjectionPoseFactor, Constructor2) {
-  SmartFactor factor1(rankTol, linThreshold);
+  SmartStereoProjectionPoseFactor factor1(model, params);
 }
 
 /* ************************************************************************* */
 TEST( SmartStereoProjectionPoseFactor, Constructor3) {
-  SmartFactor::shared_ptr factor1(new SmartFactor());
+  SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor(model));
-  factor1->add(measurement1, poseKey1, model, K);
+  factor1->add(measurement1, poseKey1, K);
 }
 
 /* ************************************************************************* */
 TEST( SmartStereoProjectionPoseFactor, Constructor4) {
-  SmartFactor factor1(rankTol, linThreshold);
+  SmartStereoProjectionPoseFactor factor1(model, params);
-  factor1.add(measurement1, poseKey1, model, K);
+  factor1.add(measurement1, poseKey1, K);
 }
 
 /* ************************************************************************* */
 TEST( SmartStereoProjectionPoseFactor, Equals ) {
-  SmartFactor::shared_ptr factor1(new SmartFactor());
+  SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor(model));
-  factor1->add(measurement1, poseKey1, model, K);
+  factor1->add(measurement1, poseKey1, K);
 
-  SmartFactor::shared_ptr factor2(new SmartFactor());
+  SmartStereoProjectionPoseFactor::shared_ptr factor2(new SmartStereoProjectionPoseFactor(model));
-  factor2->add(measurement1, poseKey1, model, K);
+  factor2->add(measurement1, poseKey1, K);
 
   CHECK(assert_equal(*factor1, *factor2));
 }
@@ -137,15 +134,15 @@ TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ) {
   values.insert(x1, level_pose);
   values.insert(x2, level_pose_right);
 
-  SmartFactor factor1;
+  SmartStereoProjectionPoseFactor factor1(model);
-  factor1.add(level_uv, x1, model, K2);
+  factor1.add(level_uv, x1, K2);
-  factor1.add(level_uv_right, x2, model, K2);
+  factor1.add(level_uv_right, x2, K2);
 
   double actualError = factor1.error(values);
   double expectedError = 0.0;
   EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7);
 
-  SmartFactor::Cameras cameras = factor1.cameras(values);
+  SmartStereoProjectionPoseFactor::Cameras cameras = factor1.cameras(values);
   double actualError2 = factor1.totalReprojectionError(cameras);
   EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7);
 
@@ -179,21 +176,17 @@ TEST( SmartStereoProjectionPoseFactor, noisy ) {
       Point3(0.5, 0.1, 0.3));
   values.insert(x2, level_pose_right.compose(noise_pose));
 
-  SmartFactor::shared_ptr factor1(new SmartFactor());
+  SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor(model));
-  factor1->add(level_uv, x1, model, K);
+  factor1->add(level_uv, x1, K);
-  factor1->add(level_uv_right, x2, model, K);
+  factor1->add(level_uv_right, x2, K);
 
   double actualError1 = factor1->error(values);
 
-  SmartFactor::shared_ptr factor2(new SmartFactor());
+  SmartStereoProjectionPoseFactor::shared_ptr factor2(new SmartStereoProjectionPoseFactor(model));
   vector<StereoPoint2> measurements;
   measurements.push_back(level_uv);
   measurements.push_back(level_uv_right);
 
-  vector<SharedNoiseModel> noises;
-  noises.push_back(model);
-  noises.push_back(model);
-
   vector<boost::shared_ptr<Cal3_S2Stereo> > Ks; ///< shared pointer to calibration object (one for each camera)
   Ks.push_back(K);
   Ks.push_back(K);
@@ -202,7 +195,7 @@ TEST( SmartStereoProjectionPoseFactor, noisy ) {
   views.push_back(x1);
   views.push_back(x2);
 
-  factor2->add(measurements, views, noises, Ks);
+  factor2->add(measurements, views, Ks);
 
   double actualError2 = factor2->error(values);
 
@@ -211,6 +204,7 @@ TEST( SmartStereoProjectionPoseFactor, noisy ) {
 
 /* *************************************************************************/
 TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) {
+
   // 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, K2);
@@ -229,11 +223,11 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) {
   Point3 landmark3(3, 0, 3.0);
 
   // 1. Project three landmarks into three cameras and triangulate
-  vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1,
+  vector<StereoPoint2> measurements_l1 = stereo_projectToMultipleCameras(cam1,
       cam2, cam3, landmark1);
-  vector<StereoPoint2> measurements_cam2 = stereo_projectToMultipleCameras(cam1,
+  vector<StereoPoint2> measurements_l2 = stereo_projectToMultipleCameras(cam1,
       cam2, cam3, landmark2);
-  vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1,
+  vector<StereoPoint2> measurements_l3 = stereo_projectToMultipleCameras(cam1,
       cam2, cam3, landmark3);
 
   vector<Key> views;
@@ -241,17 +235,21 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) {
   views.push_back(x2);
   views.push_back(x3);
 
-  SmartFactor::shared_ptr smartFactor1(new SmartFactor());
+  SmartStereoProjectionParams smart_params;
-  smartFactor1->add(measurements_cam1, views, model, K2);
+  smart_params.triangulation.enableEPI = true;
+  SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(model, smart_params));
+  smartFactor1->add(measurements_l1, views, K2);
 
-  SmartFactor::shared_ptr smartFactor2(new SmartFactor());
+  SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(model, smart_params));
-  smartFactor2->add(measurements_cam2, views, model, K2);
+  smartFactor2->add(measurements_l2, views, K2);
 
-  SmartFactor::shared_ptr smartFactor3(new SmartFactor());
+  SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(model, smart_params));
-  smartFactor3->add(measurements_cam3, views, model, K2);
+  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);
@@ -274,16 +272,24 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) {
                   -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598),
               Point3(0.1, -0.1, 1.9)), values.at<Pose3>(x3)));
 
-  EXPECT_DOUBLES_EQUAL(1888864, graph.error(values), 1);
+  //  cout << std::setprecision(10) << "\n----SmartStereoFactor graph initial error: " << graph.error(values) << endl;
+  EXPECT_DOUBLES_EQUAL(797312.95069157204, graph.error(values), 1e-7);
+
+  // get triangulated landmarks from smart factors
+  Point3 landmark1_smart = *smartFactor1->point();
+  Point3 landmark2_smart = *smartFactor2->point();
+  Point3 landmark3_smart = *smartFactor3->point();
 
   Values result;
   gttic_(SmartStereoProjectionPoseFactor);
-  LevenbergMarquardtOptimizer optimizer(graph, values, params);
+  LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
   result = optimizer.optimize();
   gttoc_(SmartStereoProjectionPoseFactor);
   tictoc_finishedIteration_();
 
-  EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-6);
+  EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-5);
+
+//  cout << std::setprecision(10) << "SmartStereoFactor graph optimized error: " << graph.error(result) << endl;
 
   GaussianFactorGraph::shared_ptr GFG = graph.linearize(result);
   VectorValues delta = GFG->optimize();
@@ -292,6 +298,51 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) {
 
   // result.print("results of 3 camera, 3 landmark optimization \n");
   EXPECT(assert_equal(pose3, result.at<Pose3>(x3)));
+
+  /* ***************************************************************
+   * Same problem with regular Stereo factors, expect same error!
+   * ****************************************************************/
+
+//  landmark1_smart.print("landmark1_smart");
+//  landmark2_smart.print("landmark2_smart");
+//  landmark3_smart.print("landmark3_smart");
+
+  // add landmarks to values
+  values.insert(L(1), landmark1_smart);
+  values.insert(L(2), landmark2_smart);
+  values.insert(L(3), landmark3_smart);
+
+  // add factors
+  NonlinearFactorGraph graph2;
+
+  graph2.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
+  graph2.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));
+
+  typedef GenericStereoFactor<Pose3, Point3> ProjectionFactor;
+
+  bool verboseCheirality = true;
+
+  graph2.push_back(ProjectionFactor(measurements_l1[0], model, x1, L(1), K2, false, verboseCheirality));
+  graph2.push_back(ProjectionFactor(measurements_l1[1], model, x2, L(1), K2, false, verboseCheirality));
+  graph2.push_back(ProjectionFactor(measurements_l1[2], model, x3, L(1), K2, false, verboseCheirality));
+
+  graph2.push_back(ProjectionFactor(measurements_l2[0], model, x1, L(2), K2, false, verboseCheirality));
+  graph2.push_back(ProjectionFactor(measurements_l2[1], model, x2, L(2), K2, false, verboseCheirality));
+  graph2.push_back(ProjectionFactor(measurements_l2[2], model, x3, L(2), K2, false, verboseCheirality));
+
+  graph2.push_back(ProjectionFactor(measurements_l3[0], model, x1, L(3), K2, false, verboseCheirality));
+  graph2.push_back(ProjectionFactor(measurements_l3[1], model, x2, L(3), K2, false, verboseCheirality));
+  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);
+
+  LevenbergMarquardtOptimizer optimizer2(graph2, values, lm_params);
+  Values result2 = optimizer2.optimize();
+  EXPECT_DOUBLES_EQUAL(0, graph2.error(result2), 1e-5);
+
+//  cout << std::setprecision(10) << "StereoFactor graph optimized error: " << graph2.error(result2) << endl;
+
 }
 
 /* *************************************************************************/
@@ -325,17 +376,17 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) {
   vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1,
       cam2, cam3, landmark3);
 
-  SmartFactor::shared_ptr smartFactor1(
+  SmartStereoProjectionParams params;
-      new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD));
+  params.setLinearizationMode(JACOBIAN_SVD);
-  smartFactor1->add(measurements_cam1, views, model, K);
 
-  SmartFactor::shared_ptr smartFactor2(
+  SmartStereoProjectionPoseFactor::shared_ptr smartFactor1( new SmartStereoProjectionPoseFactor(model, params));
-      new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD));
+  smartFactor1->add(measurements_cam1, views, K);
-  smartFactor2->add(measurements_cam2, views, model, K);
 
-  SmartFactor::shared_ptr smartFactor3(
+  SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(model, params));
-      new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD));
+  smartFactor2->add(measurements_cam2, views, K);
-  smartFactor3->add(measurements_cam3, views, model, K);
+
+  SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(model, params));
+  smartFactor3->add(measurements_cam3, views, K);
 
   const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
 
@@ -355,7 +406,7 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) {
   values.insert(x3, pose3 * noise_pose);
 
   Values result;
-  LevenbergMarquardtOptimizer optimizer(graph, values, params);
+  LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
   result = optimizer.optimize();
   EXPECT(assert_equal(pose3, result.at<Pose3>(x3)));
 }
@@ -363,7 +414,7 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) {
 /* *************************************************************************/
 TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) {
 
-  double excludeLandmarksFutherThanDist = 2;
+//  double excludeLandmarksFutherThanDist = 2;
 
   vector<Key> views;
   views.push_back(x1);
@@ -393,20 +444,18 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) {
   vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1,
       cam2, cam3, landmark3);
 
-  SmartFactor::shared_ptr smartFactor1(
+  SmartStereoProjectionParams params;
-      new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD,
+  params.setLinearizationMode(JACOBIAN_SVD);
-          excludeLandmarksFutherThanDist));
+  params.setLandmarkDistanceThreshold(2);
-  smartFactor1->add(measurements_cam1, views, model, K);
 
-  SmartFactor::shared_ptr smartFactor2(
+  SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(model, params));
-      new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD,
+  smartFactor1->add(measurements_cam1, views, K);
-          excludeLandmarksFutherThanDist));
-  smartFactor2->add(measurements_cam2, views, model, K);
 
-  SmartFactor::shared_ptr smartFactor3(
+  SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(model, params));
-      new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD,
+  smartFactor2->add(measurements_cam2, views, K);
-          excludeLandmarksFutherThanDist));
+
-  smartFactor3->add(measurements_cam3, views, model, K);
+  SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(model, params));
+  smartFactor3->add(measurements_cam3, views, K);
 
   const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
 
@@ -427,7 +476,7 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) {
 
   // All factors are disabled and pose should remain where it is
   Values result;
-  LevenbergMarquardtOptimizer optimizer(graph, values, params);
+  LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
   result = optimizer.optimize();
   EXPECT(assert_equal(values.at<Pose3>(x3), result.at<Pose3>(x3)));
 }
@@ -435,9 +484,6 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) {
 /* *************************************************************************/
 TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
 
-  double excludeLandmarksFutherThanDist = 1e10;
-  double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error
-
   vector<Key> views;
   views.push_back(x1);
   views.push_back(x2);
@@ -471,25 +517,26 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
 
   measurements_cam4.at(0) = measurements_cam4.at(0) + StereoPoint2(10, 10, 1); // add outlier
 
-  SmartFactor::shared_ptr smartFactor1(
+  SmartStereoProjectionParams params;
-      new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD,
+  params.setLinearizationMode(JACOBIAN_SVD);
-          excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
+  params.setDynamicOutlierRejectionThreshold(1);
-  smartFactor1->add(measurements_cam1, views, model, K);
 
-  SmartFactor::shared_ptr smartFactor2(
-      new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD,
-          excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
-  smartFactor2->add(measurements_cam2, views, model, K);
 
-  SmartFactor::shared_ptr smartFactor3(
+  SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(model, params));
-      new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD,
+  smartFactor1->add(measurements_cam1, views, K);
-          excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
-  smartFactor3->add(measurements_cam3, views, model, K);
 
-  SmartFactor::shared_ptr smartFactor4(
+  SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(model, params));
-      new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD,
+  smartFactor2->add(measurements_cam2, views, K);
-          excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
+
-  smartFactor4->add(measurements_cam4, views, model, K);
+  SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(model, params));
+  smartFactor3->add(measurements_cam3, views, K);
+
+  SmartStereoProjectionPoseFactor::shared_ptr smartFactor4(new SmartStereoProjectionPoseFactor(model, params));
+  smartFactor4->add(measurements_cam4, views, K);
+
+  // same as factor 4, but dynamic outlier rejection is off
+  SmartStereoProjectionPoseFactor::shared_ptr smartFactor4b(new SmartStereoProjectionPoseFactor(model));
+  smartFactor4b->add(measurements_cam4, views, K);
 
   const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
 
@@ -508,9 +555,25 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
   values.insert(x2, pose2);
   values.insert(x3, pose3);
 
-  // All factors are disabled and pose should remain where it is
+  EXPECT_DOUBLES_EQUAL(0, smartFactor1->error(values), 1e-9);
+  EXPECT_DOUBLES_EQUAL(0, smartFactor2->error(values), 1e-9);
+  EXPECT_DOUBLES_EQUAL(0, smartFactor3->error(values), 1e-9);
+  // zero error due to dynamic outlier rejection
+  EXPECT_DOUBLES_EQUAL(0, smartFactor4->error(values), 1e-9);
+
+  // dynamic outlier rejection is off
+  EXPECT_DOUBLES_EQUAL(6700, smartFactor4b->error(values), 1e-9);
+
+  // Factors 1-3 should have valid point, factor 4 should not
+  EXPECT(smartFactor1->point());
+  EXPECT(smartFactor2->point());
+  EXPECT(smartFactor3->point());
+  EXPECT(smartFactor4->point().degenerate());
+  EXPECT(smartFactor4b->point());
+
+  // Factor 4 is disabled, pose 3 stays put
   Values result;
-  LevenbergMarquardtOptimizer optimizer(graph, values, params);
+  LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
   result = optimizer.optimize();
   EXPECT(assert_equal(pose3, result.at<Pose3>(x3)));
 }
@@ -545,13 +608,13 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
 //  stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
 //  stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
 //
-//  SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, JACOBIAN_Q));
+//  SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(1, -1, false, false, JACOBIAN_Q));
 //  smartFactor1->add(measurements_cam1, views, model, K);
 //
-//  SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, JACOBIAN_Q));
+//  SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(1, -1, false, false, JACOBIAN_Q));
 //  smartFactor2->add(measurements_cam2, views, model, K);
 //
-//  SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, JACOBIAN_Q));
+//  SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(1, -1, false, false, JACOBIAN_Q));
 //  smartFactor3->add(measurements_cam3, views, model, K);
 //
 //  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@@ -571,7 +634,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
 //  values.insert(x3, pose3*noise_pose);
 //
 ////  Values result;
-//  LevenbergMarquardtOptimizer optimizer(graph, values, params);
+//  LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
 //  result = optimizer.optimize();
 //  EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
 //}
@@ -630,7 +693,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
 //  values.insert(L(2), landmark2);
 //  values.insert(L(3), landmark3);
 //
-//  LevenbergMarquardtOptimizer optimizer(graph, values, params);
+//  LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
 //  Values result = optimizer.optimize();
 //
 //  EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
@@ -669,17 +732,17 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
   vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1,
       cam2, cam3, landmark3);
 
-  // Create smartfactors
+  SmartStereoProjectionParams params;
-  double rankTol = 10;
+  params.setRankTolerance(10);
 
-  SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol));
+  SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(model, params));
-  smartFactor1->add(measurements_cam1, views, model, K);
+  smartFactor1->add(measurements_cam1, views, K);
 
-  SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol));
+  SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(model, params));
-  smartFactor2->add(measurements_cam2, views, model, K);
+  smartFactor2->add(measurements_cam2, views, K);
 
-  SmartFactor::shared_ptr smartFactor3(new SmartFactor(rankTol));
+  SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(model, params));
-  smartFactor3->add(measurements_cam3, views, model, K);
+  smartFactor3->add(measurements_cam3, views, K);
 
   // Create graph to optimize
   NonlinearFactorGraph graph;
@@ -755,10 +818,10 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
 //  stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
 //
 //  double rankTol = 50;
-//  SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol, linThreshold, manageDegeneracy));
+//  SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(rankTol, linThreshold, manageDegeneracy));
 //  smartFactor1->add(measurements_cam1, views, model, K2);
 //
-//  SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol, linThreshold, manageDegeneracy));
+//  SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(rankTol, linThreshold, manageDegeneracy));
 //  smartFactor2->add(measurements_cam2, views, model, K2);
 //
 //  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@@ -781,7 +844,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
 //
 //  Values result;
 //  gttic_(SmartStereoProjectionPoseFactor);
-//  LevenbergMarquardtOptimizer optimizer(graph, values, params);
+//  LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
 //  result = optimizer.optimize();
 //  gttoc_(SmartStereoProjectionPoseFactor);
 //  tictoc_finishedIteration_();
@@ -824,13 +887,13 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
 //
 //  double rankTol = 10;
 //
-//  SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol, linThreshold, manageDegeneracy));
+//  SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(rankTol, linThreshold, manageDegeneracy));
 //  smartFactor1->add(measurements_cam1, views, model, K);
 //
-//  SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol, linThreshold, manageDegeneracy));
+//  SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(rankTol, linThreshold, manageDegeneracy));
 //  smartFactor2->add(measurements_cam2, views, model, K);
 //
-//  SmartFactor::shared_ptr smartFactor3(new SmartFactor(rankTol, linThreshold, manageDegeneracy));
+//  SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(rankTol, linThreshold, manageDegeneracy));
 //  smartFactor3->add(measurements_cam3, views, model, K);
 //
 //  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@@ -855,7 +918,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
 //
 //  Values result;
 //  gttic_(SmartStereoProjectionPoseFactor);
-//  LevenbergMarquardtOptimizer optimizer(graph, values, params);
+//  LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
 //  result = optimizer.optimize();
 //  gttoc_(SmartStereoProjectionPoseFactor);
 //  tictoc_finishedIteration_();
@@ -889,7 +952,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
 //  measurements_cam1.push_back(cam1_uv1);
 //  measurements_cam1.push_back(cam2_uv1);
 //
-//  SmartFactor::shared_ptr smartFactor1(new SmartFactor());
+//  SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor());
 //  smartFactor1->add(measurements_cam1,views, model, K2);
 //
 //  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3));
@@ -930,8 +993,8 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) {
   vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1,
       cam2, cam3, landmark1);
 
-  SmartFactor::shared_ptr smartFactorInstance(new SmartFactor());
+  SmartStereoProjectionPoseFactor::shared_ptr smartFactorInstance(new SmartStereoProjectionPoseFactor(model));
-  smartFactorInstance->add(measurements_cam1, views, model, K);
+  smartFactorInstance->add(measurements_cam1, views, K);
 
   Values values;
   values.insert(x1, pose1);
@@ -977,6 +1040,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) {
 
 /* *************************************************************************/
 TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) {
+
   vector<Key> views;
   views.push_back(x1);
   views.push_back(x2);
@@ -997,8 +1061,8 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) {
   vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1,
       cam2, cam3, landmark1);
 
-  SmartFactor::shared_ptr smartFactor(new SmartFactor());
+  SmartStereoProjectionPoseFactor::shared_ptr smartFactor(new SmartStereoProjectionPoseFactor(model));
-  smartFactor->add(measurements_cam1, views, model, K2);
+  smartFactor->add(measurements_cam1, views, K2);
 
   Values values;
   values.insert(x1, pose1);
@@ -1021,7 +1085,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) {
   // Hessian is invariant to rotations in the nondegenerate case
   EXPECT(
       assert_equal(hessianFactor->information(),
-          hessianFactorRot->information(), 1e-8));
+          hessianFactorRot->information(), 1e-6));
 
   Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2),
       Point3(10, -4, 5));
@@ -1037,7 +1101,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) {
   // Hessian is invariant to rotations and translations in the nondegenerate case
   EXPECT(
       assert_equal(hessianFactor->information(),
-          hessianFactorRotTran->information(), 1e-8));
+          hessianFactorRotTran->information(), 1e-6));
 }
 
 /* ************************************************************************* */