Merged in feature/TemplatedSmartFactors (pull request #39)

Templated Smart Factors

examples/SFMExample_SmartFactor.cpp

@@ -61,8 +61,7 @@ using namespace std;
 using namespace gtsam;
 
 // Make the typename short so it looks much cleaner
-typedef gtsam::SmartProjectionPoseFactor<gtsam::Pose3, gtsam::Point3,
+typedef gtsam::SmartProjectionPoseFactor<gtsam::Pose3, gtsam::Cal3_S2> SmartFactor;
-    gtsam::Cal3_S2> SmartFactor;
 
 /* ************************************************************************* */
 int main(int argc, char* argv[]) {

gtsam/geometry/Cal3_S2Stereo.h

@@ -52,6 +52,11 @@ namespace gtsam {
     /// constructor from vector
     Cal3_S2Stereo(const Vector &d): K_(d(0), d(1), d(2), d(3), d(4)), b_(d(5)){}
 
+    /// easy constructor; field-of-view in degrees, assumes zero skew
+    Cal3_S2Stereo(double fov, int w, int h, double b) :
+      K_(fov, w, h), b_(b) {
+    }
+
     /// @}
     /// @name Testable
     /// @{

gtsam/geometry/StereoCamera.cpp

@@ -30,7 +30,8 @@ namespace gtsam {
 
   /* ************************************************************************* */
   StereoPoint2 StereoCamera::project(const Point3& point,
-      boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
+      boost::optional<Matrix&> H1, boost::optional<Matrix&> H2,
+      boost::optional<Matrix&> H3) const {
 
 #ifdef STEREOCAMERA_CHAIN_RULE
     const Point3 q = leftCamPose_.transform_to(point, H1, H2);

gtsam/geometry/StereoCamera.h

@@ -114,21 +114,18 @@ public:
 
   /*
    * project 3D point and compute optional derivatives
+   * @param H1 derivative with respect to pose
+   * @param H2 derivative with respect to point
+   * @param H3 IGNORED (for calibration)
    */
   StereoPoint2 project(const Point3& point,
       boost::optional<Matrix&> H1 = boost::none,
-      boost::optional<Matrix&> H2 = boost::none) const;
+      boost::optional<Matrix&> H2 = boost::none,
+      boost::optional<Matrix&> H3 = boost::none) const;
 
-  /*
+  /**
-   * to accomodate tsam's assumption that K is estimated, too
+   *
    */
-  StereoPoint2 project(const Point3& point,
-      boost::optional<Matrix&> H1,
-      boost::optional<Matrix&> H1_k,
-      boost::optional<Matrix&> H2) const {
-    return project(point, H1, H2);
-  }
-
   Point3 backproject(const StereoPoint2& z) const {
     Vector measured = z.vector();
     double Z = K_->baseline()*K_->fx()/(measured[0]-measured[1]);

gtsam/geometry/StereoPoint2.cpp

@@ -19,10 +19,18 @@
 #include <gtsam/geometry/StereoPoint2.h>
 
 using namespace std;
-using namespace gtsam;
+
+namespace gtsam {
 
 /* ************************************************************************* */
 void StereoPoint2::print(const string& s) const {
   cout << s << "(" << uL_ << ", " << uR_ << ", " << v_ << ")"  << endl;
 }
+
 /* ************************************************************************* */
+ostream &operator<<(ostream &os, const StereoPoint2& p) {
+  os << '(' << p.uL() << ", " << p.uR() << ", " << p.v() << ')';
+  return os;
+}
+
+} // namespace gtsam

gtsam/geometry/StereoPoint2.h

@@ -143,15 +143,18 @@ namespace gtsam {
     }
 
     /** convenient function to get a Point2 from the left image */
-    inline Point2 point2(){
+    Point2 point2() const {
       return Point2(uL_, v_);
     }
 
     /** convenient function to get a Point2 from the right image */
-    inline Point2 right(){
+    Point2 right() const {
       return Point2(uR_, v_);
     }
 
+    /// Streaming
+    GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const StereoPoint2& p);
+
   private:
 
     /// @}

gtsam/slam/JacobianFactorQ.h

@@ -12,10 +12,10 @@ namespace gtsam {
 /**
  * JacobianFactor for Schur complement that uses Q noise model
  */
-template<size_t D>
+template<size_t D, size_t ZDim>
-class JacobianFactorQ: public JacobianSchurFactor<D> {
+class JacobianFactorQ: public JacobianSchurFactor<D, ZDim> {
 
-  typedef JacobianSchurFactor<D> Base;
+  typedef JacobianSchurFactor<D, ZDim> Base;
 
 public:
 
@@ -25,7 +25,7 @@ public:
 
   /// Empty constructor with keys
   JacobianFactorQ(const FastVector<Key>& keys,
-      const SharedDiagonal& model =  SharedDiagonal()) : JacobianSchurFactor<D>() {
+      const SharedDiagonal& model =  SharedDiagonal()) : JacobianSchurFactor<D, ZDim>() {
     Matrix zeroMatrix = Matrix::Zero(0,D);
     Vector zeroVector = Vector::Zero(0);
     typedef std::pair<Key, Matrix> KeyMatrix;
@@ -40,8 +40,8 @@ public:
   JacobianFactorQ(const std::vector<typename Base::KeyMatrix2D>& Fblocks,
       const Matrix& E, const Matrix3& P, const Vector& b,
       const SharedDiagonal& model = SharedDiagonal()) :
-      JacobianSchurFactor<D>() {
+      JacobianSchurFactor<D, ZDim>() {
-    size_t j = 0, m2 = E.rows(), m = m2 / 2;
+    size_t j = 0, m2 = E.rows(), m = m2 / ZDim;
     // Calculate projector Q
     Matrix Q = eye(m2) - E * P * E.transpose();
     // Calculate pre-computed Jacobian matrices
@@ -49,9 +49,9 @@ public:
     typedef std::pair<Key, Matrix> KeyMatrix;
     std::vector < KeyMatrix > QF;
     QF.reserve(m);
-    // Below, we compute each 2m*D block A_j = Q_j * F_j = (2m*2) * (2*D)
+    // Below, we compute each mZDim*D block A_j = Q_j * F_j = (mZDim*ZDim) * (Zdim*D)
     BOOST_FOREACH(const typename Base::KeyMatrix2D& it, Fblocks)
-      QF.push_back(KeyMatrix(it.first, Q.block(0, 2 * j++, m2, 2) * it.second));
+      QF.push_back(KeyMatrix(it.first, Q.block(0, ZDim * j++, m2, ZDim) * it.second));
     // Which is then passed to the normal JacobianFactor constructor
     JacobianFactor::fillTerms(QF, Q * b, model);
   }

gtsam/slam/JacobianFactorQR.h

@@ -12,10 +12,10 @@ namespace gtsam {
 /**
  * JacobianFactor for Schur complement that uses Q noise model
  */
-template<size_t D>
+template<size_t D, size_t ZDim>
-class JacobianFactorQR: public JacobianSchurFactor<D> {
+class JacobianFactorQR: public JacobianSchurFactor<D, ZDim> {
 
-  typedef JacobianSchurFactor<D> Base;
+  typedef JacobianSchurFactor<D, ZDim> Base;
 
 public:
 
@@ -25,14 +25,14 @@ public:
   JacobianFactorQR(const std::vector<typename Base::KeyMatrix2D>& Fblocks,
       const Matrix& E, const Matrix3& P, const Vector& b,
       const SharedDiagonal& model = SharedDiagonal()) :
-      JacobianSchurFactor<D>() {
+      JacobianSchurFactor<D, ZDim>() {
     // Create a number of Jacobian factors in a factor graph
     GaussianFactorGraph gfg;
     Symbol pointKey('p', 0);
     size_t i = 0;
     BOOST_FOREACH(const typename Base::KeyMatrix2D& it, Fblocks) {
-      gfg.add(pointKey, E.block<2, 3>(2 * i, 0), it.first, it.second,
+      gfg.add(pointKey, E.block<ZDim, 3>(ZDim * i, 0), it.first, it.second,
-          b.segment<2>(2 * i), model);
+          b.segment<ZDim>(ZDim * i), model);
       i += 1;
     }
     //gfg.print("gfg");

gtsam/slam/JacobianFactorSVD.h

@@ -11,12 +11,12 @@ namespace gtsam {
 /**
  * JacobianFactor for Schur complement that uses Q noise model
  */
-template<size_t D>
+template<size_t D, size_t ZDim>
-class JacobianFactorSVD: public JacobianSchurFactor<D> {
+class JacobianFactorSVD: public JacobianSchurFactor<D, ZDim> {
 
 public:
 
-  typedef Eigen::Matrix<double, 2, D> Matrix2D;
+  typedef Eigen::Matrix<double, ZDim, D> Matrix2D;   // e.g 2 x 6 with Z=Point2
   typedef std::pair<Key, Matrix2D> KeyMatrix2D;
   typedef std::pair<Key, Matrix> KeyMatrix;
 
@@ -25,7 +25,7 @@ public:
 
   /// Empty constructor with keys
   JacobianFactorSVD(const FastVector<Key>& keys,
-      const SharedDiagonal& model =  SharedDiagonal()) : JacobianSchurFactor<D>() {
+      const SharedDiagonal& model =  SharedDiagonal()) : JacobianSchurFactor<D, ZDim>() {
     Matrix zeroMatrix = Matrix::Zero(0,D);
     Vector zeroVector = Vector::Zero(0);
     std::vector<KeyMatrix> QF;
@@ -37,9 +37,9 @@ public:
 
   /// Constructor
   JacobianFactorSVD(const std::vector<KeyMatrix2D>& Fblocks, const Matrix& Enull, const Vector& b,
-      const SharedDiagonal& model =  SharedDiagonal()) : JacobianSchurFactor<D>() {
+      const SharedDiagonal& model =  SharedDiagonal()) : JacobianSchurFactor<D, ZDim>() {
-    size_t numKeys = Enull.rows() / 2;
+    size_t numKeys = Enull.rows() / ZDim;
-    size_t j = 0, m2 = 2*numKeys-3;
+    size_t j = 0, m2 = ZDim*numKeys-3;
     // PLAIN NULL SPACE TRICK
     // Matrix Q = Enull * Enull.transpose();
     // BOOST_FOREACH(const KeyMatrix2D& it, Fblocks)
@@ -48,7 +48,7 @@ public:
     std::vector<KeyMatrix> QF;
     QF.reserve(numKeys);
     BOOST_FOREACH(const KeyMatrix2D& it, Fblocks)
-    QF.push_back(KeyMatrix(it.first, (Enull.transpose()).block(0, 2 * j++, m2, 2) * it.second));
+    QF.push_back(KeyMatrix(it.first, (Enull.transpose()).block(0, ZDim * j++, m2, ZDim) * it.second));
     JacobianFactor::fillTerms(QF, Enull.transpose() * b, model);
   }
 };

gtsam/slam/JacobianSchurFactor.h

@@ -18,12 +18,12 @@ namespace gtsam {
 /**
  * JacobianFactor for Schur complement that uses Q noise model
  */
-template<size_t D>
+template<size_t D, size_t ZDim>
 class JacobianSchurFactor: public JacobianFactor {
 
 public:
 
-  typedef Eigen::Matrix<double, 2, D> Matrix2D;
+  typedef Eigen::Matrix<double, ZDim, D> Matrix2D;
   typedef std::pair<Key, Matrix2D> KeyMatrix2D;
 
   // Use eigen magic to access raw memory

gtsam/slam/SmartFactorBase.h

@@ -19,16 +19,16 @@
 
 #pragma once
 
-#include "JacobianFactorQ.h"
+#include <gtsam/slam/JacobianFactorQ.h>
-#include "JacobianFactorSVD.h"
+#include <gtsam/slam/JacobianFactorSVD.h>
-#include "RegularImplicitSchurFactor.h"
+#include <gtsam/slam/RegularImplicitSchurFactor.h>
-#include "RegularHessianFactor.h"
+#include <gtsam/slam/RegularHessianFactor.h>
 
 #include <gtsam/nonlinear/NonlinearFactor.h>
-#include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/geometry/PinholeCamera.h>  // for Cheirality exception
+#include <gtsam/geometry/StereoCamera.h>
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/inference/Symbol.h>
-#include <gtsam/slam/dataset.h>
 
 #include <boost/optional.hpp>
 #include <boost/make_shared.hpp>
@@ -36,40 +36,48 @@
 
 namespace gtsam {
 /// Base class with no internal point, completely functional
-template<class POSE, class CALIBRATION, size_t D>
+template<class POSE, class Z, class CAMERA, size_t D>
 class SmartFactorBase: public NonlinearFactor {
+
 protected:
 
   // Keep a copy of measurement and calibration for I/O
-  std::vector<Point2> measured_; ///< 2D measurement for each of the m views
+  std::vector<Z> measured_; ///< 2D measurement for each of the m views
   std::vector<SharedNoiseModel> noise_; ///< noise model used
   ///< (important that the order is the same as the keys that we use to create the factor)
 
   boost::optional<POSE> body_P_sensor_; ///< The pose of the sensor in the body frame (one for all cameras)
 
+  static const int ZDim = traits::dimension<Z>::value;    ///< Measurement dimension
+
   /// Definitions for blocks of F
-  typedef Eigen::Matrix<double, 2, D> Matrix2D; // F
+  typedef Eigen::Matrix<double, ZDim, D> Matrix2D; // F
-  typedef Eigen::Matrix<double, D, 2> MatrixD2; // F'
+  typedef Eigen::Matrix<double, D, ZDim> MatrixD2; // F'
   typedef std::pair<Key, Matrix2D> KeyMatrix2D; // Fblocks
   typedef Eigen::Matrix<double, D, D> MatrixDD; // camera hessian block
-  typedef Eigen::Matrix<double, 2, 3> Matrix23;
+  typedef Eigen::Matrix<double, ZDim, 3> Matrix23;
   typedef Eigen::Matrix<double, D, 1> VectorD;
-  typedef Eigen::Matrix<double, 2, 2> Matrix2;
+  typedef Eigen::Matrix<double, ZDim, ZDim> Matrix2;
 
   /// shorthand for base class type
   typedef NonlinearFactor Base;
 
   /// shorthand for this class
-  typedef SmartFactorBase<POSE, CALIBRATION, D> This;
+  typedef SmartFactorBase<POSE, Z, CAMERA, D> This;
+
 
 public:
 
+
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
   /// shorthand for a smart pointer to a factor
   typedef boost::shared_ptr<This> shared_ptr;
 
   /// shorthand for a pinhole camera
-  typedef PinholeCamera<CALIBRATION> Camera;
+  typedef CAMERA Camera;
-  typedef std::vector<Camera> Cameras;
+  typedef std::vector<CAMERA> Cameras;
 
   /**
    * Constructor
@@ -89,7 +97,7 @@ public:
    * @param poseKey is the index corresponding to the camera observing the landmark
    * @param noise_i is the measurement noise
    */
-  void add(const Point2& measured_i, const Key& poseKey_i,
+  void add(const Z& measured_i, const Key& poseKey_i,
       const SharedNoiseModel& noise_i) {
     this->measured_.push_back(measured_i);
     this->keys_.push_back(poseKey_i);
@@ -100,7 +108,7 @@ public:
    * variant of the previous add: adds a bunch of measurements, together with the camera keys and noises
    */
   // ****************************************************************************************************
-  void add(std::vector<Point2>& measurements, std::vector<Key>& poseKeys,
+  void add(std::vector<Z>& measurements, std::vector<Key>& poseKeys,
       std::vector<SharedNoiseModel>& noises) {
     for (size_t i = 0; i < measurements.size(); i++) {
       this->measured_.push_back(measurements.at(i));
@@ -113,7 +121,7 @@ public:
    * variant of the previous add: adds a bunch of measurements and uses the same noise model for all of them
    */
   // ****************************************************************************************************
-  void add(std::vector<Point2>& measurements, std::vector<Key>& poseKeys,
+  void add(std::vector<Z>& measurements, std::vector<Key>& poseKeys,
       const SharedNoiseModel& noise) {
     for (size_t i = 0; i < measurements.size(); i++) {
       this->measured_.push_back(measurements.at(i));
@@ -127,7 +135,8 @@ public:
    * The noise is assumed to be the same for all measurements
    */
   // ****************************************************************************************************
-  void add(const SfM_Track& trackToAdd, const SharedNoiseModel& noise) {
+  template<class SFM_TRACK>
+  void add(const SFM_TRACK& trackToAdd, const SharedNoiseModel& noise) {
     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);
@@ -136,7 +145,7 @@ public:
   }
 
   /** return the measurements */
-  const std::vector<Point2>& measured() const {
+  const std::vector<Z>& measured() const {
     return measured_;
   }
 
@@ -179,18 +188,18 @@ public:
   }
 
   // ****************************************************************************************************
-  /// Calculate vector of re-projection errors, before applying noise model
+//  /// Calculate vector of re-projection errors, before applying noise model
   Vector reprojectionError(const Cameras& cameras, const Point3& point) const {
 
-    Vector b = zero(2 * cameras.size());
+    Vector b = zero(ZDim * cameras.size());
 
     size_t i = 0;
-    BOOST_FOREACH(const Camera& camera, cameras) {
+    BOOST_FOREACH(const CAMERA& camera, cameras) {
-      const Point2& zi = this->measured_.at(i);
+      const Z& zi = this->measured_.at(i);
       try {
-        Point2 e(camera.project(point) - zi);
+        Z e(camera.project(point) - zi);
-        b[2 * i] = e.x();
+        b[ZDim * i] = e.x();
-        b[2 * i + 1] = e.y();
+        b[ZDim * i + 1] = e.y();
       } catch (CheiralityException& e) {
         std::cout << "Cheirality exception " << std::endl;
         exit(EXIT_FAILURE);
@@ -215,10 +224,10 @@ public:
     double overallError = 0;
 
     size_t i = 0;
-    BOOST_FOREACH(const Camera& camera, cameras) {
+    BOOST_FOREACH(const CAMERA& camera, cameras) {
-      const Point2& zi = this->measured_.at(i);
+      const Z& zi = this->measured_.at(i);
       try {
-        Point2 reprojectionError(camera.project(point) - zi);
+        Z reprojectionError(camera.project(point) - zi);
         overallError += 0.5
         * this->noise_.at(i)->distance(reprojectionError.vector());
       } catch (CheiralityException&) {
@@ -236,19 +245,19 @@ public:
       const Point3& point) const {
 
     int numKeys = this->keys_.size();
-    E = zeros(2 * numKeys, 3);
+    E = zeros(ZDim * numKeys, 3);
     Vector b = zero(2 * numKeys);
 
-    Matrix Ei(2, 3);
+    Matrix Ei(ZDim, 3);
     for (size_t i = 0; i < this->measured_.size(); i++) {
       try {
-        cameras[i].project(point, boost::none, Ei);
+        cameras[i].project(point, boost::none, Ei, boost::none);
       } catch (CheiralityException& e) {
         std::cout << "Cheirality exception " << std::endl;
         exit(EXIT_FAILURE);
       }
       this->noise_.at(i)->WhitenSystem(Ei, b);
-      E.block<2, 3>(2 * i, 0) = Ei;
+      E.block<ZDim, 3>(ZDim * i, 0) = Ei;
     }
 
     // Matrix PointCov;
@@ -262,11 +271,11 @@ public:
       Vector& b, const Cameras& cameras, const Point3& point) const {
 
     size_t numKeys = this->keys_.size();
-    E = zeros(2 * numKeys, 3);
+    E = zeros(ZDim * numKeys, 3);
-    b = zero(2 * numKeys);
+    b = zero(ZDim * numKeys);
     double f = 0;
 
-    Matrix Fi(2, 6), Ei(2, 3), Hcali(2, D - 6), Hcam(2, D);
+    Matrix Fi(ZDim, 6), Ei(ZDim, 3), Hcali(ZDim, D - 6), Hcam(ZDim, D);
     for (size_t i = 0; i < this->measured_.size(); i++) {
 
       Vector bi;
@@ -288,12 +297,12 @@ public:
       if (D == 6) { // optimize only camera pose
         Fblocks.push_back(KeyMatrix2D(this->keys_[i], Fi));
       } else {
-        Hcam.block<2, 6>(0, 0) = Fi; // 2 x 6 block for the cameras
+        Hcam.block<ZDim, 6>(0, 0) = Fi; // ZDim x 6 block for the cameras
-        Hcam.block<2, D - 6>(0, 6) = Hcali; // 2 x nrCal block for the cameras
+        Hcam.block<ZDim, D - 6>(0, 6) = Hcali; // ZDim x nrCal block for the cameras
         Fblocks.push_back(KeyMatrix2D(this->keys_[i], Hcam));
       }
-      E.block<2, 3>(2 * i, 0) = Ei;
+      E.block<ZDim, 3>(ZDim * i, 0) = Ei;
-      subInsert(b, bi, 2 * i);
+      subInsert(b, bi, ZDim * i);
     }
     return f;
   }
@@ -334,10 +343,10 @@ public:
     std::vector<KeyMatrix2D> Fblocks;
     double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point,
         lambda);
-    F = zeros(2 * numKeys, D * numKeys);
+    F = zeros(This::ZDim * numKeys, D * numKeys);
 
     for (size_t i = 0; i < this->keys_.size(); ++i) {
-      F.block<2, D>(2 * i, D * i) = Fblocks.at(i).second; // 2 x 6 block for the cameras
+      F.block<This::ZDim, D>(This::ZDim * i, D * i) = Fblocks.at(i).second; // ZDim x 6 block for the cameras
     }
     return f;
   }
@@ -356,9 +365,9 @@ public:
     // Do SVD on A
     Eigen::JacobiSVD<Matrix> svd(E, Eigen::ComputeFullU);
     Vector s = svd.singularValues();
-    // Enull = zeros(2 * numKeys, 2 * numKeys - 3);
+    // Enull = zeros(ZDim * numKeys, ZDim * numKeys - 3);
     size_t numKeys = this->keys_.size();
-    Enull = svd.matrixU().block(0, 3, 2 * numKeys, 2 * numKeys - 3); // last 2m-3 columns
+    Enull = svd.matrixU().block(0, 3, ZDim * numKeys, ZDim * numKeys - 3); // last ZDimm-3 columns
 
     return f;
   }
@@ -372,11 +381,11 @@ public:
     int numKeys = this->keys_.size();
     std::vector<KeyMatrix2D> Fblocks;
     double f = computeJacobiansSVD(Fblocks, Enull, b, cameras, point);
-    F.resize(2 * numKeys, D * numKeys);
+    F.resize(ZDim * numKeys, D * numKeys);
     F.setZero();
 
     for (size_t i = 0; i < this->keys_.size(); ++i)
-      F.block<2, D>(2 * i, D * i) = Fblocks.at(i).second; // 2 x 6 block for the cameras
+      F.block<ZDim, D>(ZDim * i, D * i) = Fblocks.at(i).second; // ZDim x 6 block for the cameras
 
     return f;
   }
@@ -437,9 +446,9 @@ public:
     int numKeys = this->keys_.size();
 
     /// Compute Full F ????
-    Matrix F = zeros(2 * numKeys, D * numKeys);
+    Matrix F = zeros(ZDim * numKeys, D * numKeys);
     for (size_t i = 0; i < this->keys_.size(); ++i)
-      F.block<2, D>(2 * i, D * i) = Fblocks.at(i).second; // 2 x 6 block for the cameras
+      F.block<ZDim, D>(ZDim * i, D * i) = Fblocks.at(i).second; // ZDim x 6 block for the cameras
 
     Matrix H(D * numKeys, D * numKeys);
     Vector gs_vector;
@@ -477,16 +486,16 @@ public:
     for (size_t i1 = 0; i1 < numKeys; i1++) { // for each camera
 
       const Matrix2D& Fi1 = Fblocks.at(i1).second;
-      const Matrix23 Ei1_P = E.block<2, 3>(2 * i1, 0) * P;
+      const Matrix23 Ei1_P = E.block<ZDim, 3>(ZDim * i1, 0) * P;
 
       // D = (Dx2) * (2)
       // (augmentedHessian.matrix()).block<D,1> (i1,numKeys+1) = Fi1.transpose() * b.segment < 2 > (2 * i1); // F' * b
-      augmentedHessian(i1, numKeys) = Fi1.transpose() * b.segment<2>(2 * i1) // F' * b
+      augmentedHessian(i1, numKeys) = Fi1.transpose() * b.segment<ZDim>(ZDim * i1) // F' * b
-      - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (Dx2) * (2x3) * (3*2m) * (2m x 1)
+      - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1)
 
-      // (DxD) = (Dx2) * ( (2xD) - (2x3) * (3x2) * (2xD) )
+      // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
       augmentedHessian(i1, i1) = Fi1.transpose()
-          * (Fi1 - Ei1_P * E.block<2, 3>(2 * i1, 0).transpose() * Fi1);
+          * (Fi1 - Ei1_P * E.block<ZDim, 3>(ZDim * i1, 0).transpose() * Fi1);
 
       // upper triangular part of the hessian
       for (size_t i2 = i1 + 1; i2 < numKeys; i2++) { // for each camera
@@ -494,7 +503,7 @@ public:
 
         // (DxD) = (Dx2) * ( (2x2) * (2xD) )
         augmentedHessian(i1, i2) = -Fi1.transpose()
-            * (Ei1_P * E.block<2, 3>(2 * i2, 0).transpose() * Fi2);
+            * (Ei1_P * E.block<ZDim, 3>(ZDim * i2, 0).transpose() * Fi2);
       }
     } // end of for over cameras
   }
@@ -521,16 +530,16 @@ public:
       // X  X  X X  14
       const Matrix2D& Fi1 = Fblocks.at(i1).second;
 
-      const Matrix23 Ei1_P = E.block<2, 3>(2 * i1, 0) * P;
+      const Matrix23 Ei1_P = E.block<ZDim, 3>(ZDim * i1, 0) * P;
 
       { // for i1 = i2
         // D = (Dx2) * (2)
-        gs.at(i1) = Fi1.transpose() * b.segment<2>(2 * i1) // F' * b
+        gs.at(i1) = Fi1.transpose() * b.segment<ZDim>(ZDim * i1) // F' * b
-                      -Fi1.transpose() * (Ei1_P * (E.transpose() * b));  // D = (Dx2) * (2x3) * (3*2m) * (2m x 1)
+                      -Fi1.transpose() * (Ei1_P * (E.transpose() * b));  // D = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1)
 
-        // (DxD) = (Dx2) * ( (2xD) - (2x3) * (3x2) * (2xD) )
+        // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
         Gs.at(GsIndex) = Fi1.transpose()
-            * (Fi1 - Ei1_P * E.block<2, 3>(2 * i1, 0).transpose() * Fi1);
+            * (Fi1 - Ei1_P * E.block<ZDim, 3>(ZDim * i1, 0).transpose() * Fi1);
         GsIndex++;
       }
       // upper triangular part of the hessian
@@ -539,7 +548,7 @@ public:
 
         // (DxD) = (Dx2) * ( (2x2) * (2xD) )
         Gs.at(GsIndex) = -Fi1.transpose()
-            * (Ei1_P * E.block<2, 3>(2 * i2, 0).transpose() * Fi2);
+            * (Ei1_P * E.block<ZDim, 3>(ZDim * i2, 0).transpose() * Fi2);
         GsIndex++;
       }
     } // end of for over cameras
@@ -587,9 +596,9 @@ public:
     for (size_t i1 = 0; i1 < numKeys; i1++) { // for each camera in the current factor
 
       const Matrix2D& Fi1 = Fblocks.at(i1).second;
-      const Matrix23 Ei1_P = E.block<2, 3>(2 * i1, 0) * P;
+      const Matrix23 Ei1_P = E.block<ZDim, 3>(ZDim * i1, 0) * P;
 
-      // D = (Dx2) * (2)
+      // D = (DxZDim) * (ZDim)
       // allKeys are the list of all camera keys in the group, e.g, (1,3,4,5,7)
       // we should map those to a slot in the local (grouped) hessian (0,1,2,3,4)
       // Key cameraKey_i1 = this->keys_[i1];
@@ -599,15 +608,15 @@ public:
       // vectorBlock = augmentedHessian(aug_i1, aug_numKeys).knownOffDiagonal();
       // add contribution of current factor
       augmentedHessian(aug_i1, aug_numKeys) = augmentedHessian(aug_i1, aug_numKeys).knownOffDiagonal()
-          + Fi1.transpose() * b.segment<2>(2 * i1) // F' * b
+          + Fi1.transpose() * b.segment<ZDim>(ZDim * i1) // F' * b
-      - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (Dx2) * (2x3) * (3*2m) * (2m x 1)
+      - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1)
 
-      // (DxD) = (Dx2) * ( (2xD) - (2x3) * (3x2) * (2xD) )
+      // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
       // main block diagonal - store previous block
       matrixBlock = augmentedHessian(aug_i1, aug_i1);
       // add contribution of current factor
       augmentedHessian(aug_i1, aug_i1) = matrixBlock +
-          (  Fi1.transpose() * (Fi1 - Ei1_P * E.block<2, 3>(2 * i1, 0).transpose() * Fi1)  );
+          (  Fi1.transpose() * (Fi1 - Ei1_P * E.block<ZDim, 3>(ZDim * i1, 0).transpose() * Fi1)  );
 
       // upper triangular part of the hessian
       for (size_t i2 = i1 + 1; i2 < numKeys; i2++) { // for each camera
@@ -616,12 +625,12 @@ public:
         //Key cameraKey_i2 = this->keys_[i2];
         DenseIndex aug_i2 = KeySlotMap[this->keys_[i2]];
 
-        // (DxD) = (Dx2) * ( (2x2) * (2xD) )
+        // (DxD) = (DxZDim) * ( (ZDimxZDim) * (ZDimxD) )
         // off diagonal block - store previous block
         // matrixBlock = augmentedHessian(aug_i1, aug_i2).knownOffDiagonal();
         // add contribution of current factor
         augmentedHessian(aug_i1, aug_i2) = augmentedHessian(aug_i1, aug_i2).knownOffDiagonal()
-            - Fi1.transpose() * (Ei1_P * E.block<2, 3>(2 * i2, 0).transpose() * Fi2);
+            - Fi1.transpose() * (Ei1_P * E.block<ZDim, 3>(ZDim * i2, 0).transpose() * Fi2);
       }
     } // end of for over cameras
 
@@ -641,7 +650,7 @@ public:
   }
 
   // ****************************************************************************************************
-  boost::shared_ptr<JacobianFactorQ<D> > createJacobianQFactor(
+  boost::shared_ptr<JacobianFactorQ<D, ZDim> > createJacobianQFactor(
       const Cameras& cameras, const Point3& point, double lambda = 0.0,
       bool diagonalDamping = false) const {
     std::vector<KeyMatrix2D> Fblocks;
@@ -650,7 +659,7 @@ public:
     Vector b;
     computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda,
         diagonalDamping);
-    return boost::make_shared<JacobianFactorQ<D> >(Fblocks, E, PointCov, b);
+    return boost::make_shared<JacobianFactorQ<D, ZDim> >(Fblocks, E, PointCov, b);
   }
 
   // ****************************************************************************************************
@@ -659,9 +668,9 @@ public:
     size_t numKeys = this->keys_.size();
     std::vector<KeyMatrix2D> Fblocks;
     Vector b;
-    Matrix Enull(2*numKeys, 2*numKeys-3);
+    Matrix Enull(ZDim*numKeys, ZDim*numKeys-3);
     computeJacobiansSVD(Fblocks, Enull, b, cameras, point, lambda);
-    return boost::make_shared< JacobianFactorSVD<6> >(Fblocks, Enull, b);
+    return boost::make_shared< JacobianFactorSVD<6, ZDim> >(Fblocks, Enull, b);
   }
 
 private:
@@ -676,4 +685,7 @@ private:
   }
 };
 
+template<class POSE, class Z, class CAMERA, size_t D>
+const int SmartFactorBase<POSE, Z, CAMERA, D>::ZDim;
+
 } // \ namespace gtsam

gtsam/slam/SmartProjectionFactor.h

@@ -19,7 +19,7 @@
 
 #pragma once
 
-#include "SmartFactorBase.h"
+#include <gtsam/slam/SmartFactorBase.h>
 
 #include <gtsam/geometry/triangulation.h>
 #include <gtsam/geometry/Pose3.h>
@@ -61,8 +61,8 @@ enum LinearizationMode {
  * SmartProjectionFactor: triangulates point
  * TODO: why LANDMARK parameter?
  */
-template<class POSE, class LANDMARK, class CALIBRATION, size_t D>
+template<class POSE, class CALIBRATION, size_t D>
-class SmartProjectionFactor: public SmartFactorBase<POSE, CALIBRATION, D> {
+class SmartProjectionFactor: public SmartFactorBase<POSE, gtsam::Point2, gtsam::PinholeCamera<CALIBRATION>, D> {
 protected:
 
   // Some triangulation parameters
@@ -92,7 +92,7 @@ protected:
   typedef boost::shared_ptr<SmartProjectionFactorState> SmartFactorStatePtr;
 
   /// shorthand for base class type
-  typedef SmartFactorBase<POSE, CALIBRATION, D> Base;
+  typedef SmartFactorBase<POSE, gtsam::Point2, gtsam::PinholeCamera<CALIBRATION>, D> Base;
 
   double landmarkDistanceThreshold_; // if the landmark is triangulated at a
   // distance larger than that the factor is considered degenerate
@@ -102,7 +102,9 @@ protected:
   // and the factor is disregarded if the error is large
 
   /// shorthand for this class
-  typedef SmartProjectionFactor<POSE, LANDMARK, CALIBRATION, D> This;
+  typedef SmartProjectionFactor<POSE, CALIBRATION, D> This;
+
+  static const int ZDim = traits::dimension<Point2>::value;    ///< Measurement dimension
 
 public:
 
@@ -418,16 +420,16 @@ public:
   }
 
   /// create factor
-  boost::shared_ptr<JacobianFactorQ<D> > createJacobianQFactor(
+  boost::shared_ptr<JacobianFactorQ<D, ZDim> > createJacobianQFactor(
       const Cameras& cameras, double lambda) const {
     if (triangulateForLinearize(cameras))
       return Base::createJacobianQFactor(cameras, point_, lambda);
     else
-      return boost::make_shared< JacobianFactorQ<D> >(this->keys_);
+      return boost::make_shared< JacobianFactorQ<D, ZDim> >(this->keys_);
   }
 
   /// Create a factor, takes values
-  boost::shared_ptr<JacobianFactorQ<D> > createJacobianQFactor(
+  boost::shared_ptr<JacobianFactorQ<D, ZDim> > createJacobianQFactor(
       const Values& values, double lambda) const {
     Cameras myCameras;
     // TODO triangulate twice ??
@@ -435,7 +437,7 @@ public:
     if (nonDegenerate)
       return createJacobianQFactor(myCameras, lambda);
     else
-      return boost::make_shared< JacobianFactorQ<D> >(this->keys_);
+      return boost::make_shared< JacobianFactorQ<D, ZDim> >(this->keys_);
   }
 
   /// different (faster) way to compute Jacobian factor
@@ -444,7 +446,7 @@ public:
     if (triangulateForLinearize(cameras))
       return Base::createJacobianSVDFactor(cameras, point_, lambda);
     else
-      return boost::make_shared< JacobianFactorSVD<D> >(this->keys_);
+      return boost::make_shared< JacobianFactorSVD<D, ZDim> >(this->keys_);
   }
 
   /// Returns true if nonDegenerate
@@ -707,4 +709,7 @@ private:
   }
 };
 
+template<class POSE, class CALIBRATION, size_t D>
+const int SmartProjectionFactor<POSE, CALIBRATION, D>::ZDim;
+
 } // \ namespace gtsam

gtsam/slam/SmartProjectionPoseFactor.h

@@ -19,7 +19,7 @@
 
 #pragma once
 
-#include "SmartProjectionFactor.h"
+#include <gtsam/slam/SmartProjectionFactor.h>
 
 namespace gtsam {
 /**
@@ -37,8 +37,8 @@ namespace gtsam {
  * The calibration is known here. The factor only constraints poses (variable dimension is 6)
  * @addtogroup SLAM
  */
-template<class POSE, class LANDMARK, class CALIBRATION>
+template<class POSE, class CALIBRATION>
-class SmartProjectionPoseFactor: public SmartProjectionFactor<POSE, LANDMARK, CALIBRATION, 6> {
+class SmartProjectionPoseFactor: public SmartProjectionFactor<POSE, CALIBRATION, 6> {
 protected:
 
   LinearizationMode linearizeTo_;  ///< How to linearize the factor (HESSIAN, JACOBIAN_SVD, JACOBIAN_Q)
@@ -48,10 +48,10 @@ protected:
 public:
 
   /// shorthand for base class type
-  typedef SmartProjectionFactor<POSE, LANDMARK, CALIBRATION, 6> Base;
+  typedef SmartProjectionFactor<POSE, CALIBRATION, 6> Base;
 
   /// shorthand for this class
-  typedef SmartProjectionPoseFactor<POSE, LANDMARK, CALIBRATION> This;
+  typedef SmartProjectionPoseFactor<POSE, CALIBRATION> This;
 
   /// shorthand for a smart pointer to a factor
   typedef boost::shared_ptr<This> shared_ptr;

gtsam/slam/tests/testRegularImplicitSchurFactor.cpp

@@ -114,7 +114,7 @@ TEST( regularImplicitSchurFactor, addHessianMultiply ) {
 
   // Create JacobianFactor with same error
   const SharedDiagonal model;
-  JacobianFactorQ<6> jf(Fblocks, E, P, b, model);
+  JacobianFactorQ<6, 2> jf(Fblocks, E, P, b, model);
 
   { // error
     double expectedError = jf.error(xvalues);
@@ -164,7 +164,7 @@ TEST( regularImplicitSchurFactor, addHessianMultiply ) {
   }
 
   // Create JacobianFactorQR
-  JacobianFactorQR<6> jfq(Fblocks, E, P, b, model);
+  JacobianFactorQR<6, 2> jfq(Fblocks, E, P, b, model);
   {
     const SharedDiagonal model;
     VectorValues yActual = zero;

gtsam/slam/tests/testSmartProjectionPoseFactor.cpp

@@ -60,8 +60,8 @@ static Key poseKey1(x1);
 static Point2 measurement1(323.0, 240.0);
 static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
 
-typedef SmartProjectionPoseFactor<Pose3,Point3,Cal3_S2> SmartFactor;
+typedef SmartProjectionPoseFactor<Pose3,Cal3_S2> SmartFactor;
-typedef SmartProjectionPoseFactor<Pose3,Point3,Cal3Bundler> SmartFactorBundler;
+typedef SmartProjectionPoseFactor<Pose3,Cal3Bundler> SmartFactorBundler;
 
 void projectToMultipleCameras(
     SimpleCamera cam1, SimpleCamera cam2, SimpleCamera cam3, Point3 landmark,
@@ -1202,7 +1202,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ){
 
 /* ************************************************************************* */
 TEST( SmartProjectionPoseFactor, ConstructorWithCal3Bundler) {
-  SmartProjectionPoseFactor<Pose3,Point3,Cal3Bundler> factor1(rankTol, linThreshold);
+  SmartProjectionPoseFactor<Pose3,Cal3Bundler> factor1(rankTol, linThreshold);
   boost::shared_ptr<Cal3Bundler> Kbundler(new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000));
   factor1.add(measurement1, poseKey1, model, Kbundler);
 }

gtsam_unstable/examples/SmartProjectionFactorExample.cpp

@@ -46,7 +46,7 @@ using namespace gtsam;
 
 int main(int argc, char** argv){
 
-  typedef SmartProjectionPoseFactor<Pose3, Point3, Cal3_S2> SmartFactor;
+  typedef SmartProjectionPoseFactor<Pose3, Cal3_S2> SmartFactor;
 
   Values initial_estimate;
   NonlinearFactorGraph graph;

gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp

@@ -0,0 +1,153 @@
+/* ----------------------------------------------------------------------------
+
+* GTSAM Copyright 2010, Georgia Tech Research Corporation,
+* Atlanta, Georgia 30332-0415
+* All Rights Reserved
+* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+* See LICENSE for the license information
+
+* -------------------------------------------------------------------------- */
+
+/**
+* @file SmartProjectionFactorExample.cpp
+* @brief A stereo visual odometry example
+* @date May 30, 2014
+* @author Stephen Camp
+* @author Chris Beall
+*/
+
+
+/**
+ * A smart projection factor example based on stereo data, throwing away the
+ * measurement from the right camera
+ *  -robot starts at origin
+ *  -moves forward, taking periodic stereo measurements
+ *  -makes monocular observations of many landmarks
+ */
+
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/geometry/Cal3_S2Stereo.h>
+#include <gtsam/nonlinear/Values.h>
+#include <gtsam/nonlinear/NonlinearEquality.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/slam/dataset.h>
+
+#include <gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h>
+
+#include <string>
+#include <fstream>
+#include <iostream>
+
+using namespace std;
+using namespace gtsam;
+
+int main(int argc, char** argv){
+
+  typedef SmartStereoProjectionPoseFactor<Pose3, Point3, Cal3_S2Stereo> SmartFactor;
+
+  bool output_poses = true;
+  bool output_initial_poses = true;
+  string poseOutput("../../../examples/data/optimized_poses.txt");
+  string init_poseOutput("../../../examples/data/initial_poses.txt");
+  Values initial_estimate;
+  NonlinearFactorGraph graph;
+  const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,1);
+  ofstream pose3Out, init_pose3Out;
+
+  bool add_initial_noise = true;
+
+  string calibration_loc = findExampleDataFile("VO_calibration.txt");
+  string pose_loc = findExampleDataFile("VO_camera_poses_large.txt");
+  string factor_loc = findExampleDataFile("VO_stereo_factors_large.txt");
+  
+  //read camera calibration info from file
+  // focal lengths fx, fy, skew s, principal point u0, v0, baseline b
+  cout << "Reading calibration info" << endl;
+  ifstream calibration_file(calibration_loc.c_str());
+
+  double fx, fy, s, u0, v0, b;
+  calibration_file >> fx >> fy >> s >> u0 >> v0 >> b;
+  const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fx, fy, s, u0, v0,b));
+
+  cout << "Reading camera poses" << endl;
+  ifstream pose_file(pose_loc.c_str());
+
+  int pose_id;
+  MatrixRowMajor m(4,4);
+  //read camera pose parameters and use to make initial estimates of camera poses
+  while (pose_file >> pose_id) {
+    for (int i = 0; i < 16; i++) {
+      pose_file >> m.data()[i];
+    }
+    if(add_initial_noise){
+      m(1,3) += (pose_id % 10)/10.0;
+    }
+    initial_estimate.insert(Symbol('x', pose_id), Pose3(m));
+  }
+
+  Values initial_pose_values = initial_estimate.filter<Pose3>();
+  if(output_poses){
+    init_pose3Out.open(init_poseOutput.c_str(),ios::out);
+    for(int i = 1; i<=initial_pose_values.size(); i++){
+      init_pose3Out << i << " " << initial_pose_values.at<Pose3>(Symbol('x',i)).matrix().format(Eigen::IOFormat(Eigen::StreamPrecision, 0,
+        " ", " ")) << endl;
+    }
+  }
+  
+  // camera and landmark keys
+  size_t x, l;
+
+  // pixel coordinates uL, uR, v (same for left/right images due to rectification)
+  // landmark coordinates X, Y, Z in camera frame, resulting from triangulation
+  double uL, uR, v, X, Y, Z;
+  ifstream factor_file(factor_loc.c_str());
+  cout << "Reading stereo factors" << endl;
+
+  //read stereo measurements and construct smart factors
+
+  SmartFactor::shared_ptr factor(new SmartFactor());
+  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());
+      current_l = l;
+    }
+    factor->add(StereoPoint2(uL,uR,v), Symbol('x',x), model, K);
+  }
+
+  Pose3 first_pose = initial_estimate.at<Pose3>(Symbol('x',1));
+  //constrain the first pose such that it cannot change from its original value during optimization
+  // NOTE: NonlinearEquality forces the optimizer to use QR rather than Cholesky
+  // QR is much slower than Cholesky, but numerically more stable
+  graph.push_back(NonlinearEquality<Pose3>(Symbol('x',1),first_pose));
+
+  LevenbergMarquardtParams params;
+  params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
+  params.verbosity = NonlinearOptimizerParams::ERROR;
+
+  cout << "Optimizing" << endl;
+  //create Levenberg-Marquardt optimizer to optimize the factor graph
+  LevenbergMarquardtOptimizer optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate, params);
+  Values result = optimizer.optimize();
+
+  cout << "Final result sample:" << endl;
+  Values pose_values = result.filter<Pose3>();
+  pose_values.print("Final camera poses:\n");
+
+  if(output_poses){
+    pose3Out.open(poseOutput.c_str(),ios::out);
+    for(int i = 1; i<=pose_values.size(); i++){
+      pose3Out << i << " " << pose_values.at<Pose3>(Symbol('x',i)).matrix().format(Eigen::IOFormat(Eigen::StreamPrecision, 0,
+        " ", " ")) << endl;
+    }
+    cout << "Writing output" << endl;
+  }
+
+  return 0;
+}

gtsam_unstable/slam/SmartStereoProjectionFactor.h

@@ -0,0 +1,748 @@
+/* ----------------------------------------------------------------------------
+ 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+ 
+ * See LICENSE for the license information
+ 
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file   SmartStereoProjectionFactor.h
+ * @brief  Base class to create smart factors on poses or cameras
+ * @author Luca Carlone
+ * @author Zsolt Kira
+ * @author Frank Dellaert
+ */
+
+#pragma once
+
+#include <gtsam/slam/SmartFactorBase.h>
+
+#include <gtsam/geometry/triangulation.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/geometry/StereoCamera.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/slam/dataset.h>
+
+#include <boost/optional.hpp>
+#include <boost/make_shared.hpp>
+#include <vector>
+
+namespace gtsam {
+
+/**
+ * Structure for storing some state memory, used to speed up optimization
+ * @addtogroup SLAM
+ */
+class SmartStereoProjectionFactorState {
+
+protected:
+
+public:
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  SmartStereoProjectionFactorState() {
+  }
+  // Hessian representation (after Schur complement)
+  bool calculatedHessian;
+  Matrix H;
+  Vector gs_vector;
+  std::vector<Matrix> Gs;
+  std::vector<Vector> gs;
+  double f;
+};
+
+enum LinearizationMode {
+  HESSIAN, JACOBIAN_SVD, JACOBIAN_Q
+};
+
+/**
+ * SmartStereoProjectionFactor: triangulates point
+ * TODO: why LANDMARK parameter?
+ */
+template<class POSE, class LANDMARK, class CALIBRATION, size_t D>
+class SmartStereoProjectionFactor: public SmartFactorBase<POSE, gtsam::StereoPoint2, gtsam::StereoCamera, D> {
+protected:
+
+  // Some triangulation parameters
+  const double rankTolerance_; ///< threshold to decide whether triangulation is degenerate_
+  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 bool enableEPI_; ///< if set to true, will refine triangulation using LM
+
+  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_;
+
+  // verbosity handling for Cheirality Exceptions
+  const bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false)
+  const bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false)
+
+  boost::shared_ptr<SmartStereoProjectionFactorState> state_;
+
+  /// shorthand for smart projection factor state variable
+  typedef boost::shared_ptr<SmartStereoProjectionFactorState> SmartFactorStatePtr;
+
+  /// shorthand for base class type
+  typedef SmartFactorBase<POSE, gtsam::StereoPoint2, gtsam::StereoCamera, D> Base;
+
+  double landmarkDistanceThreshold_; // if the landmark is triangulated at a
+  // distance larger than that the factor is considered degenerate
+
+  double dynamicOutlierRejectionThreshold_; // if this is nonnegative the factor will check if the
+  // average reprojection error is smaller than this threshold after triangulation,
+  // and the factor is disregarded if the error is large
+
+  /// shorthand for this class
+  typedef SmartStereoProjectionFactor<POSE, LANDMARK, CALIBRATION, D> This;
+
+  typedef traits::dimension<gtsam::StereoPoint2> ZDim_t;    ///< Dimension trait of measurement type
+
+public:
+
+  /// shorthand for a smart pointer to a factor
+  typedef boost::shared_ptr<This> shared_ptr;
+
+  /// shorthand for a StereoCamera // TODO: Get rid of this?
+  typedef StereoCamera Camera;
+
+  /// Vector of cameras
+  typedef std::vector<Camera> Cameras;
+
+  /**
+   * Constructor
+   * @param rankTol tolerance used to check if point triangulation is degenerate
+   * @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 rankTol, const double linThreshold,
+      const bool manageDegeneracy, const bool enableEPI,
+      boost::optional<POSE> body_P_sensor = boost::none,
+      double landmarkDistanceThreshold = 1e10,
+      double dynamicOutlierRejectionThreshold = -1,
+      SmartFactorStatePtr state = SmartFactorStatePtr(new SmartStereoProjectionFactorState())) :
+      Base(body_P_sensor), rankTolerance_(rankTol), retriangulationThreshold_(
+          1e-5), manageDegeneracy_(manageDegeneracy), enableEPI_(enableEPI), linearizationThreshold_(
+          linThreshold), degenerate_(false), cheiralityException_(false), throwCheirality_(
+          false), verboseCheirality_(false), state_(state),
+          landmarkDistanceThreshold_(landmarkDistanceThreshold),
+          dynamicOutlierRejectionThreshold_(dynamicOutlierRejectionThreshold) {
+  }
+
+  /** Virtual destructor */
+  virtual ~SmartStereoProjectionFactor() {
+  }
+
+  /**
+   * print
+   * @param s optional string naming the factor
+   * @param keyFormatter optional formatter useful for printing Symbols
+   */
+  void print(const std::string& s = "", const KeyFormatter& keyFormatter =
+      DefaultKeyFormatter) const {
+    std::cout << s << "SmartStereoProjectionFactor, z = \n";
+    std::cout << "rankTolerance_ = " << rankTolerance_ << std::endl;
+    std::cout << "degenerate_ = " << degenerate_ << std::endl;
+    std::cout << "cheiralityException_ = " << cheiralityException_ << std::endl;
+    std::cout << "linearizationThreshold_ = " << linearizationThreshold_ << std::endl;
+    Base::print("", keyFormatter);
+  }
+
+  /// 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
+    // Note that this is not yet "selecting linearization", that will come later, and we only check if the
+    // current linearization is the "same" (up to tolerance) w.r.t. the last time we triangulated the point_
+
+    size_t m = cameras.size();
+
+    bool retriangulate = false;
+
+    // if we do not have a previous linearization point_ or the new linearization point_ includes more poses
+    if (cameraPosesTriangulation_.empty()
+        || cameras.size() != cameraPosesTriangulation_.size())
+      retriangulate = true;
+
+    if (!retriangulate) {
+      for (size_t i = 0; i < cameras.size(); i++) {
+        if (!cameras[i].pose().equals(cameraPosesTriangulation_[i],
+            retriangulationThreshold_)) {
+          retriangulate = true; // at least two poses are different, hence we retriangulate
+          break;
+        }
+      }
+    }
+
+    if (retriangulate) { // we store the current poses used for triangulation
+      cameraPosesTriangulation_.clear();
+      cameraPosesTriangulation_.reserve(m);
+      for (size_t i = 0; i < m; i++)
+        // cameraPosesTriangulation_[i] = cameras[i].pose();
+        cameraPosesTriangulation_.push_back(cameras[i].pose());
+    }
+
+    return retriangulate; // if we arrive to this point_ all poses are the same and we don't need re-triangulation
+  }
+
+  /// This function checks if the new linearization point_ is 'close'  to the previous one used for linearization
+  bool decideIfLinearize(const Cameras& cameras) const {
+    // "selective linearization"
+    // The function evaluates how close are the old and the new poses, transformed in the ref frame of the first pose
+    // (we only care about the "rigidity" of the poses, not about their absolute pose)
+
+    if (this->linearizationThreshold_ < 0) //by convention if linearizationThreshold is negative we always relinearize
+      return true;
+
+    // if we do not have a previous linearization point_ or the new linearization point_ includes more poses
+    if (cameraPosesLinearization_.empty()
+        || (cameras.size() != cameraPosesLinearization_.size()))
+      return true;
+
+    Pose3 firstCameraPose, firstCameraPoseOld;
+    for (size_t i = 0; i < cameras.size(); i++) {
+
+      if (i == 0) { // we store the initial pose, this is useful for selective re-linearization
+        firstCameraPose = cameras[i].pose();
+        firstCameraPoseOld = cameraPosesLinearization_[i];
+        continue;
+      }
+
+      // we compare the poses in the frame of the first pose
+      Pose3 localCameraPose = firstCameraPose.between(cameras[i].pose());
+      Pose3 localCameraPoseOld = firstCameraPoseOld.between(
+          cameraPosesLinearization_[i]);
+      if (!localCameraPose.equals(localCameraPoseOld,
+          this->linearizationThreshold_))
+        return true; // at least two "relative" poses are different, hence we re-linearize
+    }
+    return false; // if we arrive to this point_ all poses are the same and we don't need re-linearize
+  }
+
+  /// triangulateSafe
+  size_t triangulateSafe(const Values& values) const {
+    return triangulateSafe(this->cameras(values));
+  }
+
+  /// triangulateSafe
+  size_t 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) {
+      // We triangulate the 3D position of the landmark
+      try {
+        // std::cout << "triangulatePoint3 i \n" << rankTolerance << std::endl;
+
+        //TODO: Chris will replace this with something else for stereo
+//        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<Cal3_S2>(mono_cameras, mono_measurements,
+            rankTolerance_, 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_) > 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(dynamicOutlierRejectionThreshold_ > 0 &&
+            totalReprojError/m > 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;
+  }
+
+  /// triangulate
+  bool triangulateForLinearize(const Cameras& cameras) const {
+
+    bool isDebug = false;
+    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<D> > createHessianFactor(
+      const Cameras& cameras, const double lambda = 0.0) const {
+
+    bool isDebug = false;
+    size_t numKeys = this->keys_.size();
+    // Create structures for Hessian Factors
+    std::vector < Key > js;
+    std::vector < Matrix > Gs(numKeys * (numKeys + 1) / 2);
+    std::vector < Vector > gs(numKeys);
+
+    if (this->measured_.size() != cameras.size()) {
+      std::cout
+          << "SmartProjectionHessianFactor: this->measured_.size() inconsistent with input"
+          << std::endl;
+      exit(1);
+    }
+
+    this->triangulateSafe(cameras);
+    if (isDebug) std::cout << "point_ = " << point_ << std::endl;
+
+    if (numKeys < 2
+        || (!this->manageDegeneracy_
+            && (this->cheiralityException_ || this->degenerate_))) {
+      if (isDebug) std::cout << "In linearize: exception" << std::endl;
+      BOOST_FOREACH(gtsam::Matrix& m, Gs)
+        m = zeros(D, D);
+      BOOST_FOREACH(Vector& v, gs)
+        v = zero(D);
+      return boost::make_shared<RegularHessianFactor<D> >(this->keys_, Gs, gs,
+          0.0);
+    }
+
+    // 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;
+      if (isDebug) std::cout << "degenerate_ = true" << std::endl;
+    }
+
+    bool doLinearize = this->decideIfLinearize(cameras);
+
+    if (isDebug) std::cout << "doLinearize = " << doLinearize << std::endl;
+
+    if (this->linearizationThreshold_ >= 0 && doLinearize) // 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();
+
+    if (!doLinearize) { // return the previous Hessian factor
+      std::cout << "=============================" << std::endl;
+      std::cout << "doLinearize " << doLinearize << std::endl;
+      std::cout << "this->linearizationThreshold_ "
+          << this->linearizationThreshold_ << std::endl;
+      std::cout << "this->degenerate_ " << this->degenerate_ << std::endl;
+      std::cout
+          << "something wrong in SmartProjectionHessianFactor: selective relinearization should be disabled"
+          << std::endl;
+      exit(1);
+      return boost::make_shared<RegularHessianFactor<D> >(this->keys_,
+          this->state_->Gs, this->state_->gs, this->state_->f);
+    }
+
+    // ==================================================================
+    Matrix F, E;
+    Matrix3 PointCov;
+    Vector b;
+    double f = computeJacobians(F, E, PointCov, b, cameras, lambda);
+
+    // Schur complement trick
+    // Frank says: should be possible to do this more efficiently?
+    // And we care, as in grouped factors this is called repeatedly
+    Matrix H(D * numKeys, D * numKeys);
+    Vector gs_vector;
+
+    H.noalias() = F.transpose() * (F - (E * (PointCov * (E.transpose() * F))));
+    gs_vector.noalias() = F.transpose()
+        * (b - (E * (PointCov * (E.transpose() * b))));
+
+    if (isDebug) std::cout << "gs_vector size " << gs_vector.size() << std::endl;
+    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 * D;
+      gs.at(i1) = gs_vector.segment < D > (i1D);
+      for (DenseIndex i2 = 0; i2 < (DenseIndex)numKeys; i2++) {
+        if (i2 >= i1) {
+          Gs.at(GsCount2) = H.block < D, D > (i1D, i2 * D);
+          GsCount2++;
+        }
+      }
+    }
+    // ==================================================================
+    if (this->linearizationThreshold_ >= 0) { // if we do not use selective relinearization we don't need to store these variables
+      this->state_->Gs = Gs;
+      this->state_->gs = gs;
+      this->state_->f = f;
+    }
+    return boost::make_shared<RegularHessianFactor<D> >(this->keys_, Gs, gs, f);
+  }
+
+//  // create factor
+//  boost::shared_ptr<ImplicitSchurFactor<D> > createImplicitSchurFactor(
+//      const Cameras& cameras, double lambda) const {
+//    if (triangulateForLinearize(cameras))
+//      return Base::createImplicitSchurFactor(cameras, point_, lambda);
+//    else
+//      return boost::shared_ptr<ImplicitSchurFactor<D> >();
+//  }
+//
+//  /// create factor
+//  boost::shared_ptr<JacobianFactorQ<D> > createJacobianQFactor(
+//      const Cameras& cameras, double lambda) const {
+//    if (triangulateForLinearize(cameras))
+//      return Base::createJacobianQFactor(cameras, point_, lambda);
+//    else
+//      return boost::make_shared< JacobianFactorQ<D> >(this->keys_);
+//  }
+//
+//  /// Create a factor, takes values
+//  boost::shared_ptr<JacobianFactorQ<D> > createJacobianQFactor(
+//      const Values& values, double lambda) const {
+//    Cameras myCameras;
+//    // TODO triangulate twice ??
+//    bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras);
+//    if (nonDegenerate)
+//      return createJacobianQFactor(myCameras, lambda);
+//    else
+//      return boost::make_shared< JacobianFactorQ<D> >(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);
+    else
+      return boost::make_shared< JacobianFactorSVD<D, ZDim_t::value> >(this->keys_);
+  }
+
+  /// Returns true if nonDegenerate
+  bool computeCamerasAndTriangulate(const Values& values,
+      Cameras& myCameras) const {
+    Values valuesFactor;
+
+    // Select only the cameras
+    BOOST_FOREACH(const Key key, this->keys_)
+      valuesFactor.insert(key, values.at(key));
+
+    myCameras = this->cameras(valuesFactor);
+    size_t nrCameras = this->triangulateSafe(myCameras);
+
+    if (nrCameras < 2
+        || (!this->manageDegeneracy_
+            && (this->cheiralityException_ || this->degenerate_)))
+      return false;
+
+    // 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;
+
+    if (this->degenerate_) {
+      std::cout << "SmartStereoProjectionFactor: this is not ready" << std::endl;
+      std::cout << "this->cheiralityException_ " << this->cheiralityException_
+          << std::endl;
+      std::cout << "this->degenerate_ " << this->degenerate_ << std::endl;
+    }
+    return true;
+  }
+
+  /// Takes values
+  bool computeEP(Matrix& E, Matrix& PointCov, const Values& values) const {
+    Cameras myCameras;
+    bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras);
+    if (nonDegenerate)
+      computeEP(E, PointCov, myCameras);
+    return nonDegenerate;
+  }
+
+  /// Assumes non-degenerate !
+  void computeEP(Matrix& E, Matrix& PointCov, const Cameras& cameras) const {
+    return Base::computeEP(E, PointCov, cameras, point_);
+  }
+
+  /// Version that takes values, and creates the point
+  bool computeJacobians(std::vector<typename Base::KeyMatrix2D>& Fblocks,
+      Matrix& E, Matrix& PointCov, Vector& b, const Values& values) const {
+    Cameras myCameras;
+    bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras);
+    if (nonDegenerate)
+      computeJacobians(Fblocks, E, PointCov, b, myCameras, 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
+  double computeJacobians(std::vector<typename Base::KeyMatrix2D>& Fblocks,
+      Matrix& E, Vector& b, const Cameras& cameras) const {
+    if (this->degenerate_) {
+      throw("FIXME: computeJacobians degenerate case commented out!");
+//      std::cout << "manage degeneracy " << manageDegeneracy_ << std::endl;
+//      std::cout << "point " << point_ << std::endl;
+//      std::cout
+//          << "SmartStereoProjectionFactor: Management of degeneracy is disabled - not ready to be used"
+//          << std::endl;
+//      if (D > 6) {
+//        std::cout
+//            << "Management of degeneracy is not yet ready when one also optimizes for the calibration "
+//            << std::endl;
+//      }
+//
+//      int numKeys = this->keys_.size();
+//      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::KeyMatrix2D(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
+      return Base::computeJacobians(Fblocks, E, b, cameras, point_);
+    } // end else
+  }
+
+//  /// Version that computes PointCov, with optional lambda parameter
+//  double computeJacobians(std::vector<typename Base::KeyMatrix2D>& Fblocks,
+//      Matrix& E, Matrix& PointCov, Vector& b, const Cameras& cameras,
+//      const double lambda = 0.0) const {
+//
+//    double f = computeJacobians(Fblocks, E, b, cameras);
+//
+//    // Point covariance inv(E'*E)
+//    PointCov.noalias() = (E.transpose() * E + lambda * eye(E.cols())).inverse();
+//
+//    return f;
+//  }
+//
+//  /// takes values
+//  bool computeJacobiansSVD(std::vector<typename Base::KeyMatrix2D>& Fblocks,
+//      Matrix& Enull, Vector& b, const Values& values) const {
+//    typename Base::Cameras myCameras;
+//    double good = computeCamerasAndTriangulate(values, myCameras);
+//    if (good)
+//      computeJacobiansSVD(Fblocks, Enull, b, myCameras);
+//    return true;
+//  }
+//
+//  /// SVD version
+//  double computeJacobiansSVD(std::vector<typename Base::KeyMatrix2D>& Fblocks,
+//      Matrix& Enull, Vector& b, const Cameras& cameras) const {
+//    return Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, point_);
+//  }
+//
+//  /// Returns Matrix, TODO: maybe should not exist -> not sparse !
+//  // TODO should there be a lambda?
+//  double computeJacobiansSVD(Matrix& F, Matrix& Enull, Vector& b,
+//      const Cameras& cameras) const {
+//    return Base::computeJacobiansSVD(F, Enull, b, cameras, point_);
+//  }
+
+  /// Returns Matrix, TODO: maybe should not exist -> not sparse !
+  double computeJacobians(Matrix& F, Matrix& E, Matrix3& PointCov, Vector& b,
+      const Cameras& cameras, const double lambda) const {
+    return Base::computeJacobians(F, E, PointCov, b, cameras, point_, lambda);
+  }
+
+  /// Calculate vector of re-projection errors, before applying noise model
+  /// Assumes triangulation was done and degeneracy handled
+  Vector reprojectionError(const Cameras& cameras) const {
+    return Base::reprojectionError(cameras, point_);
+  }
+
+  /// Calculate vector of re-projection errors, before applying noise model
+  Vector reprojectionError(const Values& values) const {
+    Cameras myCameras;
+    bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras);
+    if (nonDegenerate)
+      return reprojectionError(myCameras);
+    else
+      return zero(myCameras.size() * 3);
+  }
+
+  /**
+   * Calculate the error of the factor.
+   * This is the log-likelihood, e.g. \f$ 0.5(h(x)-z)^2/\sigma^2 \f$ in case of Gaussian.
+   * In this class, we take the raw prediction error \f$ h(x)-z \f$, ask the noise model
+   * to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5.
+   */
+  double totalReprojectionError(const Cameras& cameras,
+      boost::optional<Point3> externalPoint = boost::none) const {
+
+    size_t nrCameras;
+    if (externalPoint) {
+      nrCameras = this->keys_.size();
+      point_ = *externalPoint;
+      degenerate_ = false;
+      cheiralityException_ = false;
+    } else {
+      nrCameras = this->triangulateSafe(cameras);
+    }
+
+    if (nrCameras < 2
+        || (!this->manageDegeneracy_
+            && (this->cheiralityException_ || this->degenerate_))) {
+      // 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
+  virtual Cameras cameras(const Values& values) const = 0;
+
+  /** return the landmark */
+  boost::optional<Point3> point() const {
+    return point_;
+  }
+
+  /** COMPUTE the landmark */
+  boost::optional<Point3> point(const Values& values) const {
+    triangulateSafe(values);
+    return point_;
+  }
+
+  /** return the degenerate state */
+  inline bool isDegenerate() const {
+    return (cheiralityException_ || degenerate_);
+  }
+
+  /** return the cheirality status flag */
+  inline bool isPointBehindCamera() const {
+    return cheiralityException_;
+  }
+  /** return chirality verbosity */
+  inline bool verboseCheirality() const {
+    return verboseCheirality_;
+  }
+
+  /** return flag for throwing cheirality exceptions */
+  inline bool throwCheirality() const {
+    return throwCheirality_;
+  }
+
+private:
+
+  /// Serialization function
+  friend class boost::serialization::access;
+  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(verboseCheirality_);
+  }
+};
+
+} // \ namespace gtsam

gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h

@@ -0,0 +1,218 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file   SmartStereoProjectionPoseFactor.h
+ * @brief  Produces an Hessian factors on POSES from monocular measurements of a single landmark
+ * @author Luca Carlone
+ * @author Chris Beall
+ * @author Zsolt Kira
+ */
+
+#pragma once
+
+#include <gtsam_unstable/slam/SmartStereoProjectionFactor.h>
+
+namespace gtsam {
+/**
+ *
+ * @addtogroup SLAM
+ *
+ * If you are using the factor, please cite:
+ * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating conditionally
+ * independent sets in factor graphs: a unifying perspective based on smart factors,
+ * Int. Conf. on Robotics and Automation (ICRA), 2014.
+ *
+ */
+
+/**
+ * The calibration is known here. The factor only constraints poses (variable dimension is 6)
+ * @addtogroup SLAM
+ */
+template<class POSE, class LANDMARK, class CALIBRATION>
+class SmartStereoProjectionPoseFactor: public SmartStereoProjectionFactor<POSE, LANDMARK, CALIBRATION, 6> {
+protected:
+
+  LinearizationMode linearizeTo_;  ///< How to linearize the factor (HESSIAN, JACOBIAN_SVD, JACOBIAN_Q)
+
+  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<POSE, LANDMARK, CALIBRATION, 6> Base;
+
+  /// shorthand for this class
+  typedef SmartStereoProjectionPoseFactor<POSE, LANDMARK, CALIBRATION> 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 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)
+   */
+  SmartStereoProjectionPoseFactor(const double rankTol = 1,
+      const double linThreshold = -1, const bool manageDegeneracy = false,
+      const bool enableEPI = false, boost::optional<POSE> body_P_sensor = boost::none,
+      LinearizationMode linearizeTo = HESSIAN, double landmarkDistanceThreshold = 1e10,
+      double dynamicOutlierRejectionThreshold = -1) :
+        Base(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor,
+        landmarkDistanceThreshold, dynamicOutlierRejectionThreshold), linearizeTo_(linearizeTo) {}
+
+  /** Virtual destructor */
+  virtual ~SmartStereoProjectionPoseFactor() {}
+
+  /**
+   * 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_i is the (known) camera calibration
+   */
+  void add(const StereoPoint2 measured_i, const Key poseKey_i,
+      const SharedNoiseModel noise_i,
+      const boost::shared_ptr<CALIBRATION> K_i) {
+    Base::add(measured_i, poseKey_i, noise_i);
+    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<CALIBRATION> > Ks) {
+    Base::add(measurements, poseKeys, noises);
+    for (size_t i = 0; i < measurements.size(); i++) {
+      K_all_.push_back(Ks.at(i));
+    }
+  }
+
+  /**
+   * 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) {
+    for (size_t i = 0; i < measurements.size(); i++) {
+      Base::add(measurements.at(i), poseKeys.at(i), noise);
+      K_all_.push_back(K);
+    }
+  }
+
+  /**
+   * print
+   * @param s optional string naming the factor
+   * @param keyFormatter optional formatter useful for printing Symbols
+   */
+  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_)
+    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);
+
+    return e && Base::equals(p, tol);
+  }
+
+  /// get the dimension of the factor
+  virtual size_t dim() const {
+    return 6 * this->keys_.size();
+  }
+
+  /**
+   * 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.
+   */
+  virtual double error(const Values& values) const {
+    if (this->active(values)) {
+      return this->totalReprojectionError(cameras(values));
+    } else { // else of active flag
+      return 0.0;
+    }
+  }
+
+  /** return the calibration object */
+  inline const std::vector<boost::shared_ptr<CALIBRATION> > calibration() const {
+    return K_all_;
+  }
+
+private:
+
+  /// Serialization function
+  friend class boost::serialization::access;
+  template<class ARCHIVE>
+  void serialize(ARCHIVE & ar, const unsigned int version) {
+    ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
+    ar & BOOST_SERIALIZATION_NVP(K_all_);
+  }
+
+}; // end of class declaration
+
+} // \ namespace gtsam