Merged in feature/SmartFactors3 (pull request #124)

SmartFactors refactor phase 3

cmake/GtsamTesting.cmake

@@ -195,7 +195,9 @@ macro(gtsamAddTestsGlob_impl groupName globPatterns excludedFiles linkLibraries)
 			add_test(NAME ${target_name} COMMAND ${target_name})
 			add_dependencies(check.${groupName} ${target_name})
 			add_dependencies(check ${target_name})
-			add_dependencies(all.tests ${script_name})
+			if(NOT XCODE_VERSION)
+				add_dependencies(all.tests ${script_name})
+			endif()
 		
 			# Add TOPSRCDIR
 			set_property(SOURCE ${script_srcs} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"")

examples/Data/.gitignore

@@ -0,0 +1 @@
+*.txt

examples/SFMExample_SmartFactor.cpp

@@ -34,6 +34,9 @@ using namespace gtsam;
 // Make the typename short so it looks much cleaner
 typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor;
 
+// create a typedef to the camera type
+typedef PinholePose<Cal3_S2> Camera;
+
 /* ************************************************************************* */
 int main(int argc, char* argv[]) {
 
@@ -55,12 +58,12 @@ 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());
+    SmartFactor::shared_ptr smartfactor(new SmartFactor(K));
 
     for (size_t i = 0; i < poses.size(); ++i) {
 
       // generate the 2D measurement
-      SimpleCamera camera(poses[i], *K);
+      Camera camera(poses[i], K);
       Point2 measurement = camera.project(points[j]);
 
       // call add() function to add measurement into a single factor, here we need to add:
@@ -68,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, K);
+      smartfactor->add(measurement, i, measurementNoise);
     }
 
     // insert the smart factor in the graph
@@ -77,15 +80,15 @@ int main(int argc, char* argv[]) {
 
   // Add a prior on pose x0. This indirectly specifies where the origin is.
   // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
-  noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas(
+  noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas(
       (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished());
-  graph.push_back(PriorFactor<Pose3>(0, poses[0], poseNoise));
+  graph.push_back(PriorFactor<Camera>(0, Camera(poses[0],K), noise));
 
   // Because the structure-from-motion problem has a scale ambiguity, the problem is
   // still under-constrained. Here we add a prior on the second pose x1, so this will
   // fix the scale by indicating the distance between x0 and x1.
   // Because these two are fixed, the rest of the poses will be also be fixed.
-  graph.push_back(PriorFactor<Pose3>(1, poses[1], poseNoise)); // add directly to graph
+  graph.push_back(PriorFactor<Camera>(1, Camera(poses[1],K), noise)); // add directly to graph
 
   graph.print("Factor Graph:\n");
 
@@ -94,7 +97,7 @@ int main(int argc, char* argv[]) {
   Values initialEstimate;
   Pose3 delta(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
   for (size_t i = 0; i < poses.size(); ++i)
-    initialEstimate.insert(i, poses[i].compose(delta));
+    initialEstimate.insert(i, Camera(poses[i].compose(delta), K));
   initialEstimate.print("Initial Estimates:\n");
 
   // Optimize the graph and print results

examples/SFMExample_SmartFactorPCG.cpp

@@ -30,6 +30,9 @@ using namespace gtsam;
 // Make the typename short so it looks much cleaner
 typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor;
 
+// create a typedef to the camera type
+typedef PinholePose<Cal3_S2> Camera;
+
 /* ************************************************************************* */
 int main(int argc, char* argv[]) {
 
@@ -51,16 +54,16 @@ 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());
+    SmartFactor::shared_ptr smartfactor(new SmartFactor(K));
 
     for (size_t i = 0; i < poses.size(); ++i) {
 
       // generate the 2D measurement
-      SimpleCamera camera(poses[i], *K);
+      Camera camera(poses[i], K);
       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, K);
+      smartfactor->add(measurement, i, measurementNoise);
     }
 
     // insert the smart factor in the graph
@@ -69,18 +72,18 @@ int main(int argc, char* argv[]) {
 
   // Add a prior on pose x0. This indirectly specifies where the origin is.
   // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
-  noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas(
+  noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas(
       (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished());
-  graph.push_back(PriorFactor<Pose3>(0, poses[0], poseNoise));
+  graph.push_back(PriorFactor<Camera>(0, Camera(poses[0],K), noise));
 
   // Fix the scale ambiguity by adding a prior
-  graph.push_back(PriorFactor<Pose3>(1, poses[1], poseNoise));
+  graph.push_back(PriorFactor<Camera>(1, Camera(poses[0],K), noise));
 
   // Create the initial estimate to the solution
   Values initialEstimate;
   Pose3 delta(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
   for (size_t i = 0; i < poses.size(); ++i)
-    initialEstimate.insert(i, poses[i].compose(delta));
+    initialEstimate.insert(i, Camera(poses[i].compose(delta),K));
 
   // We will use LM in the outer optimization loop, but by specifying "Iterative" below
   // We indicate that an iterative linear solver should be used.

gtsam.h

@@ -812,7 +812,7 @@ class CalibratedCamera {
 
   // Action on Point3
   gtsam::Point2 project(const gtsam::Point3& point) const;
-  static gtsam::Point2 project_to_camera(const gtsam::Point3& cameraPoint);
+  static gtsam::Point2 Project(const gtsam::Point3& cameraPoint);
 
   // Standard Interface
   gtsam::Pose3 pose() const;
@@ -848,7 +848,7 @@ class PinholeCamera {
   static size_t Dim();
 
   // Transformations and measurement functions
-  static gtsam::Point2 project_to_camera(const gtsam::Point3& cameraPoint);
+  static gtsam::Point2 Project(const gtsam::Point3& cameraPoint);
   pair<gtsam::Point2,bool> projectSafe(const gtsam::Point3& pw) const;
   gtsam::Point2 project(const gtsam::Point3& point);
   gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const;
@@ -884,7 +884,7 @@ virtual class SimpleCamera {
   static size_t Dim();
 
   // Transformations and measurement functions
-  static gtsam::Point2 project_to_camera(const gtsam::Point3& cameraPoint);
+  static gtsam::Point2 Project(const gtsam::Point3& cameraPoint);
   pair<gtsam::Point2,bool> projectSafe(const gtsam::Point3& pw) const;
   gtsam::Point2 project(const gtsam::Point3& point);
   gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const;
@@ -2352,18 +2352,31 @@ virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor {
   void serialize() const;
 };
 
+#include <gtsam/slam/SmartProjectionFactor.h>
+class SmartProjectionParams {
+  SmartProjectionParams();
+  // TODO(frank): make these work:
+  //  void setLinearizationMode(LinearizationMode linMode);
+  //  void setDegeneracyMode(DegeneracyMode degMode);
+  void setRankTolerance(double rankTol);
+  void setEnableEPI(bool enableEPI);
+  void setLandmarkDistanceThreshold(bool landmarkDistanceThreshold);
+  void setDynamicOutlierRejectionThreshold(bool dynOutRejectionThreshold);
+};
+
 #include <gtsam/slam/SmartProjectionPoseFactor.h>
 template<CALIBRATION>
-virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor {
+virtual class SmartProjectionPoseFactor: gtsam::NonlinearFactor {
 
-  SmartProjectionPoseFactor(double rankTol, double linThreshold,
+  SmartProjectionPoseFactor(const CALIBRATION* K);
-      bool manageDegeneracy, bool enableEPI, const gtsam::Pose3& body_P_sensor);
+  SmartProjectionPoseFactor(const CALIBRATION* K,
-
+      const gtsam::Pose3& body_P_sensor);
-  SmartProjectionPoseFactor(double rankTol);
+  SmartProjectionPoseFactor(const CALIBRATION* K,
-  SmartProjectionPoseFactor();
+      const gtsam::Pose3& body_P_sensor,
+      const gtsam::SmartProjectionParams& params);
 
   void add(const gtsam::Point2& measured_i, size_t poseKey_i,
-      const gtsam::noiseModel::Base* noise_i, const CALIBRATION* K_i);
+      const gtsam::noiseModel::Base* noise_i);
 
   // enabling serialization functionality
   //void serialize() const;

gtsam/geometry/CalibratedCamera.cpp

@@ -85,8 +85,7 @@ const Pose3& PinholeBase::getPose(OptionalJacobian<6, 6> H) const {
 }
 
 /* ************************************************************************* */
-Point2 PinholeBase::project_to_camera(const Point3& pc,
+Point2 PinholeBase::Project(const Point3& pc, OptionalJacobian<2, 3> Dpoint) {
-    OptionalJacobian<2, 3> Dpoint) {
   double d = 1.0 / pc.z();
   const double u = pc.x() * d, v = pc.y() * d;
   if (Dpoint)
@@ -94,10 +93,22 @@ Point2 PinholeBase::project_to_camera(const Point3& pc,
   return Point2(u, v);
 }
 
+/* ************************************************************************* */
+Point2 PinholeBase::Project(const Unit3& pc, OptionalJacobian<2, 2> Dpoint) {
+  if (Dpoint) {
+    Matrix32 Dpoint3_pc;
+    Matrix23 Duv_point3;
+    Point2 uv = Project(pc.point3(Dpoint3_pc), Duv_point3);
+    *Dpoint = Duv_point3 * Dpoint3_pc;
+    return uv;
+  } else
+    return Project(pc.point3());
+}
+
 /* ************************************************************************* */
 pair<Point2, bool> PinholeBase::projectSafe(const Point3& pw) const {
   const Point3 pc = pose().transform_to(pw);
-  const Point2 pn = project_to_camera(pc);
+  const Point2 pn = Project(pc);
   return make_pair(pn, pc.z() > 0);
 }
 
@@ -109,9 +120,9 @@ Point2 PinholeBase::project2(const Point3& point, OptionalJacobian<2, 6> Dpose,
   const Point3 q = pose().transform_to(point, boost::none, Dpoint ? &Rt : 0);
 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
   if (q.z() <= 0)
-  throw CheiralityException();
+    throw CheiralityException();
 #endif
-  const Point2 pn = project_to_camera(q);
+  const Point2 pn = Project(q);
 
   if (Dpose || Dpoint) {
     const double d = 1.0 / q.z();
@@ -123,6 +134,32 @@ Point2 PinholeBase::project2(const Point3& point, OptionalJacobian<2, 6> Dpose,
   return pn;
 }
 
+/* ************************************************************************* */
+Point2 PinholeBase::project2(const Unit3& pw, OptionalJacobian<2, 6> Dpose,
+    OptionalJacobian<2, 2> Dpoint) const {
+
+  // world to camera coordinate
+  Matrix23 Dpc_rot;
+  Matrix2 Dpc_point;
+  const Unit3 pc = pose().rotation().unrotate(pw, Dpose ? &Dpc_rot : 0,
+      Dpose ? &Dpc_point : 0);
+
+  // camera to normalized image coordinate
+  Matrix2 Dpn_pc;
+  const Point2 pn = Project(pc, Dpose || Dpoint ? &Dpn_pc : 0);
+
+  // chain the Jacobian matrices
+  if (Dpose) {
+    // only rotation is important
+    Matrix26 Dpc_pose;
+    Dpc_pose.setZero();
+    Dpc_pose.leftCols<3>() = Dpc_rot;
+    *Dpose = Dpn_pc * Dpc_pose; // 2x2 * 2x6
+  }
+  if (Dpoint)
+    *Dpoint = Dpn_pc * Dpc_point; // 2x2 * 2*2
+  return pn;
+}
 /* ************************************************************************* */
 Point3 PinholeBase::backproject_from_camera(const Point2& p,
     const double depth) {

gtsam/geometry/CalibratedCamera.h

@@ -44,6 +44,18 @@ public:
  */
 class GTSAM_EXPORT PinholeBase {
 
+public:
+
+  /** Pose Concept requirements */
+  typedef Rot3 Rotation;
+  typedef Point3 Translation;
+
+  /**
+   *  Some classes template on either PinholeCamera or StereoCamera,
+   *  and this typedef informs those classes what "project" returns.
+   */
+  typedef Point2 Measurement;
+
 private:
 
   Pose3 pose_; ///< 3D pose of camera
@@ -135,6 +147,16 @@ public:
     return pose_;
   }
 
+  /// get rotation
+  const Rot3& rotation() const {
+    return pose_.rotation();
+  }
+
+  /// get translation
+  const Point3& translation() const {
+    return pose_.translation();
+  }
+
   /// return pose, with derivative
   const Pose3& getPose(OptionalJacobian<6, 6> H) const;
 
@@ -147,14 +169,21 @@ public:
    * Does *not* throw a CheiralityException, even if pc behind image plane
    * @param pc point in camera coordinates
    */
-  static Point2 project_to_camera(const Point3& pc, //
+  static Point2 Project(const Point3& pc, //
       OptionalJacobian<2, 3> Dpoint = boost::none);
 
+  /**
+   * Project from 3D point at infinity in camera coordinates into image
+   * Does *not* throw a CheiralityException, even if pc behind image plane
+   * @param pc point in camera coordinates
+   */
+  static Point2 Project(const Unit3& pc, //
+      OptionalJacobian<2, 2> Dpoint = boost::none);
+
   /// Project a point into the image and check depth
   std::pair<Point2, bool> projectSafe(const Point3& pw) const;
 
-  /**
+  /** Project point into the image
-   * Project point into the image
    * Throws a CheiralityException if point behind image plane iff GTSAM_THROW_CHEIRALITY_EXCEPTION
    * @param point 3D point in world coordinates
    * @return the intrinsic coordinates of the projected point
@@ -162,9 +191,33 @@ public:
   Point2 project2(const Point3& point, OptionalJacobian<2, 6> Dpose =
       boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const;
 
+  /** Project point at infinity into the image
+   * Throws a CheiralityException if point behind image plane iff GTSAM_THROW_CHEIRALITY_EXCEPTION
+   * @param point 3D point in world coordinates
+   * @return the intrinsic coordinates of the projected point
+   */
+  Point2 project2(const Unit3& point,
+      OptionalJacobian<2, 6> Dpose = boost::none,
+      OptionalJacobian<2, 2> Dpoint = boost::none) const;
+
   /// backproject a 2-dimensional point to a 3-dimensional point at given depth
   static Point3 backproject_from_camera(const Point2& p, const double depth);
 
+  /// @}
+  /// @name Advanced interface
+  /// @{
+
+  /**
+   * Return the start and end indices (inclusive) of the translation component of the
+   * exponential map parameterization
+   * @return a pair of [start, end] indices into the tangent space vector
+   */
+  inline static std::pair<size_t, size_t> translationInterval() {
+    return std::make_pair(3, 5);
+  }
+
+  /// @}
+
 private:
 
   /** Serialization function */

gtsam/geometry/CameraSet.h

@@ -21,13 +21,14 @@
 #include <gtsam/geometry/Point3.h>
 #include <gtsam/geometry/CalibratedCamera.h> // for Cheirality exception
 #include <gtsam/base/Testable.h>
+#include <gtsam/base/SymmetricBlockMatrix.h>
+#include <gtsam/base/FastMap.h>
 #include <vector>
 
 namespace gtsam {
 
 /**
  * @brief A set of cameras, all with their own calibration
- * Assumes that a camera is laid out as 6 Pose3 parameters then calibration
  */
 template<class CAMERA>
 class CameraSet: public std::vector<CAMERA> {
@@ -40,28 +41,46 @@ protected:
    */
   typedef typename CAMERA::Measurement Z;
 
+  static const int D = traits<CAMERA>::dimension; ///< Camera dimension
   static const int ZDim = traits<Z>::dimension; ///< Measurement dimension
-  static const int Dim = traits<CAMERA>::dimension; ///< Camera dimension
+
+  /// Make a vector of re-projection errors
+  static Vector ErrorVector(const std::vector<Z>& predicted,
+      const std::vector<Z>& measured) {
+
+    // Check size
+    size_t m = predicted.size();
+    if (measured.size() != m)
+      throw std::runtime_error("CameraSet::errors: size mismatch");
+
+    // Project and fill error vector
+    Vector b(ZDim * m);
+    for (size_t i = 0, row = 0; i < m; i++, row += ZDim) {
+      Z e = predicted[i] - measured[i];
+      b.segment<ZDim>(row) = e.vector();
+    }
+    return b;
+  }
 
 public:
 
   /// Definitions for blocks of F
-  typedef Eigen::Matrix<double, ZDim, Dim> MatrixZD; // F
+  typedef Eigen::Matrix<double, ZDim, D> MatrixZD;
-  typedef std::pair<Key, MatrixZD> FBlock; // Fblocks
+  typedef std::vector<MatrixZD> FBlocks;
 
   /**
    * print
    * @param s optional string naming the factor
    * @param keyFormatter optional formatter useful for printing Symbols
    */
-  void print(const std::string& s = "") const {
+  virtual void print(const std::string& s = "") const {
     std::cout << s << "CameraSet, cameras = \n";
     for (size_t k = 0; k < this->size(); ++k)
-      this->at(k).print();
+      this->at(k).print(s);
   }
 
   /// equals
-  virtual bool equals(const CameraSet& p, double tol = 1e-9) const {
+  bool equals(const CameraSet& p, double tol = 1e-9) const {
     if (this->size() != p.size())
       return false;
     bool camerasAreEqual = true;
@@ -74,31 +93,227 @@ public:
   }
 
   /**
-   * Project a point, with derivatives in this, point, and calibration
+   * Project a point (possibly Unit3 at infinity), with derivatives
+   * Note that F is a sparse block-diagonal matrix, so instead of a large dense
+   * matrix this function returns the diagonal blocks.
    * throws CheiralityException
    */
-  std::vector<Z> project(const Point3& point, boost::optional<Matrix&> F =
+  template<class POINT>
-      boost::none, boost::optional<Matrix&> E = boost::none,
+  std::vector<Z> project2(const POINT& point, //
-      boost::optional<Matrix&> H = boost::none) const {
+      boost::optional<FBlocks&> Fs = boost::none, //
+      boost::optional<Matrix&> E = boost::none) const {
 
-    size_t nrCameras = this->size();
+    static const int N = FixedDimension<POINT>::value;
-    if (F) F->resize(ZDim * nrCameras, 6);
-    if (E) E->resize(ZDim * nrCameras, 3);
-    if (H && Dim > 6) H->resize(ZDim * nrCameras, Dim - 6);
-    std::vector<Z> z(nrCameras);
 
-    for (size_t i = 0; i < nrCameras; i++) {
+    // Allocate result
-      Eigen::Matrix<double, ZDim, 6> Fi;
+    size_t m = this->size();
-      Eigen::Matrix<double, ZDim, 3> Ei;
+    std::vector<Z> z(m);
-      Eigen::Matrix<double, ZDim, Dim - 6> Hi;
+
-      z[i] = this->at(i).project(point, F ? &Fi : 0, E ? &Ei : 0, H ? &Hi : 0);
+    // Allocate derivatives
-      if (F) F->block<ZDim, 6>(ZDim * i, 0) = Fi;
+    if (E)
-      if (E) E->block<ZDim, 3>(ZDim * i, 0) = Ei;
+      E->resize(ZDim * m, N);
-      if (H) H->block<ZDim, Dim - 6>(ZDim * i, 0) = Hi;
+    if (Fs)
+      Fs->resize(m);
+
+    // Project and fill derivatives
+    for (size_t i = 0; i < m; i++) {
+      MatrixZD Fi;
+      Eigen::Matrix<double, ZDim, N> Ei;
+      z[i] = this->at(i).project2(point, Fs ? &Fi : 0, E ? &Ei : 0);
+      if (Fs)
+        (*Fs)[i] = Fi;
+      if (E)
+        E->block<ZDim, N>(ZDim * i, 0) = Ei;
     }
+
     return z;
   }
 
+  /// Calculate vector [project2(point)-z] of re-projection errors
+  template<class POINT>
+  Vector reprojectionError(const POINT& point, const std::vector<Z>& measured,
+      boost::optional<FBlocks&> Fs = boost::none, //
+      boost::optional<Matrix&> E = boost::none) const {
+    return ErrorVector(project2(point, Fs, E), measured);
+  }
+
+  /**
+   * Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix
+   * G = F' * F - F' * E * P * E' * F
+   * g = F' * (b - E * P * E' * b)
+   * Fixed size version
+   */
+  template<int N> // N = 2 or 3
+  static SymmetricBlockMatrix SchurComplement(const FBlocks& Fs,
+      const Matrix& E, const Eigen::Matrix<double, N, N>& P, const Vector& b) {
+
+    // a single point is observed in m cameras
+    size_t m = Fs.size();
+
+    // Create a SymmetricBlockMatrix
+    size_t M1 = D * m + 1;
+    std::vector<DenseIndex> dims(m + 1); // this also includes the b term
+    std::fill(dims.begin(), dims.end() - 1, D);
+    dims.back() = 1;
+    SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1));
+
+    // Blockwise Schur complement
+    for (size_t i = 0; i < m; i++) { // for each camera
+
+      const MatrixZD& Fi = Fs[i];
+      const Eigen::Matrix<double, 2, N> Ei_P = //
+          E.block(ZDim * i, 0, ZDim, N) * P;
+
+      // D = (Dx2) * ZDim
+      augmentedHessian(i, m) = Fi.transpose() * b.segment<ZDim>(ZDim * i) // F' * b
+      - Fi.transpose() * (Ei_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1)
+
+      // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
+      augmentedHessian(i, i) = Fi.transpose()
+          * (Fi - Ei_P * E.block(ZDim * i, 0, ZDim, N).transpose() * Fi);
+
+      // upper triangular part of the hessian
+      for (size_t j = i + 1; j < m; j++) { // for each camera
+        const MatrixZD& Fj = Fs[j];
+
+        // (DxD) = (Dx2) * ( (2x2) * (2xD) )
+        augmentedHessian(i, j) = -Fi.transpose()
+            * (Ei_P * E.block(ZDim * j, 0, ZDim, N).transpose() * Fj);
+      }
+    } // end of for over cameras
+
+    augmentedHessian(m, m)(0, 0) += b.squaredNorm();
+    return augmentedHessian;
+  }
+
+  /// Computes Point Covariance P, with lambda parameter
+  template<int N> // N = 2 or 3
+  static void ComputePointCovariance(Eigen::Matrix<double, N, N>& P,
+      const Matrix& E, double lambda, bool diagonalDamping = false) {
+
+    Matrix EtE = E.transpose() * E;
+
+    if (diagonalDamping) { // diagonal of the hessian
+      EtE.diagonal() += lambda * EtE.diagonal();
+    } else {
+      DenseIndex n = E.cols();
+      EtE += lambda * Eigen::MatrixXd::Identity(n, n);
+    }
+
+    P = (EtE).inverse();
+  }
+
+  /// Computes Point Covariance P, with lambda parameter, dynamic version
+  static Matrix PointCov(const Matrix& E, const double lambda = 0.0,
+      bool diagonalDamping = false) {
+    if (E.cols() == 2) {
+      Matrix2 P2;
+      ComputePointCovariance(P2, E, lambda, diagonalDamping);
+      return P2;
+    } else {
+      Matrix3 P3;
+      ComputePointCovariance(P3, E, lambda, diagonalDamping);
+      return P3;
+    }
+  }
+
+  /**
+   * Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix
+   * Dynamic version
+   */
+  static SymmetricBlockMatrix SchurComplement(const FBlocks& Fblocks,
+      const Matrix& E, const Vector& b, const double lambda = 0.0,
+      bool diagonalDamping = false) {
+    SymmetricBlockMatrix augmentedHessian;
+    if (E.cols() == 2) {
+      Matrix2 P;
+      ComputePointCovariance(P, E, lambda, diagonalDamping);
+      augmentedHessian = SchurComplement(Fblocks, E, P, b);
+    } else {
+      Matrix3 P;
+      ComputePointCovariance(P, E, lambda, diagonalDamping);
+      augmentedHessian = SchurComplement(Fblocks, E, P, b);
+    }
+    return augmentedHessian;
+  }
+
+  /**
+   * Applies Schur complement (exploiting block structure) to get a smart factor on cameras,
+   * and adds the contribution of the smart factor to a pre-allocated augmented Hessian.
+   */
+  template<int N> // N = 2 or 3
+  static void UpdateSchurComplement(const FBlocks& Fs, const Matrix& E,
+      const Eigen::Matrix<double, N, N>& P, const Vector& b,
+      const FastVector<Key>& allKeys, const FastVector<Key>& keys,
+      /*output ->*/SymmetricBlockMatrix& augmentedHessian) {
+
+    assert(keys.size()==Fs.size());
+    assert(keys.size()<=allKeys.size());
+
+    FastMap<Key, size_t> KeySlotMap;
+    for (size_t slot = 0; slot < allKeys.size(); slot++)
+      KeySlotMap.insert(std::make_pair(allKeys[slot], slot));
+
+    // Schur complement trick
+    // G = F' * F - F' * E * P * E' * F
+    // g = F' * (b - E * P * E' * b)
+
+    Eigen::Matrix<double, D, D> matrixBlock;
+    typedef SymmetricBlockMatrix::Block Block; ///< A block from the Hessian matrix
+
+    // a single point is observed in m cameras
+    size_t m = Fs.size(); // cameras observing current point
+    size_t M = (augmentedHessian.rows() - 1) / D; // all cameras in the group
+    assert(allKeys.size()==M);
+
+    // Blockwise Schur complement
+    for (size_t i = 0; i < m; i++) { // for each camera in the current factor
+
+      const MatrixZD& Fi = Fs[i];
+      const Eigen::Matrix<double, 2, N> Ei_P = E.template block<ZDim, N>(
+          ZDim * i, 0) * P;
+
+      // 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_i = this->keys_[i];
+      DenseIndex aug_i = KeySlotMap.at(keys[i]);
+
+      // information vector - store previous vector
+      // vectorBlock = augmentedHessian(aug_i, aug_m).knownOffDiagonal();
+      // add contribution of current factor
+      augmentedHessian(aug_i, M) = augmentedHessian(aug_i, M).knownOffDiagonal()
+          + Fi.transpose() * b.segment<ZDim>(ZDim * i) // F' * b
+      - Fi.transpose() * (Ei_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1)
+
+      // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
+      // main block diagonal - store previous block
+      matrixBlock = augmentedHessian(aug_i, aug_i);
+      // add contribution of current factor
+      augmentedHessian(aug_i, aug_i) = matrixBlock
+          + (Fi.transpose()
+              * (Fi - Ei_P * E.template block<ZDim, N>(ZDim * i, 0).transpose() * Fi));
+
+      // upper triangular part of the hessian
+      for (size_t j = i + 1; j < m; j++) { // for each camera
+        const MatrixZD& Fj = Fs[j];
+
+        DenseIndex aug_j = KeySlotMap.at(keys[j]);
+
+        // (DxD) = (DxZDim) * ( (ZDimxZDim) * (ZDimxD) )
+        // off diagonal block - store previous block
+        // matrixBlock = augmentedHessian(aug_i, aug_j).knownOffDiagonal();
+        // add contribution of current factor
+        augmentedHessian(aug_i, aug_j) =
+            augmentedHessian(aug_i, aug_j).knownOffDiagonal()
+                - Fi.transpose()
+                    * (Ei_P * E.template block<ZDim, N>(ZDim * j, 0).transpose() * Fj);
+      }
+    } // end of for over cameras
+
+    augmentedHessian(M, M)(0, 0) += b.squaredNorm();
+  }
+
 private:
 
   /// Serialization function
@@ -109,6 +324,9 @@ private:
   }
 };
 
+template<class CAMERA>
+const int CameraSet<CAMERA>::D;
+
 template<class CAMERA>
 const int CameraSet<CAMERA>::ZDim;
 

gtsam/geometry/PinholeCamera.h

@@ -31,14 +31,6 @@ namespace gtsam {
 template<typename Calibration>
 class GTSAM_EXPORT PinholeCamera: public PinholeBaseK<Calibration> {
 
-public:
-
-  /**
-   *  Some classes template on either PinholeCamera or StereoCamera,
-   *  and this typedef informs those classes what "project" returns.
-   */
-  typedef Point2 Measurement;
-
 private:
 
   typedef PinholeBaseK<Calibration> Base; ///< base class has 3D pose as private member
@@ -153,7 +145,7 @@ public:
   const Pose3& getPose(OptionalJacobian<6, dimension> H) const {
     if (H) {
       H->setZero();
-      H->block(0, 0, 6, 6) = I_6x6;
+      H->template block<6, 6>(0, 0) = I_6x6;
     }
     return Base::pose();
   }
@@ -184,16 +176,15 @@ public:
     if ((size_t) d.size() == 6)
       return PinholeCamera(this->pose().retract(d), calibration());
     else
-      return PinholeCamera(this->pose().retract(d.head(6)),
+      return PinholeCamera(this->pose().retract(d.head<6>()),
           calibration().retract(d.tail(calibration().dim())));
   }
 
   /// return canonical coordinate
   VectorK6 localCoordinates(const PinholeCamera& T2) const {
     VectorK6 d;
-    // TODO: why does d.head<6>() not compile??
+    d.template head<6>() = this->pose().localCoordinates(T2.pose());
-    d.head(6) = this->pose().localCoordinates(T2.pose());
+    d.template tail<DimK>() = calibration().localCoordinates(T2.calibration());
-    d.tail(DimK) = calibration().localCoordinates(T2.calibration());
     return d;
   }
 
@@ -208,101 +199,34 @@ public:
 
   typedef Eigen::Matrix<double, 2, DimK> Matrix2K;
 
-  /** project a point from world coordinate to the image
+  /** Templated projection of a 3D point or a point at infinity into the image
-   *  @param pw is a point in world coordinates
+   *  @param pw either a Point3 or a Unit3, in world coordinates
-   *  @param Dpose is the Jacobian w.r.t. pose3
-   *  @param Dpoint is the Jacobian w.r.t. point3
-   *  @param Dcal is the Jacobian w.r.t. calibration
    */
-  Point2 project(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
+  template<class POINT>
-      OptionalJacobian<2, 3> Dpoint = boost::none,
+  Point2 _project2(const POINT& pw, OptionalJacobian<2, dimension> Dcamera,
-      OptionalJacobian<2, DimK> Dcal = boost::none) const {
+      OptionalJacobian<2, FixedDimension<POINT>::value> Dpoint) const {
-
+    // We just call 3-derivative version in Base
-    // project to normalized coordinates
-    const Point2 pn = PinholeBase::project2(pw, Dpose, Dpoint);
-
-    // uncalibrate to pixel coordinates
-    Matrix2 Dpi_pn;
-    const Point2 pi = calibration().uncalibrate(pn, Dcal,
-        Dpose || Dpoint ? &Dpi_pn : 0);
-
-    // If needed, apply chain rule
-    if (Dpose)
-      *Dpose = Dpi_pn * *Dpose;
-    if (Dpoint)
-      *Dpoint = Dpi_pn * *Dpoint;
-
-    return pi;
-  }
-
-  /** project a point at infinity from world coordinate to the image
-   *  @param pw is a point in the world coordinate (it is pw = lambda*[pw_x  pw_y  pw_z] with lambda->inf)
-   *  @param Dpose is the Jacobian w.r.t. pose3
-   *  @param Dpoint is the Jacobian w.r.t. point3
-   *  @param Dcal is the Jacobian w.r.t. calibration
-   */
-  Point2 projectPointAtInfinity(const Point3& pw, OptionalJacobian<2, 6> Dpose =
-      boost::none, OptionalJacobian<2, 2> Dpoint = boost::none,
-      OptionalJacobian<2, DimK> Dcal = boost::none) const {
-
-    if (!Dpose && !Dpoint && !Dcal) {
-      const Point3 pc = this->pose().rotation().unrotate(pw); // get direction in camera frame (translation does not matter)
-      const Point2 pn = Base::project_to_camera(pc); // project the point to the camera
-      return K_.uncalibrate(pn);
-    }
-
-    // world to camera coordinate
-    Matrix3 Dpc_rot, Dpc_point;
-    const Point3 pc = this->pose().rotation().unrotate(pw, Dpc_rot, Dpc_point);
-
-    Matrix36 Dpc_pose;
-    Dpc_pose.setZero();
-    Dpc_pose.leftCols<3>() = Dpc_rot;
-
-    // camera to normalized image coordinate
-    Matrix23 Dpn_pc; // 2*3
-    const Point2 pn = Base::project_to_camera(pc, Dpn_pc);
-
-    // uncalibration
-    Matrix2 Dpi_pn; // 2*2
-    const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn);
-
-    // chain the Jacobian matrices
-    const Matrix23 Dpi_pc = Dpi_pn * Dpn_pc;
-    if (Dpose)
-      *Dpose = Dpi_pc * Dpc_pose;
-    if (Dpoint)
-      *Dpoint = (Dpi_pc * Dpc_point).leftCols<2>(); // only 2dof are important for the point (direction-only)
-    return pi;
-  }
-
-  /** project a point from world coordinate to the image, fixed Jacobians
-   *  @param pw is a point in the world coordinate
-   */
-  Point2 project2(
-      const Point3& pw, //
-      OptionalJacobian<2, dimension> Dcamera = boost::none,
-      OptionalJacobian<2, 3> Dpoint = boost::none) const {
-
-    // project to normalized coordinates
     Matrix26 Dpose;
-    const Point2 pn = PinholeBase::project2(pw, Dpose, Dpoint);
+    Eigen::Matrix<double, 2, DimK> Dcal;
-
+    Point2 pi = Base::project(pw, Dcamera ? &Dpose : 0, Dpoint,
-    // uncalibrate to pixel coordinates
+        Dcamera ? &Dcal : 0);
-    Matrix2K Dcal;
-    Matrix2 Dpi_pn;
-    const Point2 pi = calibration().uncalibrate(pn, Dcamera ? &Dcal : 0,
-        Dcamera || Dpoint ? &Dpi_pn : 0);
-
-    // If needed, calculate derivatives
     if (Dcamera)
-      *Dcamera << Dpi_pn * Dpose, Dcal;
+      *Dcamera << Dpose, Dcal;
-    if (Dpoint)
-      *Dpoint = Dpi_pn * (*Dpoint);
-
     return pi;
   }
 
+  /// project a 3D point from world coordinates into the image
+  Point2 project2(const Point3& pw, OptionalJacobian<2, dimension> Dcamera =
+      boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const {
+    return _project2(pw, Dcamera, Dpoint);
+  }
+
+  /// project a point at infinity from world coordinates into the image
+  Point2 project2(const Unit3& pw, OptionalJacobian<2, dimension> Dcamera =
+      boost::none, OptionalJacobian<2, 2> Dpoint = boost::none) const {
+    return _project2(pw, Dcamera, Dpoint);
+  }
+
   /**
    * Calculate range to a landmark
    * @param point 3D location of landmark
@@ -350,7 +274,7 @@ public:
     if (Dother) {
       Dother->resize(1, 6 + CalibrationB::dimension);
       Dother->setZero();
-      Dother->block(0, 0, 1, 6) = Dother_;
+      Dother->block<1, 6>(0, 0) = Dother_;
     }
     return result;
   }

gtsam/geometry/PinholePose.h

@@ -30,14 +30,19 @@ namespace gtsam {
  * @addtogroup geometry
  * \nosubgrouping
  */
-template<typename Calibration>
+template<typename CALIBRATION>
 class GTSAM_EXPORT PinholeBaseK: public PinholeBase {
 
-  GTSAM_CONCEPT_MANIFOLD_TYPE(Calibration)
+private:
 
-public  :
+  GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION);
 
-  typedef Calibration CalibrationType;
+  // Get dimensions of calibration type at compile time
+  static const int DimK = FixedDimension<CALIBRATION>::value;
+
+public:
+
+  typedef CALIBRATION CalibrationType;
 
   /// @name Standard Constructors
   /// @{
@@ -67,7 +72,7 @@ public  :
   }
 
   /// return calibration
-  virtual const Calibration& calibration() const = 0;
+  virtual const CALIBRATION& calibration() const = 0;
 
   /// @}
   /// @name Transformations and measurement functions
@@ -80,27 +85,65 @@ public  :
     return pn;
   }
 
-  /** project a point from world coordinate to the image, fixed Jacobians
+  /** project a point from world coordinate to the image
    *  @param pw is a point in the world coordinates
    */
-  Point2 project2(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
+  Point2 project(const Point3& pw) const {
-      OptionalJacobian<2, 3> Dpoint = boost::none) const {
+    const Point2 pn = PinholeBase::project2(pw); // project to normalized coordinates
+    return calibration().uncalibrate(pn); // uncalibrate to pixel coordinates
+  }
+
+  /** project a point from world coordinate to the image
+   *  @param pw is a point at infinity in the world coordinates
+   */
+  Point2 project(const Unit3& pw) const {
+    const Unit3 pc = pose().rotation().unrotate(pw); // convert to camera frame
+    const Point2 pn = PinholeBase::Project(pc); // project to normalized coordinates
+    return calibration().uncalibrate(pn);  // uncalibrate to pixel coordinates
+  }
+
+  /** Templated projection of a point (possibly at infinity) from world coordinate to the image
+   *  @param pw is a 3D point or aUnit3 (point at infinity) in world coordinates
+   *  @param Dpose is the Jacobian w.r.t. pose3
+   *  @param Dpoint is the Jacobian w.r.t. point3
+   *  @param Dcal is the Jacobian w.r.t. calibration
+   */
+  template <class POINT>
+  Point2 _project(const POINT& pw, OptionalJacobian<2, 6> Dpose,
+      OptionalJacobian<2, FixedDimension<POINT>::value> Dpoint,
+      OptionalJacobian<2, DimK> Dcal) const {
 
     // project to normalized coordinates
     const Point2 pn = PinholeBase::project2(pw, Dpose, Dpoint);
 
     // uncalibrate to pixel coordinates
     Matrix2 Dpi_pn;
-    const Point2 pi = calibration().uncalibrate(pn, boost::none,
+    const Point2 pi = calibration().uncalibrate(pn, Dcal,
         Dpose || Dpoint ? &Dpi_pn : 0);
 
     // If needed, apply chain rule
-    if (Dpose) *Dpose = Dpi_pn * (*Dpose);
+    if (Dpose)
-    if (Dpoint) *Dpoint = Dpi_pn * (*Dpoint);
+    *Dpose = Dpi_pn * *Dpose;
+    if (Dpoint)
+    *Dpoint = Dpi_pn * *Dpoint;
 
     return pi;
   }
 
+  /// project a 3D point from world coordinates into the image
+  Point2 project(const Point3& pw, OptionalJacobian<2, 6> Dpose,
+      OptionalJacobian<2, 3> Dpoint = boost::none,
+      OptionalJacobian<2, DimK> Dcal = boost::none) const {
+    return _project(pw, Dpose, Dpoint, Dcal);
+  }
+
+  /// project a point at infinity from world coordinates into the image
+  Point2 project(const Unit3& pw, OptionalJacobian<2, 6> Dpose,
+      OptionalJacobian<2, 2> Dpoint = boost::none,
+      OptionalJacobian<2, DimK> Dcal = boost::none) const {
+    return _project(pw, Dpose, Dpoint, Dcal);
+  }
+
   /// backproject a 2-dimensional point to a 3-dimensional point at given depth
   Point3 backproject(const Point2& p, double depth) const {
     const Point2 pn = calibration().calibrate(p);
@@ -108,9 +151,9 @@ public  :
   }
 
   /// backproject a 2-dimensional point to a 3-dimensional point at infinity
-  Point3 backprojectPointAtInfinity(const Point2& p) const {
+  Unit3 backprojectPointAtInfinity(const Point2& p) const {
     const Point2 pn = calibration().calibrate(p);
-    const Point3 pc(pn.x(), pn.y(), 1.0); //by convention the last element is 1
+    const Unit3 pc(pn.x(), pn.y(), 1.0); //by convention the last element is 1
     return pose().rotation().rotate(pc);
   }
 
@@ -178,13 +221,13 @@ private:
  * @addtogroup geometry
  * \nosubgrouping
  */
-template<typename Calibration>
+template<typename CALIBRATION>
-class GTSAM_EXPORT PinholePose: public PinholeBaseK<Calibration> {
+class GTSAM_EXPORT PinholePose: public PinholeBaseK<CALIBRATION> {
 
 private:
 
-  typedef PinholeBaseK<Calibration> Base; ///< base class has 3D pose as private member
+  typedef PinholeBaseK<CALIBRATION> Base; ///< base class has 3D pose as private member
-  boost::shared_ptr<Calibration> K_; ///< shared pointer to fixed calibration
+  boost::shared_ptr<CALIBRATION> K_; ///< shared pointer to fixed calibration
 
 public:
 
@@ -201,11 +244,11 @@ public:
 
   /** constructor with pose, uses default calibration */
   explicit PinholePose(const Pose3& pose) :
-      Base(pose), K_(new Calibration()) {
+      Base(pose), K_(new CALIBRATION()) {
   }
 
   /** constructor with pose and calibration */
-  PinholePose(const Pose3& pose, const boost::shared_ptr<Calibration>& K) :
+  PinholePose(const Pose3& pose, const boost::shared_ptr<CALIBRATION>& K) :
       Base(pose), K_(K) {
   }
 
@@ -220,14 +263,14 @@ public:
    * (theta 0 = looking in direction of positive X axis)
    * @param height camera height
    */
-  static PinholePose Level(const boost::shared_ptr<Calibration>& K,
+  static PinholePose Level(const boost::shared_ptr<CALIBRATION>& K,
       const Pose2& pose2, double height) {
     return PinholePose(Base::LevelPose(pose2, height), K);
   }
 
   /// PinholePose::level with default calibration
   static PinholePose Level(const Pose2& pose2, double height) {
-    return PinholePose::Level(boost::make_shared<Calibration>(), pose2, height);
+    return PinholePose::Level(boost::make_shared<CALIBRATION>(), pose2, height);
   }
 
   /**
@@ -240,8 +283,8 @@ public:
    * @param K optional calibration parameter
    */
   static PinholePose Lookat(const Point3& eye, const Point3& target,
-      const Point3& upVector, const boost::shared_ptr<Calibration>& K =
+      const Point3& upVector, const boost::shared_ptr<CALIBRATION>& K =
-          boost::make_shared<Calibration>()) {
+          boost::make_shared<CALIBRATION>()) {
     return PinholePose(Base::LookatPose(eye, target, upVector), K);
   }
 
@@ -251,12 +294,12 @@ public:
 
   /// Init from 6D vector
   explicit PinholePose(const Vector &v) :
-      Base(v), K_(new Calibration()) {
+      Base(v), K_(new CALIBRATION()) {
   }
 
   /// Init from Vector and calibration
   PinholePose(const Vector &v, const Vector &K) :
-      Base(v), K_(new Calibration(K)) {
+      Base(v), K_(new CALIBRATION(K)) {
   }
 
   /// @}
@@ -286,10 +329,26 @@ public:
   }
 
   /// return calibration
-  virtual const Calibration& calibration() const {
+  virtual const CALIBRATION& calibration() const {
     return *K_;
   }
 
+  /** project a point from world coordinate to the image, 2 derivatives only
+   *  @param pw is a point in world coordinates
+   *  @param Dpose is the Jacobian w.r.t. the whole camera (really only the pose)
+   *  @param Dpoint is the Jacobian w.r.t. point3
+   */
+  Point2 project2(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
+      OptionalJacobian<2, 3> Dpoint = boost::none) const {
+    return Base::project(pw, Dpose, Dpoint);
+  }
+
+  /// project2 version for point at infinity
+  Point2 project2(const Unit3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
+      OptionalJacobian<2, 2> Dpoint = boost::none) const {
+    return Base::project(pw, Dpose, Dpoint);
+  }
+
   /// @}
   /// @name Manifold
   /// @{
@@ -336,14 +395,14 @@ private:
 };
 // end of class PinholePose
 
-template<typename Calibration>
+template<typename CALIBRATION>
-struct traits<PinholePose<Calibration> > : public internal::Manifold<
+struct traits<PinholePose<CALIBRATION> > : public internal::Manifold<
-    PinholePose<Calibration> > {
+    PinholePose<CALIBRATION> > {
 };
 
-template<typename Calibration>
+template<typename CALIBRATION>
-struct traits<const PinholePose<Calibration> > : public internal::Manifold<
+struct traits<const PinholePose<CALIBRATION> > : public internal::Manifold<
-    PinholePose<Calibration> > {
+    PinholePose<CALIBRATION> > {
 };
 
 } // \ gtsam

gtsam/geometry/PinholeSet.h

@@ -0,0 +1,85 @@
+/* ----------------------------------------------------------------------------
+ 
+ * 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   PinholeSet.h
+ * @brief  A CameraSet of either CalibratedCamera, PinholePose, or PinholeCamera
+ * @author Frank Dellaert
+ */
+
+#pragma once
+
+#include <gtsam/geometry/CameraSet.h>
+#include <gtsam/geometry/triangulation.h>
+#include <boost/optional.hpp>
+#include <boost/foreach.hpp>
+
+namespace gtsam {
+
+/**
+ * PinholeSet: triangulates point and keeps an estimate of it around.
+ */
+template<class CAMERA>
+class PinholeSet: public CameraSet<CAMERA> {
+
+private:
+  typedef CameraSet<CAMERA> Base;
+  typedef PinholeSet<CAMERA> This;
+
+protected:
+
+public:
+
+  /** Virtual destructor */
+  virtual ~PinholeSet() {
+  }
+
+  /// @name Testable
+  /// @{
+
+  /// print
+  virtual void print(const std::string& s = "") const {
+    Base::print(s);
+  }
+
+  /// equals
+  bool equals(const PinholeSet& p, double tol = 1e-9) const {
+    return Base::equals(p, tol); // TODO all flags
+  }
+
+  /// @}
+
+  /// triangulateSafe
+  TriangulationResult triangulateSafe(
+      const std::vector<Point2>& measured,
+      const TriangulationParameters& params) const {
+    return gtsam::triangulateSafe(*this, measured, params);
+  }
+
+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);
+  }
+};
+
+template<class CAMERA>
+struct traits<PinholeSet<CAMERA> > : public Testable<PinholeSet<CAMERA> > {
+};
+
+template<class CAMERA>
+struct traits<const PinholeSet<CAMERA> > : public Testable<PinholeSet<CAMERA> > {
+};
+
+} // \ namespace gtsam

gtsam/geometry/tests/testCalibratedCamera.cpp

@@ -88,13 +88,30 @@ TEST( CalibratedCamera, project)
 }
 
 /* ************************************************************************* */
-TEST( CalibratedCamera, Dproject_to_camera1) {
+static Point2 Project1(const Point3& point) {
-  Point3 pp(155,233,131);
+  return PinholeBase::Project(point);
-  Matrix actual;
+}
-  CalibratedCamera::project_to_camera(pp, actual);
+
-  Matrix expected_numerical = numericalDerivative11<Point2,Point3>(
+TEST( CalibratedCamera, DProject1) {
-      boost::bind(CalibratedCamera::project_to_camera, _1, boost::none), pp);
+  Point3 pp(155, 233, 131);
-  CHECK(assert_equal(expected_numerical, actual));
+  Matrix test1;
+  CalibratedCamera::Project(pp, test1);
+  Matrix test2 = numericalDerivative11<Point2, Point3>(Project1, pp);
+  CHECK(assert_equal(test1, test2));
+}
+
+/* ************************************************************************* */
+static Point2 Project2(const Unit3& point) {
+  return PinholeBase::Project(point);
+}
+
+Unit3 pointAtInfinity(0, 0, 1000);
+TEST( CalibratedCamera, DProjectInfinity) {
+  Matrix test1;
+  CalibratedCamera::Project(pointAtInfinity, test1);
+  Matrix test2 = numericalDerivative11<Point2, Unit3>(Project2,
+      pointAtInfinity);
+  CHECK(assert_equal(test1, test2));
 }
 
 /* ************************************************************************* */
@@ -128,6 +145,36 @@ TEST( CalibratedCamera, Dproject_point_pose2)
   CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
 }
 
+/* ************************************************************************* */
+static Point2 projectAtInfinity(const CalibratedCamera& camera, const Unit3& point) {
+  return camera.project2(point);
+}
+
+TEST( CalibratedCamera, Dproject_point_pose_infinity)
+{
+  Matrix Dpose, Dpoint;
+  Point2 result = camera.project2(pointAtInfinity, Dpose, Dpoint);
+  Matrix numerical_pose  = numericalDerivative21(projectAtInfinity, camera, pointAtInfinity);
+  Matrix numerical_point = numericalDerivative22(projectAtInfinity, camera, pointAtInfinity);
+  CHECK(assert_equal(Point2(), result));
+  CHECK(assert_equal(numerical_pose,  Dpose, 1e-7));
+  CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
+}
+
+/* ************************************************************************* */
+// Add a test with more arbitrary rotation
+TEST( CalibratedCamera, Dproject_point_pose2_infinity)
+{
+  static const Pose3 pose1(Rot3::ypr(0.1, -0.1, 0.4), Point3(0, 0, -10));
+  static const CalibratedCamera camera(pose1);
+  Matrix Dpose, Dpoint;
+  camera.project2(pointAtInfinity, Dpose, Dpoint);
+  Matrix numerical_pose  = numericalDerivative21(projectAtInfinity, camera, pointAtInfinity);
+  Matrix numerical_point = numericalDerivative22(projectAtInfinity, camera, pointAtInfinity);
+  CHECK(assert_equal(numerical_pose,  Dpose, 1e-7));
+  CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */

gtsam/geometry/tests/testCameraSet.cpp

@@ -31,43 +31,105 @@ using namespace gtsam;
 #include <gtsam/geometry/Cal3Bundler.h>
 TEST(CameraSet, Pinhole) {
   typedef PinholeCamera<Cal3Bundler> Camera;
+  typedef CameraSet<Camera> Set;
   typedef vector<Point2> ZZ;
-  CameraSet<Camera> set;
+  Set set;
   Camera camera;
   set.push_back(camera);
   set.push_back(camera);
   Point3 p(0, 0, 1);
-  CHECK(assert_equal(set, set));
+  EXPECT(assert_equal(set, set));
-  CameraSet<Camera> set2 = set;
+  Set set2 = set;
   set2.push_back(camera);
-  CHECK(!set.equals(set2));
+  EXPECT(!set.equals(set2));
 
   // Check measurements
   Point2 expected;
-  ZZ z = set.project(p);
+  ZZ z = set.project2(p);
-  CHECK(assert_equal(expected, z[0]));
+  EXPECT(assert_equal(expected, z[0]));
-  CHECK(assert_equal(expected, z[1]));
+  EXPECT(assert_equal(expected, z[1]));
 
   // Calculate expected derivatives using Pinhole
-  Matrix46 actualF;
+  Matrix actualE;
-  Matrix43 actualE;
+  Matrix29 F1;
-  Matrix43 actualH;
   {
-    Matrix26 F1;
     Matrix23 E1;
-    Matrix23 H1;
+    camera.project2(p, F1, E1);
-    camera.project(p, F1, E1, H1);
+    actualE.resize(4,3);
     actualE << E1, E1;
-    actualF << F1, F1;
-    actualH << H1, H1;
   }
 
   // Check computed derivatives
-  Matrix F, E, H;
+  Set::FBlocks Fs;
-  set.project(p, F, E, H);
+  Matrix E;
-  CHECK(assert_equal(actualF, F));
+  set.project2(p, Fs, E);
-  CHECK(assert_equal(actualE, E));
+  LONGS_EQUAL(2, Fs.size());
-  CHECK(assert_equal(actualH, H));
+  EXPECT(assert_equal(F1, Fs[0]));
+  EXPECT(assert_equal(F1, Fs[1]));
+  EXPECT(assert_equal(actualE, E));
+
+  // Check errors
+  ZZ measured;
+  measured.push_back(Point2(1, 2));
+  measured.push_back(Point2(3, 4));
+  Vector4 expectedV;
+
+  // reprojectionError
+  expectedV << -1, -2, -3, -4;
+  Vector actualV = set.reprojectionError(p, measured);
+  EXPECT(assert_equal(expectedV, actualV));
+
+  // Check Schur complement
+  Matrix F(4, 18);
+  F << F1, Matrix29::Zero(), Matrix29::Zero(), F1;
+  Matrix Ft = F.transpose();
+  Matrix34 Et = E.transpose();
+  Matrix3 P = Et * E;
+  Matrix schur(19, 19);
+  Vector4 b = actualV;
+  Vector v = Ft * (b - E * P * Et * b);
+  schur << Ft * F - Ft * E * P * Et * F, v, v.transpose(), 30;
+  SymmetricBlockMatrix actualReduced = Set::SchurComplement(Fs, E, P, b);
+  EXPECT(assert_equal(schur, actualReduced.matrix()));
+
+  // Check Schur complement update, same order, should just double
+  FastVector<Key> allKeys, keys;
+  allKeys.push_back(1);
+  allKeys.push_back(2);
+  keys.push_back(1);
+  keys.push_back(2);
+  Set::UpdateSchurComplement(Fs, E, P, b, allKeys, keys, actualReduced);
+  EXPECT(assert_equal((Matrix )(2.0 * schur), actualReduced.matrix()));
+
+  // Check Schur complement update, keys reversed
+  FastVector<Key> keys2;
+  keys2.push_back(2);
+  keys2.push_back(1);
+  Set::UpdateSchurComplement(Fs, E, P, b, allKeys, keys2, actualReduced);
+  Vector4 reverse_b;
+  reverse_b << b.tail<2>(), b.head<2>();
+  Vector reverse_v = Ft * (reverse_b - E * P * Et * reverse_b);
+  Matrix A(19, 19);
+  A << Ft * F - Ft * E * P * Et * F, reverse_v, reverse_v.transpose(), 30;
+  EXPECT(assert_equal((Matrix )(2.0 * schur + A), actualReduced.matrix()));
+
+  // reprojectionErrorAtInfinity
+  Unit3 pointAtInfinity(0, 0, 1000);
+  EXPECT(
+      assert_equal(pointAtInfinity,
+          camera.backprojectPointAtInfinity(Point2())));
+  actualV = set.reprojectionError(pointAtInfinity, measured, Fs, E);
+  EXPECT(assert_equal(expectedV, actualV));
+  LONGS_EQUAL(2, Fs.size());
+  {
+    Matrix22 E1;
+    camera.project2(pointAtInfinity, F1, E1);
+    actualE.resize(4,2);
+    actualE << E1, E1;
+  }
+  EXPECT(assert_equal(F1, Fs[0]));
+  EXPECT(assert_equal(F1, Fs[1]));
+  EXPECT(assert_equal(actualE, E));
 }
 
 /* ************************************************************************* */
@@ -83,26 +145,27 @@ TEST(CameraSet, Stereo) {
 
   // Check measurements
   StereoPoint2 expected(0, -1, 0);
-  ZZ z = set.project(p);
+  ZZ z = set.project2(p);
-  CHECK(assert_equal(expected, z[0]));
+  EXPECT(assert_equal(expected, z[0]));
-  CHECK(assert_equal(expected, z[1]));
+  EXPECT(assert_equal(expected, z[1]));
 
   // Calculate expected derivatives using Pinhole
-  Matrix66 actualF;
   Matrix63 actualE;
+  Matrix F1;
   {
-    Matrix36 F1;
     Matrix33 E1;
-    camera.project(p, F1, E1);
+    camera.project2(p, F1, E1);
     actualE << E1, E1;
-    actualF << F1, F1;
   }
 
   // Check computed derivatives
-  Matrix F, E;
+  CameraSet<StereoCamera>::FBlocks Fs;
-  set.project(p, F, E);
+  Matrix E;
-  CHECK(assert_equal(actualF, F));
+  set.project2(p, Fs, E);
-  CHECK(assert_equal(actualE, E));
+  LONGS_EQUAL(2, Fs.size());
+  EXPECT(assert_equal(F1, Fs[0]));
+  EXPECT(assert_equal(F1, Fs[1]));
+  EXPECT(assert_equal(actualE, E));
 }
 
 /* ************************************************************************* */

gtsam/geometry/tests/testPinholeCamera.cpp

@@ -45,10 +45,10 @@ static const Point3 point2(-0.08, 0.08, 0.0);
 static const Point3 point3( 0.08, 0.08, 0.0);
 static const Point3 point4( 0.08,-0.08, 0.0);
 
-static const Point3 point1_inf(-0.16,-0.16, -1.0);
+static const Unit3 point1_inf(-0.16,-0.16, -1.0);
-static const Point3 point2_inf(-0.16, 0.16, -1.0);
+static const Unit3 point2_inf(-0.16, 0.16, -1.0);
-static const Point3 point3_inf( 0.16, 0.16, -1.0);
+static const Unit3 point3_inf( 0.16, 0.16, -1.0);
-static const Point3 point4_inf( 0.16,-0.16, -1.0);
+static const Unit3 point4_inf( 0.16,-0.16, -1.0);
 
 /* ************************************************************************* */
 TEST( PinholeCamera, constructor)
@@ -154,9 +154,9 @@ TEST( PinholeCamera, backprojectInfinity2)
   Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down
   Camera camera(Pose3(rot, origin), K);
 
-  Point3 actual = camera.backprojectPointAtInfinity(Point2());
+  Unit3 actual = camera.backprojectPointAtInfinity(Point2());
-  Point3 expected(0., 1., 0.);
+  Unit3 expected(0., 1., 0.);
-  Point2 x = camera.projectPointAtInfinity(expected);
+  Point2 x = camera.project(expected);
 
   EXPECT(assert_equal(expected, actual));
   EXPECT(assert_equal(Point2(), x));
@@ -169,9 +169,9 @@ TEST( PinholeCamera, backprojectInfinity3)
   Rot3 rot(1., 0., 0., 0., 1., 0., 0., 0., 1.); // identity
   Camera camera(Pose3(rot, origin), K);
 
-  Point3 actual = camera.backprojectPointAtInfinity(Point2());
+  Unit3 actual = camera.backprojectPointAtInfinity(Point2());
-  Point3 expected(0., 0., 1.);
+  Unit3 expected(0., 0., 1.);
-  Point2 x = camera.projectPointAtInfinity(expected);
+  Point2 x = camera.project(expected);
 
   EXPECT(assert_equal(expected, actual));
   EXPECT(assert_equal(Point2(), x));
@@ -197,17 +197,17 @@ TEST( PinholeCamera, Dproject)
 }
 
 /* ************************************************************************* */
-static Point2 projectInfinity3(const Pose3& pose, const Point3& point3D, const Cal3_S2& cal) {
+static Point2 projectInfinity3(const Pose3& pose, const Unit3& point3D, const Cal3_S2& cal) {
-  return Camera(pose,cal).projectPointAtInfinity(point3D);
+  return Camera(pose,cal).project(point3D);
 }
 
 TEST( PinholeCamera, Dproject_Infinity)
 {
   Matrix Dpose, Dpoint, Dcal;
-  Point3 point3D(point1.x(), point1.y(), -10.0); // a point in front of the camera1
+  Unit3 point3D(point1.x(), point1.y(), -10.0); // a point in front of the camera1
 
   // test Projection
-  Point2 actual = camera.projectPointAtInfinity(point3D, Dpose, Dpoint, Dcal);
+  Point2 actual = camera.project(point3D, Dpose, Dpoint, Dcal);
   Point2 expected(-5.0, 5.0);
   EXPECT(assert_equal(actual, expected,  1e-7));
 

gtsam/geometry/tests/testPinholePose.cpp

@@ -46,10 +46,10 @@ static const Point3 point2(-0.08, 0.08, 0.0);
 static const Point3 point3( 0.08, 0.08, 0.0);
 static const Point3 point4( 0.08,-0.08, 0.0);
 
-static const Point3 point1_inf(-0.16,-0.16, -1.0);
+static const Unit3 point1_inf(-0.16,-0.16, -1.0);
-static const Point3 point2_inf(-0.16, 0.16, -1.0);
+static const Unit3 point2_inf(-0.16, 0.16, -1.0);
-static const Point3 point3_inf( 0.16, 0.16, -1.0);
+static const Unit3 point3_inf( 0.16, 0.16, -1.0);
-static const Point3 point4_inf( 0.16,-0.16, -1.0);
+static const Unit3 point4_inf( 0.16,-0.16, -1.0);
 
 /* ************************************************************************* */
 TEST( PinholePose, constructor)
@@ -144,11 +144,11 @@ TEST( PinholePose, Dproject)
 {
   Matrix Dpose, Dpoint;
   Point2 result = camera.project2(point1, Dpose, Dpoint);
-  Matrix numerical_pose  = numericalDerivative31(project3, pose, point1, K);
+  Matrix expectedDcamera  = numericalDerivative31(project3, pose, point1, K);
-  Matrix Hexpected2 = numericalDerivative32(project3, pose, point1, K);
+  Matrix expectedDpoint = numericalDerivative32(project3, pose, point1, K);
   EXPECT(assert_equal(Point2(-100,  100), result));
-  EXPECT(assert_equal(numerical_pose,  Dpose,  1e-7));
+  EXPECT(assert_equal(expectedDcamera,  Dpose,  1e-7));
-  EXPECT(assert_equal(Hexpected2, Dpoint, 1e-7));
+  EXPECT(assert_equal(expectedDpoint, Dpoint, 1e-7));
 }
 
 /* ************************************************************************* */
@@ -161,11 +161,11 @@ TEST( PinholePose, Dproject2)
 {
   Matrix Dcamera, Dpoint;
   Point2 result = camera.project2(point1, Dcamera, Dpoint);
-  Matrix Hexpected1 = numericalDerivative21(project4, camera, point1);
+  Matrix expectedDcamera = numericalDerivative21(project4, camera, point1);
-  Matrix Hexpected2  = numericalDerivative22(project4, camera, point1);
+  Matrix expectedDpoint  = numericalDerivative22(project4, camera, point1);
   EXPECT(assert_equal(result, Point2(-100,  100) ));
-  EXPECT(assert_equal(Hexpected1, Dcamera, 1e-7));
+  EXPECT(assert_equal(expectedDcamera, Dcamera, 1e-7));
-  EXPECT(assert_equal(Hexpected2,  Dpoint,  1e-7));
+  EXPECT(assert_equal(expectedDpoint,  Dpoint,  1e-7));
 }
 
 /* ************************************************************************* */
@@ -176,12 +176,31 @@ TEST( CalibratedCamera, Dproject3)
   static const Camera camera(pose1);
   Matrix Dpose, Dpoint;
   camera.project2(point1, Dpose, Dpoint);
-  Matrix numerical_pose  = numericalDerivative21(project4, camera, point1);
+  Matrix expectedDcamera  = numericalDerivative21(project4, camera, point1);
   Matrix numerical_point = numericalDerivative22(project4, camera, point1);
-  CHECK(assert_equal(numerical_pose,  Dpose, 1e-7));
+  CHECK(assert_equal(expectedDcamera,  Dpose, 1e-7));
   CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
 }
 
+/* ************************************************************************* */
+static Point2 project(const Pose3& pose, const Unit3& pointAtInfinity,
+    const Cal3_S2::shared_ptr& cal) {
+  return Camera(pose, cal).project(pointAtInfinity);
+}
+
+/* ************************************************************************* */
+TEST( PinholePose, DprojectAtInfinity2)
+{
+  Unit3 pointAtInfinity(0,0,1000);
+  Matrix Dpose, Dpoint;
+  Point2 result = camera.project2(pointAtInfinity, Dpose, Dpoint);
+  Matrix expectedDcamera  = numericalDerivative31(project, pose, pointAtInfinity, K);
+  Matrix expectedDpoint = numericalDerivative32(project, pose, pointAtInfinity, K);
+  EXPECT(assert_equal(Point2(0,0), result));
+  EXPECT(assert_equal(expectedDcamera,  Dpose,  1e-7));
+  EXPECT(assert_equal(expectedDpoint, Dpoint, 1e-7));
+}
+
 /* ************************************************************************* */
 static double range0(const Camera& camera, const Point3& point) {
   return camera.range(point);
@@ -191,12 +210,12 @@ static double range0(const Camera& camera, const Point3& point) {
 TEST( PinholePose, range0) {
   Matrix D1; Matrix D2;
   double result = camera.range(point1, D1, D2);
-  Matrix Hexpected1 = numericalDerivative21(range0, camera, point1);
+  Matrix expectedDcamera = numericalDerivative21(range0, camera, point1);
-  Matrix Hexpected2 = numericalDerivative22(range0, camera, point1);
+  Matrix expectedDpoint = numericalDerivative22(range0, camera, point1);
   EXPECT_DOUBLES_EQUAL(point1.distance(camera.pose().translation()), result,
       1e-9);
-  EXPECT(assert_equal(Hexpected1, D1, 1e-7));
+  EXPECT(assert_equal(expectedDcamera, D1, 1e-7));
-  EXPECT(assert_equal(Hexpected2, D2, 1e-7));
+  EXPECT(assert_equal(expectedDpoint, D2, 1e-7));
 }
 
 /* ************************************************************************* */
@@ -208,11 +227,11 @@ static double range1(const Camera& camera, const Pose3& pose) {
 TEST( PinholePose, range1) {
   Matrix D1; Matrix D2;
   double result = camera.range(pose1, D1, D2);
-  Matrix Hexpected1 = numericalDerivative21(range1, camera, pose1);
+  Matrix expectedDcamera = numericalDerivative21(range1, camera, pose1);
-  Matrix Hexpected2 = numericalDerivative22(range1, camera, pose1);
+  Matrix expectedDpoint = numericalDerivative22(range1, camera, pose1);
   EXPECT_DOUBLES_EQUAL(1, result, 1e-9);
-  EXPECT(assert_equal(Hexpected1, D1, 1e-7));
+  EXPECT(assert_equal(expectedDcamera, D1, 1e-7));
-  EXPECT(assert_equal(Hexpected2, D2, 1e-7));
+  EXPECT(assert_equal(expectedDpoint, D2, 1e-7));
 }
 
 /* ************************************************************************* */
@@ -228,11 +247,11 @@ static double range2(const Camera& camera, const Camera2& camera2) {
 TEST( PinholePose, range2) {
   Matrix D1; Matrix D2;
   double result = camera.range<Cal3Bundler>(camera2, D1, D2);
-  Matrix Hexpected1 = numericalDerivative21(range2, camera, camera2);
+  Matrix expectedDcamera = numericalDerivative21(range2, camera, camera2);
-  Matrix Hexpected2 = numericalDerivative22(range2, camera, camera2);
+  Matrix expectedDpoint = numericalDerivative22(range2, camera, camera2);
   EXPECT_DOUBLES_EQUAL(1, result, 1e-9);
-  EXPECT(assert_equal(Hexpected1, D1, 1e-7));
+  EXPECT(assert_equal(expectedDcamera, D1, 1e-7));
-  EXPECT(assert_equal(Hexpected2, D2, 1e-7));
+  EXPECT(assert_equal(expectedDpoint, D2, 1e-7));
 }
 
 /* ************************************************************************* */
@@ -245,11 +264,11 @@ static double range3(const Camera& camera, const CalibratedCamera& camera3) {
 TEST( PinholePose, range3) {
   Matrix D1; Matrix D2;
   double result = camera.range(camera3, D1, D2);
-  Matrix Hexpected1 = numericalDerivative21(range3, camera, camera3);
+  Matrix expectedDcamera = numericalDerivative21(range3, camera, camera3);
-  Matrix Hexpected2 = numericalDerivative22(range3, camera, camera3);
+  Matrix expectedDpoint = numericalDerivative22(range3, camera, camera3);
   EXPECT_DOUBLES_EQUAL(1, result, 1e-9);
-  EXPECT(assert_equal(Hexpected1, D1, 1e-7));
+  EXPECT(assert_equal(expectedDcamera, D1, 1e-7));
-  EXPECT(assert_equal(Hexpected2, D2, 1e-7));
+  EXPECT(assert_equal(expectedDpoint, D2, 1e-7));
 }
 
 /* ************************************************************************* */

gtsam/geometry/tests/testPinholeSet.cpp

@@ -0,0 +1,158 @@
+/* ----------------------------------------------------------------------------
+
+ * 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   testCameraSet.cpp
+ *  @brief  Unit tests for testCameraSet Class
+ *  @author Frank Dellaert
+ *  @date   Feb 19, 2015
+ */
+
+#include <gtsam/geometry/PinholeSet.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <CppUnitLite/TestHarness.h>
+#include <boost/bind.hpp>
+
+using namespace std;
+using namespace gtsam;
+
+/* ************************************************************************* */
+#include <gtsam/geometry/CalibratedCamera.h>
+TEST(PinholeSet, Stereo) {
+  typedef vector<Point2> ZZ;
+  PinholeSet<CalibratedCamera> set;
+  CalibratedCamera camera;
+  set.push_back(camera);
+  set.push_back(camera);
+  //  set.print("set: ");
+  Point3 p(0, 0, 1);
+  EXPECT_LONGS_EQUAL(6, traits<CalibratedCamera>::dimension);
+
+  // Check measurements
+  Point2 expected(0, 0);
+  ZZ z = set.project2(p);
+  EXPECT(assert_equal(expected, z[0]));
+  EXPECT(assert_equal(expected, z[1]));
+
+  // Calculate expected derivatives using Pinhole
+  Matrix43 actualE;
+  Matrix F1;
+  {
+    Matrix23 E1;
+    camera.project2(p, F1, E1);
+    actualE << E1, E1;
+  }
+
+  // Check computed derivatives
+  PinholeSet<CalibratedCamera>::FBlocks Fs;
+  Matrix E;
+  set.project2(p, Fs, E);
+  LONGS_EQUAL(2, Fs.size());
+  EXPECT(assert_equal(F1, Fs[0]));
+  EXPECT(assert_equal(F1, Fs[1]));
+  EXPECT(assert_equal(actualE, E));
+
+  // Instantiate triangulateSafe
+  // TODO triangulation does not work yet for CalibratedCamera
+  // PinholeSet<CalibratedCamera>::Result actual = set.triangulateSafe(z);
+}
+
+/* ************************************************************************* */
+// Cal3Bundler test
+#include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/geometry/Cal3Bundler.h>
+TEST(PinholeSet, Pinhole) {
+  typedef PinholeCamera<Cal3Bundler> Camera;
+  typedef vector<Point2> ZZ;
+  PinholeSet<Camera> set;
+  Camera camera;
+  set.push_back(camera);
+  set.push_back(camera);
+  Point3 p(0, 0, 1);
+  EXPECT(assert_equal(set, set));
+  PinholeSet<Camera> set2 = set;
+  set2.push_back(camera);
+  EXPECT(!set.equals(set2));
+
+  // Check measurements
+  Point2 expected;
+  ZZ z = set.project2(p);
+  EXPECT(assert_equal(expected, z[0]));
+  EXPECT(assert_equal(expected, z[1]));
+
+  // Calculate expected derivatives using Pinhole
+  Matrix actualE;
+  Matrix F1;
+  {
+    Matrix23 E1;
+    camera.project2(p, F1, E1);
+    actualE.resize(4, 3);
+    actualE << E1, E1;
+  }
+
+  // Check computed derivatives
+  {
+    PinholeSet<Camera>::FBlocks Fs;
+    Matrix E;
+    set.project2(p, Fs, E);
+    EXPECT(assert_equal(actualE, E));
+    LONGS_EQUAL(2, Fs.size());
+    EXPECT(assert_equal(F1, Fs[0]));
+    EXPECT(assert_equal(F1, Fs[1]));
+  }
+
+  // Check errors
+  ZZ measured;
+  measured.push_back(Point2(1, 2));
+  measured.push_back(Point2(3, 4));
+  Vector4 expectedV;
+
+  // reprojectionError
+  expectedV << -1, -2, -3, -4;
+  Vector actualV = set.reprojectionError(p, measured);
+  EXPECT(assert_equal(expectedV, actualV));
+
+  // reprojectionErrorAtInfinity
+  Unit3 pointAtInfinity(0, 0, 1000);
+  {
+    Matrix22 E1;
+    camera.project2(pointAtInfinity, F1, E1);
+    actualE.resize(4, 2);
+    actualE << E1, E1;
+  }
+  EXPECT(
+      assert_equal(pointAtInfinity,
+          camera.backprojectPointAtInfinity(Point2())));
+  {
+    PinholeSet<Camera>::FBlocks Fs;
+    Matrix E;
+    actualV = set.reprojectionError(pointAtInfinity, measured, Fs, E);
+    EXPECT(assert_equal(actualE, E));
+    LONGS_EQUAL(2, Fs.size());
+    EXPECT(assert_equal(F1, Fs[0]));
+    EXPECT(assert_equal(F1, Fs[1]));
+  }
+  EXPECT(assert_equal(expectedV, actualV));
+
+  // Instantiate triangulateSafe
+  TriangulationParameters params;
+  TriangulationResult actual = set.triangulateSafe(z, params);
+  CHECK(actual.degenerate());
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */
+

gtsam/geometry/tests/testTriangulation.cpp

@@ -17,6 +17,7 @@
  */
 
 #include <gtsam/geometry/triangulation.h>
+#include <gtsam/geometry/SimpleCamera.h>
 #include <gtsam/geometry/Cal3Bundler.h>
 #include <CppUnitLite/TestHarness.h>
 
@@ -49,6 +50,7 @@ Point2 z1 = camera1.project(landmark);
 Point2 z2 = camera2.project(landmark);
 
 //******************************************************************************
+// Simple test with a well-behaved two camera situation
 TEST( triangulation, twoPoses) {
 
   vector<Pose3> poses;
@@ -57,24 +59,37 @@ TEST( triangulation, twoPoses) {
   poses += pose1, pose2;
   measurements += z1, z2;
 
-  bool optimize = true;
   double rank_tol = 1e-9;
 
-  boost::optional<Point3> triangulated_landmark = triangulatePoint3(poses,
+  // 1. Test simple DLT, perfect in no noise situation
-      sharedCal, measurements, rank_tol, optimize);
+  bool optimize = false;
-  EXPECT(assert_equal(landmark, *triangulated_landmark, 1e-2));
+  boost::optional<Point3> actual1 = //
+      triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize);
+  EXPECT(assert_equal(landmark, *actual1, 1e-7));
 
-  // 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814)
+  // 2. test with optimization on, same answer
+  optimize = true;
+  boost::optional<Point3> actual2 = //
+      triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize);
+  EXPECT(assert_equal(landmark, *actual2, 1e-7));
+
+  // 3. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814)
   measurements.at(0) += Point2(0.1, 0.5);
   measurements.at(1) += Point2(-0.2, 0.3);
+  optimize = false;
+  boost::optional<Point3> actual3 = //
+      triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize);
+  EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual3, 1e-4));
 
-  boost::optional<Point3> triangulated_landmark_noise = triangulatePoint3(poses,
+  // 4. Now with optimization on
-      sharedCal, measurements, rank_tol, optimize);
+  optimize = true;
-  EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 1e-2));
+  boost::optional<Point3> actual4 = //
+      triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize);
+  EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-4));
 }
 
 //******************************************************************************
-
+// Similar, but now with Bundler calibration
 TEST( triangulation, twoPosesBundler) {
 
   boost::shared_ptr<Cal3Bundler> bundlerCal = //
@@ -95,17 +110,17 @@ TEST( triangulation, twoPosesBundler) {
   bool optimize = true;
   double rank_tol = 1e-9;
 
-  boost::optional<Point3> triangulated_landmark = triangulatePoint3(poses,
+  boost::optional<Point3> actual = //
-      bundlerCal, measurements, rank_tol, optimize);
+      triangulatePoint3(poses, bundlerCal, measurements, rank_tol, optimize);
-  EXPECT(assert_equal(landmark, *triangulated_landmark, 1e-2));
+  EXPECT(assert_equal(landmark, *actual, 1e-7));
 
-  // 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814)
+  // Add some noise and try again
   measurements.at(0) += Point2(0.1, 0.5);
   measurements.at(1) += Point2(-0.2, 0.3);
 
-  boost::optional<Point3> triangulated_landmark_noise = triangulatePoint3(poses,
+  boost::optional<Point3> actual2 = //
-      bundlerCal, measurements, rank_tol, optimize);
+      triangulatePoint3(poses, bundlerCal, measurements, rank_tol, optimize);
-  EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 1e-2));
+  EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19847), *actual2, 1e-4));
 }
 
 //******************************************************************************
@@ -116,17 +131,17 @@ TEST( triangulation, fourPoses) {
   poses += pose1, pose2;
   measurements += z1, z2;
 
-  boost::optional<Point3> triangulated_landmark = triangulatePoint3(poses,
+  boost::optional<Point3> actual = triangulatePoint3(poses, sharedCal,
-      sharedCal, measurements);
+      measurements);
-  EXPECT(assert_equal(landmark, *triangulated_landmark, 1e-2));
+  EXPECT(assert_equal(landmark, *actual, 1e-2));
 
   // 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814)
   measurements.at(0) += Point2(0.1, 0.5);
   measurements.at(1) += Point2(-0.2, 0.3);
 
-  boost::optional<Point3> triangulated_landmark_noise = //
+  boost::optional<Point3> actual2 = //
       triangulatePoint3(poses, sharedCal, measurements);
-  EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 1e-2));
+  EXPECT(assert_equal(landmark, *actual2, 1e-2));
 
   // 3. Add a slightly rotated third camera above, again with measurement noise
   Pose3 pose3 = pose1 * Pose3(Rot3::ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
@@ -150,7 +165,7 @@ TEST( triangulation, fourPoses) {
   SimpleCamera camera4(pose4, *sharedCal);
 
 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
-  CHECK_EXCEPTION(camera4.project(landmark);, CheiralityException);
+  CHECK_EXCEPTION(camera4.project(landmark), CheiralityException);
 
   poses += pose4;
   measurements += Point2(400, 400);
@@ -180,17 +195,17 @@ TEST( triangulation, fourPoses_distinct_Ks) {
   cameras += camera1, camera2;
   measurements += z1, z2;
 
-  boost::optional<Point3> triangulated_landmark = //
+  boost::optional<Point3> actual = //
       triangulatePoint3(cameras, measurements);
-  EXPECT(assert_equal(landmark, *triangulated_landmark, 1e-2));
+  EXPECT(assert_equal(landmark, *actual, 1e-2));
 
   // 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814)
   measurements.at(0) += Point2(0.1, 0.5);
   measurements.at(1) += Point2(-0.2, 0.3);
 
-  boost::optional<Point3> triangulated_landmark_noise = //
+  boost::optional<Point3> actual2 = //
       triangulatePoint3(cameras, measurements);
-  EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 1e-2));
+  EXPECT(assert_equal(landmark, *actual2, 1e-2));
 
   // 3. Add a slightly rotated third camera above, again with measurement noise
   Pose3 pose3 = pose1 * Pose3(Rot3::ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
@@ -216,7 +231,7 @@ TEST( triangulation, fourPoses_distinct_Ks) {
   SimpleCamera camera4(pose4, K4);
 
 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
-  CHECK_EXCEPTION(camera4.project(landmark);, CheiralityException);
+  CHECK_EXCEPTION(camera4.project(landmark), CheiralityException);
 
   cameras += camera4;
   measurements += Point2(400, 400);
@@ -244,23 +259,19 @@ TEST( triangulation, twoIdenticalPoses) {
 }
 
 //******************************************************************************
-/*
+TEST( triangulation, onePose) {
- TEST( triangulation, onePose) {
+  // we expect this test to fail with a TriangulationUnderconstrainedException
- // we expect this test to fail with a TriangulationUnderconstrainedException
+  // because there's only one camera observation
- // because there's only one camera observation
 
- Cal3_S2 *sharedCal(1500, 1200, 0, 640, 480);
+  vector<Pose3> poses;
+  vector<Point2> measurements;
 
- vector<Pose3> poses;
+  poses += Pose3();
- vector<Point2> measurements;
+  measurements += Point2();
 
- poses += Pose3();
+  CHECK_EXCEPTION(triangulatePoint3(poses, sharedCal, measurements),
- measurements += Point2();
+      TriangulationUnderconstrainedException);
-
+}
- CHECK_EXCEPTION(triangulatePoint3(poses, measurements, *sharedCal),
- TriangulationUnderconstrainedException);
- }
- */
 
 //******************************************************************************
 int main() {

gtsam/geometry/triangulation.cpp

@@ -46,7 +46,6 @@ Vector4 triangulateHomogeneousDLT(
   double error;
   Vector v;
   boost::tie(rank, error, v) = DLT(A, rank_tol);
-  //  std::cout << "s " << s.transpose() << std:endl;
 
   if (rank < 3)
     throw(TriangulationUnderconstrainedException());

gtsam/geometry/triangulation.h

@@ -18,6 +18,7 @@
 
 #pragma once
 
+#include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/slam/TriangulationFactor.h>
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
@@ -64,7 +65,6 @@ GTSAM_EXPORT Point3 triangulateDLT(
     const std::vector<Matrix34>& projection_matrices,
     const std::vector<Point2>& measurements, double rank_tol = 1e-9);
 
-///
 /**
  * Create a factor graph with projection factors from poses and one calibration
  * @param poses Camera poses
@@ -86,8 +86,9 @@ std::pair<NonlinearFactorGraph, Values> triangulationGraph(
   static SharedNoiseModel prior_model(noiseModel::Isotropic::Sigma(6, 1e-6));
   for (size_t i = 0; i < measurements.size(); i++) {
     const Pose3& pose_i = poses[i];
-    PinholeCamera<CALIBRATION> camera_i(pose_i, *sharedCal);
+    typedef PinholePose<CALIBRATION> Camera;
-    graph.push_back(TriangulationFactor<CALIBRATION> //
+    Camera camera_i(pose_i, sharedCal);
+    graph.push_back(TriangulationFactor<Camera> //
         (camera_i, measurements[i], unit2, landmarkKey));
   }
   return std::make_pair(graph, values);
@@ -114,13 +115,22 @@ std::pair<NonlinearFactorGraph, Values> triangulationGraph(
   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<typename CAMERA::CalibrationType> //
+    graph.push_back(TriangulationFactor<CAMERA> //
         (camera_i, measurements[i], unit2, landmarkKey));
   }
   return std::make_pair(graph, values);
 }
 
-///
+/// PinholeCamera specific version
+template<class CALIBRATION>
+std::pair<NonlinearFactorGraph, Values> triangulationGraph(
+    const std::vector<PinholeCamera<CALIBRATION> >& cameras,
+    const std::vector<Point2>& measurements, Key landmarkKey,
+    const Point3& initialEstimate) {
+  return triangulationGraph<PinholeCamera<CALIBRATION> > //
+  (cameras, measurements, landmarkKey, initialEstimate);
+}
+
 /**
  * Optimize for triangulation
  * @param graph nonlinear factors for projection
@@ -147,8 +157,8 @@ Point3 triangulateNonlinear(const std::vector<Pose3>& poses,
   // Create a factor graph and initial values
   Values values;
   NonlinearFactorGraph graph;
-  boost::tie(graph, values) = triangulationGraph(poses, sharedCal, measurements,
+  boost::tie(graph, values) = triangulationGraph<CALIBRATION> //
-      Symbol('p', 0), initialEstimate);
+      (poses, sharedCal, measurements, Symbol('p', 0), initialEstimate);
 
   return optimize(graph, values, Symbol('p', 0));
 }
@@ -168,12 +178,21 @@ Point3 triangulateNonlinear(
   // Create a factor graph and initial values
   Values values;
   NonlinearFactorGraph graph;
-  boost::tie(graph, values) = triangulationGraph(cameras, measurements,
+  boost::tie(graph, values) = triangulationGraph<CAMERA> //
-      Symbol('p', 0), initialEstimate);
+      (cameras, measurements, Symbol('p', 0), initialEstimate);
 
   return optimize(graph, values, Symbol('p', 0));
 }
 
+/// PinholeCamera specific version
+template<class CALIBRATION>
+Point3 triangulateNonlinear(
+    const std::vector<PinholeCamera<CALIBRATION> >& cameras,
+    const std::vector<Point2>& measurements, const Point3& initialEstimate) {
+  return triangulateNonlinear<PinholeCamera<CALIBRATION> > //
+  (cameras, measurements, initialEstimate);
+}
+
 /**
  * Create a 3*4 camera projection matrix from calibration and pose.
  * Functor for partial application on calibration
@@ -224,12 +243,13 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
   // Triangulate linearly
   Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol);
 
-  // The n refine using non-linear optimization
+  // Then refine using non-linear optimization
   if (optimize)
-    point = triangulateNonlinear(poses, sharedCal, measurements, point);
+    point = triangulateNonlinear<CALIBRATION> //
+        (poses, sharedCal, measurements, point);
 
 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
-  // verify that the triangulated point lies infront of all cameras
+  // verify that the triangulated point lies in front of all cameras
   BOOST_FOREACH(const Pose3& pose, poses) {
     const Point3& p_local = pose.transform_to(point);
     if (p_local.z() <= 0)
@@ -265,9 +285,8 @@ Point3 triangulatePoint3(
     throw(TriangulationUnderconstrainedException());
 
   // construct projection matrices from poses & calibration
-  typedef PinholeCamera<typename CAMERA::CalibrationType> Camera;
   std::vector<Matrix34> projection_matrices;
-  BOOST_FOREACH(const Camera& camera, cameras)
+  BOOST_FOREACH(const CAMERA& camera, cameras)
     projection_matrices.push_back(
         CameraProjectionMatrix<typename CAMERA::CalibrationType>(camera.calibration())(
             camera.pose()));
@@ -275,11 +294,11 @@ Point3 triangulatePoint3(
 
   // The n refine using non-linear optimization
   if (optimize)
-    point = triangulateNonlinear(cameras, measurements, point);
+    point = triangulateNonlinear<CAMERA>(cameras, measurements, point);
 
 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
-  // verify that the triangulated point lies infront of all cameras
+  // verify that the triangulated point lies in front of all cameras
-  BOOST_FOREACH(const Camera& camera, cameras) {
+  BOOST_FOREACH(const CAMERA& camera, cameras) {
     const Point3& p_local = camera.pose().transform_to(point);
     if (p_local.z() <= 0)
       throw(TriangulationCheiralityException());
@@ -289,5 +308,160 @@ Point3 triangulatePoint3(
   return point;
 }
 
+/// Pinhole-specific version
+template<class CALIBRATION>
+Point3 triangulatePoint3(
+    const std::vector<PinholeCamera<CALIBRATION> >& cameras,
+    const std::vector<Point2>& measurements, double rank_tol = 1e-9,
+    bool optimize = false) {
+  return triangulatePoint3<PinholeCamera<CALIBRATION> > //
+  (cameras, measurements, rank_tol, optimize);
+}
+
+struct TriangulationParameters {
+
+  double rankTolerance; ///< threshold to decide whether triangulation is result.degenerate
+  bool enableEPI; ///< if set to true, will refine triangulation using LM
+
+  /**
+   * if the landmark is triangulated at distance larger than this,
+   * result is flagged as degenerate.
+   */
+  double landmarkDistanceThreshold; //
+
+  /**
+   * If this is nonnegative the we will check if the average reprojection error
+   * is smaller than this threshold after triangulation, otherwise result is
+   * flagged as degenerate.
+   */
+  double dynamicOutlierRejectionThreshold;
+
+  /**
+   * Constructor
+   * @param rankTol tolerance used to check if point triangulation is degenerate
+   * @param enableEPI if true refine triangulation with embedded LM iterations
+   * @param landmarkDistanceThreshold flag as degenerate if point further than this
+   * @param dynamicOutlierRejectionThreshold or if average error larger than this
+   *
+   */
+  TriangulationParameters(const double _rankTolerance = 1.0,
+      const bool _enableEPI = false, double _landmarkDistanceThreshold = -1,
+      double _dynamicOutlierRejectionThreshold = -1) :
+      rankTolerance(_rankTolerance), enableEPI(_enableEPI), //
+      landmarkDistanceThreshold(_landmarkDistanceThreshold), //
+      dynamicOutlierRejectionThreshold(_dynamicOutlierRejectionThreshold) {
+  }
+
+  // stream to output
+  friend std::ostream &operator<<(std::ostream &os,
+      const TriangulationParameters& p) {
+    os << "rankTolerance = " << p.rankTolerance << std::endl;
+    os << "enableEPI = " << p.enableEPI << std::endl;
+    os << "landmarkDistanceThreshold = " << p.landmarkDistanceThreshold
+        << std::endl;
+    os << "dynamicOutlierRejectionThreshold = "
+        << p.dynamicOutlierRejectionThreshold << std::endl;
+    return os;
+  }
+};
+
+/**
+ * TriangulationResult is an optional point, along with the reasons why it is invalid.
+ */
+class TriangulationResult: public boost::optional<Point3> {
+  enum Status {
+    VALID, DEGENERATE, BEHIND_CAMERA
+  };
+  Status status_;
+  TriangulationResult(Status s) :
+      status_(s) {
+  }
+public:
+  TriangulationResult(const Point3& p) :
+      status_(VALID) {
+    reset(p);
+  }
+  static TriangulationResult Degenerate() {
+    return TriangulationResult(DEGENERATE);
+  }
+  static TriangulationResult BehindCamera() {
+    return TriangulationResult(BEHIND_CAMERA);
+  }
+  bool degenerate() const {
+    return status_ == DEGENERATE;
+  }
+  bool behindCamera() const {
+    return status_ == BEHIND_CAMERA;
+  }
+  // stream to output
+  friend std::ostream &operator<<(std::ostream &os,
+      const TriangulationResult& result) {
+    if (result)
+      os << "point = " << *result << std::endl;
+    else
+      os << "no point, status = " << result.status_ << std::endl;
+    return os;
+  }
+};
+
+/// triangulateSafe: extensive checking of the outcome
+template<class CAMERA>
+TriangulationResult triangulateSafe(const std::vector<CAMERA>& cameras,
+    const std::vector<Point2>& measured,
+    const TriangulationParameters& params) {
+
+  size_t m = cameras.size();
+
+  // if we have a single pose the corresponding factor is uninformative
+  if (m < 2)
+    return TriangulationResult::Degenerate();
+  else
+    // We triangulate the 3D position of the landmark
+    try {
+      Point3 point = triangulatePoint3<CAMERA>(cameras, measured,
+          params.rankTolerance, params.enableEPI);
+
+      // Check landmark distance and re-projection errors to avoid outliers
+      size_t i = 0;
+      double totalReprojError = 0.0;
+      BOOST_FOREACH(const CAMERA& camera, cameras) {
+        const Pose3& pose = camera.pose();
+        if (params.landmarkDistanceThreshold > 0
+            && pose.translation().distance(point)
+                > params.landmarkDistanceThreshold)
+          return TriangulationResult::Degenerate();
+#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
+        // verify that the triangulated point lies in front of all cameras
+        // Only needed if this was not yet handled by exception
+        const Point3& p_local = pose.transform_to(point);
+        if (p_local.z() <= 0)
+          return TriangulationResult::BehindCamera();
+#endif
+        // Check reprojection error
+        if (params.dynamicOutlierRejectionThreshold > 0) {
+          const Point2& zi = measured.at(i);
+          Point2 reprojectionError(camera.project(point) - zi);
+          totalReprojError += reprojectionError.vector().norm();
+        }
+        i += 1;
+      }
+      // Flag as degenerate if average reprojection error is too large
+      if (params.dynamicOutlierRejectionThreshold > 0
+          && totalReprojError / m > params.dynamicOutlierRejectionThreshold)
+        return TriangulationResult::Degenerate();
+
+      // all good!
+      return TriangulationResult(point);
+    } catch (TriangulationUnderconstrainedException&) {
+      // This exception is thrown if
+      // 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)
+      return TriangulationResult::Degenerate();
+    } catch (TriangulationCheiralityException&) {
+      // point is behind one of the cameras: can be the case of close-to-parallel cameras or may depend on outliers
+      return TriangulationResult::BehindCamera();
+    }
+}
+
 } // \namespace gtsam
 

gtsam/linear/NoiseModel.cpp

@@ -20,6 +20,7 @@
 #include <gtsam/base/timing.h>
 
 #include <boost/foreach.hpp>
+#include <boost/format.hpp>
 #include <boost/random/linear_congruential.hpp>
 #include <boost/random/normal_distribution.hpp>
 #include <boost/random/variate_generator.hpp>
@@ -506,7 +507,7 @@ Isotropic::shared_ptr Isotropic::Variance(size_t dim, double variance, bool smar
 
 /* ************************************************************************* */
 void Isotropic::print(const string& name) const {
-  cout << name << "isotropic sigma " << " " << sigma_ << endl;
+  cout << boost::format("isotropic dim=%1% sigma=%2%") % dim() % sigma_ << endl;
 }
 
 /* ************************************************************************* */
@@ -534,6 +535,11 @@ void Isotropic::WhitenInPlace(Matrix& H) const {
   H *= invsigma_;
 }
 
+/* ************************************************************************* */
+void Isotropic::whitenInPlace(Vector& v) const {
+  v *= invsigma_;
+}
+
 /* ************************************************************************* */
 void Isotropic::WhitenInPlace(Eigen::Block<Matrix> H) const {
   H *= invsigma_;

gtsam/linear/NoiseModel.h

@@ -550,6 +550,7 @@ namespace gtsam {
       virtual Vector unwhiten(const Vector& v) const;
       virtual Matrix Whiten(const Matrix& H) const;
       virtual void WhitenInPlace(Matrix& H) const;
+      virtual void whitenInPlace(Vector& v) const;
       virtual void WhitenInPlace(Eigen::Block<Matrix> H) const;
 
       /**

gtsam/linear/RegularHessianFactor.h

@@ -19,7 +19,7 @@
 #pragma once
 
 #include <gtsam/linear/HessianFactor.h>
-#include <gtsam/slam/RegularJacobianFactor.h>
+#include <gtsam/linear/RegularJacobianFactor.h>
 #include <boost/foreach.hpp>
 #include <vector>
 

gtsam/linear/VectorValues.cpp

@@ -66,6 +66,18 @@ namespace gtsam {
     return result;
   }
 
+  /* ************************************************************************* */
+  VectorValues::iterator VectorValues::insert(const std::pair<Key, Vector>& key_value) {
+    // Note that here we accept a pair with a reference to the Vector, but the Vector is copied as
+    // it is inserted into the values_ map.
+    std::pair<iterator, bool> result = values_.insert(key_value);
+    if(!result.second)
+      throw std::invalid_argument(
+      "Requested to insert variable '" + DefaultKeyFormatter(key_value.first)
+      + "' already in this VectorValues.");
+    return result.first;
+  }
+
   /* ************************************************************************* */
   void VectorValues::update(const VectorValues& values)
   {

gtsam/linear/VectorValues.h

@@ -181,23 +181,14 @@ namespace gtsam {
      * @param value The vector to be inserted.
      * @param j The index with which the value will be associated. */
     iterator insert(Key j, const Vector& value) {
-      return insert(std::make_pair(j, value)); // Note only passing a reference to the Vector
+      return insert(std::make_pair(j, value));
     }
 
     /** Insert a vector \c value with key \c j.  Throws an invalid_argument exception if the key \c
      *  j is already used.
      * @param value The vector to be inserted.
      * @param j The index with which the value will be associated. */
-    iterator insert(const std::pair<Key, Vector>& key_value) {
+    iterator insert(const std::pair<Key, Vector>& key_value);
-      // Note that here we accept a pair with a reference to the Vector, but the Vector is copied as
-      // it is inserted into the values_ map.
-      std::pair<iterator, bool> result = values_.insert(key_value);
-      if(!result.second)
-        throw std::invalid_argument(
-        "Requested to insert variable '" + DefaultKeyFormatter(key_value.first)
-        + "' already in this VectorValues.");
-      return result.first;
-    }
 
     /** Insert all values from \c values.  Throws an invalid_argument exception if any keys to be
      *  inserted are already used. */

gtsam/linear/tests/testRegularHessianFactor.cpp

@@ -15,7 +15,7 @@
  * @date    March 4, 2014
  */
 
-#include <gtsam/slam/RegularHessianFactor.h>
+#include <gtsam/linear/RegularHessianFactor.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/linear/VectorValues.h>
 

gtsam/linear/tests/testRegularJacobianFactor.cpp

@@ -16,7 +16,7 @@
  * @date    Nov 12, 2014
  */
 
-#include <gtsam/slam/RegularJacobianFactor.h>
+#include <gtsam/linear/RegularJacobianFactor.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/linear/GaussianConditional.h>
 #include <gtsam/linear/VectorValues.h>

gtsam/nonlinear/tests/testExpression.cpp

@@ -175,7 +175,8 @@ using namespace binary;
 Expression<Cal3_S2> K(3);
 
 // Create expression tree
-Expression<Point2> projection(PinholeCamera<Cal3_S2>::project_to_camera, p_cam);
+Point2 (*f)(const Point3&, OptionalJacobian<2, 3>) = &PinholeBase::Project;
+Expression<Point2> projection(f, p_cam);
 Expression<Point2> uv_hat(uncalibrate<Cal3_S2>, K, projection);
 }
 /* ************************************************************************* */

gtsam/slam/EssentialMatrixFactor.h

@@ -173,7 +173,7 @@ public:
       Point3 _1T2 = E.direction().point3();
       Point3 d1T2 = d * _1T2;
       Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2); // 2R1*((x,y,1)-d*1T2)
-      pn = SimpleCamera::project_to_camera(dP2);
+      pn = PinholeBase::Project(dP2);
 
     } else {
 
@@ -186,7 +186,7 @@ public:
       Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2, DdP2_rot, DP2_point);
 
       Matrix23 Dpn_dP2;
-      pn = SimpleCamera::project_to_camera(dP2, Dpn_dP2);
+      pn = PinholeBase::Project(dP2, Dpn_dP2);
 
       if (DE) {
         Matrix DdP2_E(3, 5);

gtsam/slam/JacobianFactorQ.h

@@ -17,7 +17,7 @@
 
 #pragma once
 
-#include "RegularJacobianFactor.h"
+#include <gtsam/linear/RegularJacobianFactor.h>
 
 namespace gtsam {
 /**
@@ -28,7 +28,7 @@ class JacobianFactorQ: public RegularJacobianFactor<D> {
 
   typedef RegularJacobianFactor<D> Base;
   typedef Eigen::Matrix<double, ZDim, D> MatrixZD;
-  typedef std::pair<Key, MatrixZD> KeyMatrixZD;
+  typedef std::pair<Key, Matrix> KeyMatrix;
 
 public:
 
@@ -42,7 +42,6 @@ public:
       Base() {
     Matrix zeroMatrix = Matrix::Zero(0, D);
     Vector zeroVector = Vector::Zero(0);
-    typedef std::pair<Key, Matrix> KeyMatrix;
     std::vector<KeyMatrix> QF;
     QF.reserve(keys.size());
     BOOST_FOREACH(const Key& key, keys)
@@ -51,24 +50,25 @@ public:
   }
 
   /// Constructor
-  JacobianFactorQ(const std::vector<KeyMatrixZD>& Fblocks, const Matrix& E,
+  JacobianFactorQ(const FastVector<Key>& keys,
-      const Matrix3& P, const Vector& b, const SharedDiagonal& model =
+      const std::vector<MatrixZD>& FBlocks, const Matrix& E, const Matrix3& P,
-          SharedDiagonal()) :
+      const Vector& b, const SharedDiagonal& model = SharedDiagonal()) :
       Base() {
     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
     // TODO: can we do better ?
-    typedef std::pair<Key, Matrix> KeyMatrix;
     std::vector<KeyMatrix> QF;
     QF.reserve(m);
     // Below, we compute each mZDim*D block A_j = Q_j * F_j = (mZDim*ZDim) * (Zdim*D)
-    BOOST_FOREACH(const KeyMatrixZD& it, Fblocks)
+    for (size_t k = 0; k < FBlocks.size(); ++k) {
+      Key key = keys[k];
       QF.push_back(
-          KeyMatrix(it.first, Q.block(0, ZDim * j++, m2, ZDim) * it.second));
+          KeyMatrix(key, - Q.block(0, ZDim * j++, m2, ZDim) * FBlocks[k]));
+    }
     // Which is then passed to the normal JacobianFactor constructor
-    JacobianFactor::fillTerms(QF, Q * b, model);
+    JacobianFactor::fillTerms(QF, - Q * b, model);
   }
 };
 // end class JacobianFactorQ

gtsam/slam/JacobianFactorQR.h

@@ -6,8 +6,8 @@
  */
 
 #pragma once
-#include <gtsam/slam/RegularJacobianFactor.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
+#include <gtsam/linear/RegularJacobianFactor.h>
 #include <gtsam/inference/Symbol.h>
 
 namespace gtsam {
@@ -22,25 +22,24 @@ class JacobianFactorQR: public RegularJacobianFactor<D> {
 
   typedef RegularJacobianFactor<D> Base;
   typedef Eigen::Matrix<double, ZDim, D> MatrixZD;
-  typedef std::pair<Key, MatrixZD> KeyMatrixZD;
 
 public:
 
   /**
    * Constructor
    */
-  JacobianFactorQR(const std::vector<KeyMatrixZD>& Fblocks, const Matrix& E,
+  JacobianFactorQR(const FastVector<Key>& keys,
-      const Matrix3& P, const Vector& b, //
+      const std::vector<MatrixZD>& FBlocks, const Matrix& E, const Matrix3& P,
+      const Vector& b, //
       const SharedDiagonal& model = SharedDiagonal()) :
       Base() {
     // Create a number of Jacobian factors in a factor graph
     GaussianFactorGraph gfg;
     Symbol pointKey('p', 0);
-    size_t i = 0;
+    for (size_t k = 0; k < FBlocks.size(); ++k) {
-    BOOST_FOREACH(const KeyMatrixZD& it, Fblocks) {
+      Key key = keys[k];
-      gfg.add(pointKey, E.block<ZDim, 3>(ZDim * i, 0), it.first, it.second,
+      gfg.add(pointKey, E.block<ZDim, 3>(ZDim * k, 0), key, FBlocks[k],
-          b.segment<ZDim>(ZDim * i), model);
+          b.segment < ZDim > (ZDim * k), model);
-      i += 1;
     }
     //gfg.print("gfg");
 

gtsam/slam/JacobianFactorSVD.h

@@ -5,18 +5,17 @@
  */
 
 #pragma once
-#include "gtsam/slam/RegularJacobianFactor.h"
+#include <gtsam/linear/RegularJacobianFactor.h>
 
 namespace gtsam {
 /**
- * JacobianFactor for Schur complement that uses Q noise model
+ * JacobianFactor for Schur complement
  */
 template<size_t D, size_t ZDim>
 class JacobianFactorSVD: public RegularJacobianFactor<D> {
 
   typedef RegularJacobianFactor<D> Base;
   typedef Eigen::Matrix<double, ZDim, D> MatrixZD; // e.g 2 x 6 with Z=Point2
-  typedef std::pair<Key, MatrixZD> KeyMatrixZD;
   typedef std::pair<Key, Matrix> KeyMatrix;
 
 public:
@@ -38,13 +37,21 @@ public:
     JacobianFactor::fillTerms(QF, zeroVector, model);
   }
 
-  /// Constructor
+  /**
-  JacobianFactorSVD(const std::vector<KeyMatrixZD>& Fblocks,
+   * @brief Constructor
-      const Matrix& Enull, const Vector& b, //
+   * Takes the CameraSet derivatives (as ZDim*D blocks of block-diagonal F)
+   * and a reduced point derivative, Enull
+   * and creates a reduced-rank Jacobian factor on the CameraSet
+   *
+   * @Fblocks:
+   */
+  JacobianFactorSVD(const FastVector<Key>& keys,
+      const std::vector<MatrixZD>& Fblocks, const Matrix& Enull,
+      const Vector& b, //
       const SharedDiagonal& model = SharedDiagonal()) :
       Base() {
     size_t numKeys = Enull.rows() / ZDim;
-    size_t j = 0, m2 = ZDim * numKeys - 3;
+    size_t m2 = ZDim * numKeys - 3;
     // PLAIN NULL SPACE TRICK
     // Matrix Q = Enull * Enull.transpose();
     // BOOST_FOREACH(const KeyMatrixZD& it, Fblocks)
@@ -52,10 +59,12 @@ public:
     // JacobianFactor factor(QF, Q * b);
     std::vector<KeyMatrix> QF;
     QF.reserve(numKeys);
-    BOOST_FOREACH(const KeyMatrixZD& it, Fblocks)
+    for (size_t k = 0; k < Fblocks.size(); ++k) {
+      Key key = keys[k];
       QF.push_back(
-          KeyMatrix(it.first,
+          KeyMatrix(key,
-              (Enull.transpose()).block(0, ZDim * j++, m2, ZDim) * it.second));
+              (Enull.transpose()).block(0, ZDim * k, m2, ZDim) * Fblocks[k]));
+    }
     JacobianFactor::fillTerms(QF, Enull.transpose() * b, model);
   }
 };

gtsam/slam/RegularImplicitSchurFactor.h

@@ -7,6 +7,7 @@
 
 #pragma once
 
+#include <gtsam/geometry/CameraSet.h>
 #include <gtsam/linear/JacobianFactor.h>
 #include <gtsam/linear/VectorValues.h>
 #include <boost/foreach.hpp>
@@ -17,7 +18,7 @@ namespace gtsam {
 /**
  * RegularImplicitSchurFactor
  */
-template<size_t D> //
+template<class CAMERA>
 class RegularImplicitSchurFactor: public GaussianFactor {
 
 public:
@@ -26,14 +27,20 @@ public:
 
 protected:
 
-  typedef Eigen::Matrix<double, 2, D> Matrix2D; ///< type of an F block
+  // This factor is closely related to a CameraSet
-  typedef Eigen::Matrix<double, D, D> MatrixDD; ///< camera hessian
+  typedef CameraSet<CAMERA> Set;
-  typedef std::pair<Key, Matrix2D> KeyMatrix2D; ///< named F block
 
-  std::vector<KeyMatrix2D> Fblocks_; ///< All 2*D F blocks (one for each camera)
+  typedef typename CAMERA::Measurement Z;
-  Matrix3 PointCovariance_; ///< the 3*3 matrix P = inv(E'E) (2*2 if degenerate)
+  static const int D = traits<CAMERA>::dimension; ///< Camera dimension
-  Matrix E_; ///< The 2m*3 E Jacobian with respect to the point
+  static const int ZDim = traits<Z>::dimension; ///< Measurement dimension
-  Vector b_; ///< 2m-dimensional RHS vector
+
+  typedef Eigen::Matrix<double, ZDim, D> MatrixZD; ///< type of an F block
+  typedef Eigen::Matrix<double, D, D> MatrixDD; ///< camera hessian
+
+  const std::vector<MatrixZD> FBlocks_; ///< All ZDim*D F blocks (one for each camera)
+  const Matrix PointCovariance_; ///< the 3*3 matrix P = inv(E'E) (2*2 if degenerate)
+  const Matrix E_; ///< The 2m*3 E Jacobian with respect to the point
+  const Vector b_; ///< 2m-dimensional RHS vector
 
 public:
 
@@ -41,54 +48,40 @@ public:
   RegularImplicitSchurFactor() {
   }
 
-  /// Construct from blcoks of F, E, inv(E'*E), and RHS vector b
+  /// Construct from blocks of F, E, inv(E'*E), and RHS vector b
-  RegularImplicitSchurFactor(const std::vector<KeyMatrix2D>& Fblocks, const Matrix& E,
+  RegularImplicitSchurFactor(const FastVector<Key>& keys,
-      const Matrix3& P, const Vector& b) :
+      const std::vector<MatrixZD>& FBlocks, const Matrix& E, const Matrix& P,
-      Fblocks_(Fblocks), PointCovariance_(P), E_(E), b_(b) {
+      const Vector& b) :
-    initKeys();
+      GaussianFactor(keys), FBlocks_(FBlocks), PointCovariance_(P), E_(E), b_(b) {
-  }
-
-  /// initialize keys from Fblocks
-  void initKeys() {
-    keys_.reserve(Fblocks_.size());
-    BOOST_FOREACH(const KeyMatrix2D& it, Fblocks_)
-      keys_.push_back(it.first);
   }
 
   /// Destructor
   virtual ~RegularImplicitSchurFactor() {
   }
 
-  // Write access, only use for construction!
+  std::vector<MatrixZD>& FBlocks() const {
-
+    return FBlocks_;
-  inline std::vector<KeyMatrix2D>& Fblocks() {
-    return Fblocks_;
   }
 
-  inline Matrix3& PointCovariance() {
+  const Matrix& E() const {
-    return PointCovariance_;
-  }
-
-  inline Matrix& E() {
     return E_;
   }
 
-  inline Vector& b() {
+  const Vector& b() const {
     return b_;
   }
 
-  /// Get matrix P
+  const Matrix& getPointCovariance() const {
-  inline const Matrix3& getPointCovariance() const {
     return PointCovariance_;
   }
 
   /// print
-  void print(const std::string& s = "",
+  void print(const std::string& s = "", const KeyFormatter& keyFormatter =
-      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+      DefaultKeyFormatter) const {
     std::cout << " RegularImplicitSchurFactor " << std::endl;
     Factor::print(s);
     for (size_t pos = 0; pos < size(); ++pos) {
-      std::cout << "Fblock:\n" << Fblocks_[pos].second << std::endl;
+      std::cout << "Fblock:\n" << FBlocks_[pos] << std::endl;
     }
     std::cout << "PointCovariance:\n" << PointCovariance_ << std::endl;
     std::cout << "E:\n" << E_ << std::endl;
@@ -100,10 +93,11 @@ public:
     const This* f = dynamic_cast<const This*>(&lf);
     if (!f)
       return false;
-    for (size_t pos = 0; pos < size(); ++pos) {
+    for (size_t k = 0; k < FBlocks_.size(); ++k) {
-      if (keys_[pos] != f->keys_[pos]) return false;
+      if (keys_[k] != f->keys_[k])
-      if (Fblocks_[pos].first != f->Fblocks_[pos].first) return false;
+        return false;
-      if (!equal_with_abs_tol(Fblocks_[pos].second,f->Fblocks_[pos].second,tol)) return false;
+      if (!equal_with_abs_tol(FBlocks_[k], f->FBlocks_[k], tol))
+        return false;
     }
     return equal_with_abs_tol(PointCovariance_, f->PointCovariance_, tol)
         && equal_with_abs_tol(E_, f->E_, tol)
@@ -126,18 +120,26 @@ public:
     return Matrix();
   }
   virtual std::pair<Matrix, Vector> jacobian() const {
-    throw std::runtime_error("RegularImplicitSchurFactor::jacobian non implemented");
+    throw std::runtime_error(
+        "RegularImplicitSchurFactor::jacobian non implemented");
     return std::make_pair(Matrix(), Vector());
   }
+
+  /// *Compute* full augmented information matrix
   virtual Matrix augmentedInformation() const {
-    throw std::runtime_error(
+
-        "RegularImplicitSchurFactor::augmentedInformation non implemented");
+    // Do the Schur complement
-    return Matrix();
+    SymmetricBlockMatrix augmentedHessian = //
+        Set::SchurComplement(FBlocks_, E_, b_);
+    return augmentedHessian.matrix();
   }
+
+  /// *Compute* full information matrix
   virtual Matrix information() const {
-    throw std::runtime_error(
+    Matrix augmented = augmentedInformation();
-        "RegularImplicitSchurFactor::information non implemented");
+    int m = this->keys_.size();
-    return Matrix();
+    size_t M = D * m;
+    return augmented.block(0, 0, M, M);
   }
 
   /// Return the diagonal of the Hessian for this factor
@@ -145,17 +147,17 @@ public:
     // diag(Hessian) = diag(F' * (I - E * PointCov * E') * F);
     VectorValues d;
 
-    for (size_t pos = 0; pos < size(); ++pos) { // for each camera
+    for (size_t k = 0; k < size(); ++k) { // for each camera
-      Key j = keys_[pos];
+      Key j = keys_[k];
 
       // Calculate Fj'*Ej for the current camera (observing a single point)
-      // D x 3 = (D x 2) * (2 x 3)
+      // D x 3 = (D x ZDim) * (ZDim x 3)
-      const Matrix2D& Fj = Fblocks_[pos].second;
+      const MatrixZD& Fj = FBlocks_[k];
-      Eigen::Matrix<double, D, 3>  FtE = Fj.transpose()
+      Eigen::Matrix<double, D, 3> FtE = Fj.transpose()
-          * E_.block<2, 3>(2 * pos, 0);
+          * E_.block<ZDim, 3>(ZDim * k, 0);
 
       Eigen::Matrix<double, D, 1> dj;
-      for (size_t k = 0; k < D; ++k) { // for each diagonal element of the camera hessian
+      for (int k = 0; k < D; ++k) { // for each diagonal element of the camera hessian
         // Vector column_k_Fj = Fj.col(k);
         dj(k) = Fj.col(k).squaredNorm(); // dot(column_k_Fj, column_k_Fj);
         // Vector column_k_FtE = FtE.row(k);
@@ -181,13 +183,13 @@ public:
       Key j = keys_[pos];
 
       // Calculate Fj'*Ej for the current camera (observing a single point)
-      // D x 3 = (D x 2) * (2 x 3)
+      // D x 3 = (D x ZDim) * (ZDim x 3)
-      const Matrix2D& Fj = Fblocks_[pos].second;
+      const MatrixZD& Fj = FBlocks_[pos];
       Eigen::Matrix<double, D, 3> FtE = Fj.transpose()
-          * E_.block<2, 3>(2 * pos, 0);
+          * E_.block<ZDim, 3>(ZDim * pos, 0);
 
       DVector dj;
-      for (size_t k = 0; k < D; ++k) { // for each diagonal element of the camera hessian
+      for (int k = 0; k < D; ++k) { // for each diagonal element of the camera hessian
         dj(k) = Fj.col(k).squaredNorm();
         // (1 x 1) = (1 x 3) * (3 * 3) * (3 x 1)
         dj(k) -= FtE.row(k) * PointCovariance_ * FtE.row(k).transpose();
@@ -202,38 +204,41 @@ public:
     // F'*(I - E*P*E')*F
     for (size_t pos = 0; pos < size(); ++pos) {
       Key j = keys_[pos];
-      // F'*F - F'*E*P*E'*F   (9*2)*(2*9) - (9*2)*(2*3)*(3*3)*(3*2)*(2*9)
+      // F'*F - F'*E*P*E'*F  e.g. (9*2)*(2*9) - (9*2)*(2*3)*(3*3)*(3*2)*(2*9)
-      const Matrix2D& Fj = Fblocks_[pos].second;
+      const MatrixZD& Fj = FBlocks_[pos];
       //      Eigen::Matrix<double, D, 3> FtE = Fj.transpose()
-      //          * E_.block<2, 3>(2 * pos, 0);
+      //          * E_.block<ZDim, 3>(ZDim * pos, 0);
       //      blocks[j] = Fj.transpose() * Fj
       //          - FtE * PointCovariance_ * FtE.transpose();
 
-      const Matrix23& Ej = E_.block<2, 3>(2 * pos, 0);
+      const Matrix23& Ej = E_.block<ZDim, 3>(ZDim * pos, 0);
-      blocks[j] = Fj.transpose() * (Fj - Ej * PointCovariance_ * Ej.transpose() * Fj);
+      blocks[j] = Fj.transpose()
+          * (Fj - Ej * PointCovariance_ * Ej.transpose() * Fj);
 
       // F'*(I - E*P*E')*F, TODO: this should work, but it does not :-(
-      //      static const Eigen::Matrix<double, 2, 2> I2 = eye(2);
+      //      static const Eigen::Matrix<double, ZDim, ZDim> I2 = eye(ZDim);
       //      Matrix2 Q = //
-      //          I2 - E_.block<2, 3>(2 * pos, 0) * PointCovariance_ * E_.block<2, 3>(2 * pos, 0).transpose();
+      //          I2 - E_.block<ZDim, 3>(ZDim * pos, 0) * PointCovariance_ * E_.block<ZDim, 3>(ZDim * pos, 0).transpose();
       //      blocks[j] = Fj.transpose() * Q * Fj;
     }
     return blocks;
   }
 
   virtual GaussianFactor::shared_ptr clone() const {
-    return boost::make_shared<RegularImplicitSchurFactor<D> >(Fblocks_,
+    return boost::make_shared<RegularImplicitSchurFactor<CAMERA> >(keys_,
-        PointCovariance_, E_, b_);
+        FBlocks_, PointCovariance_, E_, b_);
-    throw std::runtime_error("RegularImplicitSchurFactor::clone non implemented");
+    throw std::runtime_error(
+        "RegularImplicitSchurFactor::clone non implemented");
   }
   virtual bool empty() const {
     return false;
   }
 
   virtual GaussianFactor::shared_ptr negate() const {
-    return boost::make_shared<RegularImplicitSchurFactor<D> >(Fblocks_,
+    return boost::make_shared<RegularImplicitSchurFactor<CAMERA> >(keys_,
-        PointCovariance_, E_, b_);
+        FBlocks_, PointCovariance_, E_, b_);
-    throw std::runtime_error("RegularImplicitSchurFactor::negate non implemented");
+    throw std::runtime_error(
+        "RegularImplicitSchurFactor::negate non implemented");
   }
 
   // Raw Vector version of y += F'*alpha*(I - E*P*E')*F*x, for testing
@@ -251,22 +256,24 @@ public:
   typedef std::vector<Vector2> Error2s;
 
   /**
-   * @brief Calculate corrected error Q*(e-2*b) = (I - E*P*E')*(e-2*b)
+   * @brief Calculate corrected error Q*(e-ZDim*b) = (I - E*P*E')*(e-ZDim*b)
    */
   void projectError2(const Error2s& e1, Error2s& e2) const {
 
-    // d1 = E.transpose() * (e1-2*b) = (3*2m)*2m
+    // d1 = E.transpose() * (e1-ZDim*b) = (3*2m)*2m
     Vector3 d1;
     d1.setZero();
     for (size_t k = 0; k < size(); k++)
-      d1 += E_.block < 2, 3 > (2 * k, 0).transpose() * (e1[k] - 2 * b_.segment < 2 > (k * 2));
+      d1 += E_.block<ZDim, 3>(ZDim * k, 0).transpose()
+          * (e1[k] - ZDim * b_.segment<ZDim>(k * ZDim));
 
     // d2 = E.transpose() * e1 = (3*2m)*2m
     Vector3 d2 = PointCovariance_ * d1;
 
     // e3 = alpha*(e1 - E*d2) = 1*[2m-(2m*3)*3]
     for (size_t k = 0; k < size(); k++)
-      e2[k] = e1[k] - 2 * b_.segment < 2 > (k * 2) - E_.block < 2, 3 > (2 * k, 0) * d2;
+      e2[k] = e1[k] - ZDim * b_.segment<ZDim>(k * ZDim)
+          - E_.block<ZDim, 3>(ZDim * k, 0) * d2;
   }
 
   /*
@@ -286,7 +293,7 @@ public:
 
     // e1 = F * x - b = (2m*dm)*dm
     for (size_t k = 0; k < size(); ++k)
-      e1[k] = Fblocks_[k].second * x.at(keys_[k]);
+      e1[k] = FBlocks_[k] * x.at(keys_[k]);
     projectError2(e1, e2);
 
     double result = 0;
@@ -308,7 +315,7 @@ public:
 
     // e1 = F * x - b = (2m*dm)*dm
     for (size_t k = 0; k < size(); ++k)
-      e1[k] = Fblocks_[k].second * x.at(keys_[k]) - b_.segment < 2 > (k * 2);
+      e1[k] = FBlocks_[k] * x.at(keys_[k]) - b_.segment<ZDim>(k * ZDim);
     projectError(e1, e2);
 
     double result = 0;
@@ -321,21 +328,21 @@ public:
   /**
    * @brief Calculate corrected error Q*e = (I - E*P*E')*e
    */
-    void projectError(const Error2s& e1, Error2s& e2) const {
+  void projectError(const Error2s& e1, Error2s& e2) const {
 
-      // d1 = E.transpose() * e1 = (3*2m)*2m
+    // d1 = E.transpose() * e1 = (3*2m)*2m
-      Vector3 d1;
+    Vector3 d1;
-      d1.setZero();
+    d1.setZero();
-      for (size_t k = 0; k < size(); k++)
+    for (size_t k = 0; k < size(); k++)
-        d1 += E_.block < 2, 3 > (2 * k, 0).transpose() * e1[k];
+      d1 += E_.block<ZDim, 3>(ZDim * k, 0).transpose() * e1[k];
 
-      // d2 = E.transpose() * e1 = (3*2m)*2m
+    // d2 = E.transpose() * e1 = (3*2m)*2m
-      Vector3 d2 = PointCovariance_ * d1;
+    Vector3 d2 = PointCovariance_ * d1;
 
-      // e3 = alpha*(e1 - E*d2) = 1*[2m-(2m*3)*3]
+    // e3 = alpha*(e1 - E*d2) = 1*[2m-(2m*3)*3]
-      for (size_t k = 0; k < size(); k++)
+    for (size_t k = 0; k < size(); k++)
-        e2[k] = e1[k] - E_.block < 2, 3 > (2 * k, 0) * d2;
+      e2[k] = e1[k] - E_.block<ZDim, 3>(ZDim * k, 0) * d2;
-    }
+  }
 
   /// Scratch space for multiplyHessianAdd
   mutable Error2s e1, e2;
@@ -356,19 +363,17 @@ public:
     e2.resize(size());
 
     // e1 = F * x = (2m*dm)*dm
-    size_t k = 0;
+    for (size_t k = 0; k < size(); ++k) {
-    BOOST_FOREACH(const KeyMatrix2D& it, Fblocks_) {
+      Key key = keys_[k];
-      Key key = it.first;
+      e1[k] = FBlocks_[k] * ConstDMap(x + D * key);
-      e1[k++] = it.second * ConstDMap(x + D * key);
     }
 
     projectError(e1, e2);
 
     // y += F.transpose()*e2 = (2d*2m)*2m
-    k = 0;
+    for (size_t k = 0; k < size(); ++k) {
-    BOOST_FOREACH(const KeyMatrix2D& it, Fblocks_) {
+      Key key = keys_[k];
-      Key key = it.first;
+      DMap(y + D * key) += FBlocks_[k].transpose() * alpha * e2[k];
-      DMap(y + D * key) += it.second.transpose() * alpha * e2[k++];
     }
   }
 
@@ -389,7 +394,7 @@ public:
 
     // e1 = F * x = (2m*dm)*dm
     for (size_t k = 0; k < size(); ++k)
-      e1[k] = Fblocks_[k].second * x.at(keys_[k]);
+      e1[k] = FBlocks_[k] * x.at(keys_[k]);
 
     projectError(e1, e2);
 
@@ -401,8 +406,8 @@ public:
       Vector& yi = it.first->second;
       // Create the value as a zero vector if it does not exist.
       if (it.second)
-        yi = Vector::Zero(Fblocks_[k].second.cols());
+        yi = Vector::Zero(FBlocks_[k].cols());
-      yi += Fblocks_[k].second.transpose() * alpha * e2[k];
+      yi += FBlocks_[k].transpose() * alpha * e2[k];
     }
   }
 
@@ -412,9 +417,9 @@ public:
   void multiplyHessianDummy(double alpha, const VectorValues& x,
       VectorValues& y) const {
 
-    BOOST_FOREACH(const KeyMatrix2D& Fi, Fblocks_) {
+    for (size_t k = 0; k < size(); ++k) {
       static const Vector empty;
-      Key key = Fi.first;
+      Key key = keys_[k];
       std::pair<VectorValues::iterator, bool> it = y.tryInsert(key, empty);
       Vector& yi = it.first->second;
       yi = x.at(key);
@@ -429,14 +434,14 @@ public:
     e1.resize(size());
     e2.resize(size());
     for (size_t k = 0; k < size(); k++)
-      e1[k] = b_.segment < 2 > (2 * k);
+      e1[k] = b_.segment<ZDim>(ZDim * k);
     projectError(e1, e2);
 
     // g = F.transpose()*e2
     VectorValues g;
     for (size_t k = 0; k < size(); ++k) {
       Key key = keys_[k];
-      g.insert(key, -Fblocks_[k].second.transpose() * e2[k]);
+      g.insert(key, -FBlocks_[k].transpose() * e2[k]);
     }
 
     // return it
@@ -456,27 +461,33 @@ public:
     e1.resize(size());
     e2.resize(size());
     for (size_t k = 0; k < size(); k++)
-      e1[k] = b_.segment < 2 > (2 * k);
+      e1[k] = b_.segment<ZDim>(ZDim * k);
     projectError(e1, e2);
 
     for (size_t k = 0; k < size(); ++k) { // for each camera in the factor
       Key j = keys_[k];
-      DMap(d + D * j) += -Fblocks_[k].second.transpose() * e2[k];
+      DMap(d + D * j) += -FBlocks_[k].transpose() * e2[k];
     }
   }
 
   /// Gradient wrt a key at any values
   Vector gradient(Key key, const VectorValues& x) const {
-    throw std::runtime_error("gradient for RegularImplicitSchurFactor is not implemented yet");
+    throw std::runtime_error(
+        "gradient for RegularImplicitSchurFactor is not implemented yet");
   }
 
-
 };
 // end class RegularImplicitSchurFactor
 
+template<class CAMERA>
+const int RegularImplicitSchurFactor<CAMERA>::D;
+
+template<class CAMERA>
+const int RegularImplicitSchurFactor<CAMERA>::ZDim;
+
 // traits
-template<size_t D> struct traits<RegularImplicitSchurFactor<D> > : public Testable<
+template<class CAMERA> struct traits<RegularImplicitSchurFactor<CAMERA> > : public Testable<
-    RegularImplicitSchurFactor<D> > {
+    RegularImplicitSchurFactor<CAMERA> > {
 };
 
 }

gtsam/slam/SmartFactorBase.h

@@ -22,9 +22,9 @@
 #include <gtsam/slam/JacobianFactorQ.h>
 #include <gtsam/slam/JacobianFactorSVD.h>
 #include <gtsam/slam/RegularImplicitSchurFactor.h>
-#include <gtsam/slam/RegularHessianFactor.h>
 
 #include <gtsam/nonlinear/NonlinearFactor.h>
+#include <gtsam/linear/RegularHessianFactor.h>
 #include <gtsam/geometry/CameraSet.h>
 
 #include <boost/optional.hpp>
@@ -41,13 +41,23 @@ namespace gtsam {
  * The methods take a Cameras argument, which should behave like PinholeCamera, and
  * the value of a point, which is kept in the base class.
  */
-template<class CAMERA, size_t D>
+template<class CAMERA>
 class SmartFactorBase: public NonlinearFactor {
 
-protected:
+private:
-
+  typedef NonlinearFactor Base;
+  typedef SmartFactorBase<CAMERA> This;
   typedef typename CAMERA::Measurement Z;
 
+  /**
+   * As of Feb 22, 2015, the noise model is the same for all measurements and
+   * is isotropic. This allows for moving most calculations of Schur complement
+   * etc to be moved to CameraSet very easily, and also agrees pragmatically
+   * with what is normally done.
+   */
+  SharedIsotropic noiseModel_;
+
+protected:
   /**
    * 2D measurement and noise model for each of the m views
    * We keep a copy of measurements for I/O and computing the error.
@@ -55,45 +65,54 @@ protected:
    */
   std::vector<Z> measured_;
 
-  std::vector<SharedNoiseModel> noise_; ///< noise model used
+  /// @name Pose of the camera in the body frame
-
+  const boost::optional<Pose3> body_P_sensor_; ///< Pose of the camera in the body frame
-  boost::optional<Pose3> body_P_sensor_; ///< The pose of the sensor in the body frame (one for all cameras)
+  /// @}
 
+  static const int Dim = traits<CAMERA>::dimension; ///< Camera dimension
   static const int ZDim = traits<Z>::dimension; ///< Measurement dimension
 
-  /// Definitions for blocks of F
+  // Definitions for block matrices used internally
-  typedef Eigen::Matrix<double, ZDim, D> Matrix2D; // F
+  typedef Eigen::Matrix<double, Dim, ZDim> MatrixD2; // F'
-  typedef Eigen::Matrix<double, D, ZDim> MatrixD2; // F'
+  typedef Eigen::Matrix<double, Dim, Dim> MatrixDD; // camera hessian block
-  typedef std::pair<Key, Matrix2D> KeyMatrix2D; // Fblocks
-  typedef Eigen::Matrix<double, D, D> MatrixDD; // camera hessian block
   typedef Eigen::Matrix<double, ZDim, 3> Matrix23;
-  typedef Eigen::Matrix<double, D, 1> VectorD;
+  typedef Eigen::Matrix<double, Dim, 1> VectorD;
   typedef Eigen::Matrix<double, ZDim, ZDim> Matrix2;
 
-  /// shorthand for base class type
+  // check that noise model is isotropic and the same
-  typedef NonlinearFactor Base;
+  // TODO, this is hacky, we should just do this via constructor, not add
-
+  void maybeSetNoiseModel(const SharedNoiseModel& sharedNoiseModel) {
-  /// shorthand for this class
+    if (!sharedNoiseModel)
-  typedef SmartFactorBase<CAMERA, D> This;
+      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
+  typedef Eigen::Matrix<double, ZDim, Dim> MatrixZD; // F
+
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
   /// shorthand for a smart pointer to a factor
   typedef boost::shared_ptr<This> shared_ptr;
 
+  /// We use the new CameraSte data structure to refer to a set of cameras
   typedef CameraSet<CAMERA> Cameras;
 
-  /**
+  /// Constructor
-   * Constructor
-   * @param body_P_sensor is the transform from sensor to body frame (default identity)
-   */
   SmartFactorBase(boost::optional<Pose3> body_P_sensor = boost::none) :
-      body_P_sensor_(body_P_sensor) {
+          body_P_sensor_(body_P_sensor){}
-  }
 
-  /** Virtual destructor */
+  /// Virtual destructor, subclasses from NonlinearFactor
   virtual ~SmartFactorBase() {
   }
 
@@ -101,36 +120,36 @@ public:
    * Add a new measurement and pose key
    * @param measured_i is the 2m dimensional projection of a single landmark
    * @param poseKey is the index corresponding to the camera observing the landmark
-   * @param noise_i is the measurement noise
+   * @param sharedNoiseModel is the measurement noise
    */
-  void add(const Z& measured_i, const Key& poseKey_i,
+  void add(const Z& measured_i, const Key& cameraKey_i,
-      const SharedNoiseModel& noise_i) {
+      const SharedNoiseModel& sharedNoiseModel) {
     this->measured_.push_back(measured_i);
-    this->keys_.push_back(poseKey_i);
+    this->keys_.push_back(cameraKey_i);
-    this->noise_.push_back(noise_i);
+    maybeSetNoiseModel(sharedNoiseModel);
   }
 
   /**
    * Add a bunch of measurements, together with the camera keys and noises
    */
-  void add(std::vector<Z>& measurements, std::vector<Key>& poseKeys,
+  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(poseKeys.at(i));
+      this->keys_.push_back(cameraKeys.at(i));
-      this->noise_.push_back(noises.at(i));
+      maybeSetNoiseModel(noises.at(i));
     }
   }
 
   /**
-   * Add a bunch of measurements and uses the same noise model for all of them
+   * Add a bunch of measurements and use the same noise model for all of them
    */
-  void add(std::vector<Z>& measurements, std::vector<Key>& poseKeys,
+  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(poseKeys.at(i));
+      this->keys_.push_back(cameraKeys.at(i));
-      this->noise_.push_back(noise);
+      maybeSetNoiseModel(noise);
     }
   }
 
@@ -143,7 +162,7 @@ public:
     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);
-      this->noise_.push_back(noise);
+      maybeSetNoiseModel(noise);
     }
   }
 
@@ -157,9 +176,12 @@ public:
     return measured_;
   }
 
-  /** return the noise models */
+  /// Collect all cameras: important that in key order
-  const std::vector<SharedNoiseModel>& noise() const {
+  virtual Cameras cameras(const Values& values) const {
-    return noise_;
+    Cameras cameras;
+    BOOST_FOREACH(const Key& k, this->keys_)
+      cameras.push_back(values.at<CAMERA>(k));
+    return cameras;
   }
 
   /**
@@ -172,11 +194,11 @@ public:
     std::cout << s << "SmartFactorBase, z = \n";
     for (size_t k = 0; k < measured_.size(); ++k) {
       std::cout << "measurement, p = " << measured_[k] << "\t";
-      noise_[k]->print("noise model = ");
+      noiseModel_->print("noise model = ");
     }
-    if (this->body_P_sensor_)
+    if(body_P_sensor_)
-      this->body_P_sensor_->print("  sensor pose in body frame: ");
+      body_P_sensor_->print("body_P_sensor_:\n");
-    Base::print("", keyFormatter);
+    print("", keyFormatter);
   }
 
   /// equals
@@ -189,518 +211,105 @@ public:
         areMeasurementsEqual = false;
       break;
     }
-    return e && Base::equals(p, tol) && areMeasurementsEqual
+    return e && Base::equals(p, tol) && areMeasurementsEqual;
-        && ((!body_P_sensor_ && !e->body_P_sensor_)
-            || (body_P_sensor_ && e->body_P_sensor_
-                && body_P_sensor_->equals(*e->body_P_sensor_)));
   }
 
-  /// Calculate vector of re-projection errors, before applying noise model
+  /// Compute reprojection errors [h(x)-z] = [cameras.project(p)-z] and derivatives
-  Vector reprojectionError(const Cameras& cameras, const Point3& point) const {
+  template<class POINT>
-
+  Vector unwhitenedError(const Cameras& cameras, const POINT& point,
-    // Project into CameraSet
+      boost::optional<typename Cameras::FBlocks&> Fs = boost::none, //
-    std::vector<Z> predicted;
+      boost::optional<Matrix&> E = boost::none) const {
-    try {
+    Vector ue = cameras.reprojectionError(point, measured_, Fs, E);
-      predicted = cameras.project(point);
+    if(body_P_sensor_){
-    } catch (CheiralityException&) {
+      for(size_t i=0; i < Fs->size(); i++){
-      std::cout << "reprojectionError: Cheirality exception " << std::endl;
+        Pose3 w_Pose_body = (cameras[i].pose()).compose(body_P_sensor_->inverse());
-      exit(EXIT_FAILURE); // TODO: throw exception
+        Matrix J(6, 6);
+        Pose3 world_P_body = w_Pose_body.compose(*body_P_sensor_, J);
+        Fs->at(i) = Fs->at(i) * J;
+      }
     }
-
+    return ue;
-    // Calculate vector of errors
-    size_t nrCameras = cameras.size();
-    Vector b(ZDim * nrCameras);
-    for (size_t i = 0, row = 0; i < nrCameras; i++, row += ZDim) {
-      Z e = predicted[i] - measured_.at(i);
-      b.segment<ZDim>(row) = e.vector();
-    }
-
-    return b;
-  }
-
-  /// Calculate vector of re-projection errors, noise model applied
-  Vector whitenedError(const Cameras& cameras, const Point3& point) const {
-    Vector b = reprojectionError(cameras, point);
-    size_t nrCameras = cameras.size();
-    for (size_t i = 0, row = 0; i < nrCameras; i++, row += ZDim)
-      b.segment<ZDim>(row) = noise_.at(i)->whiten(b.segment<ZDim>(row));
-    return b;
   }
 
   /**
-   * Calculate the error of the factor.
+   * Calculate vector of re-projection errors [h(x)-z] = [cameras.project(p) - z]
+   * Noise model applied
+   */
+  template<class POINT>
+  Vector whitenedError(const Cameras& cameras, const POINT& point) const {
+    Vector e = cameras.reprojectionError(point, measured_);
+    if (noiseModel_)
+      noiseModel_->whitenInPlace(e);
+    return e;
+  }
+
+  /** 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.
-   * This is different from reprojectionError(cameras,point) as each point is whitened
+   * Will be used in "error(Values)" function required by NonlinearFactor base class
    */
+  template<class POINT>
   double totalReprojectionError(const Cameras& cameras,
-      const Point3& point) const {
+      const POINT& point) const {
-    Vector b = reprojectionError(cameras, point);
+    Vector e = whitenedError(cameras, point);
-    double overallError = 0;
+    return 0.5 * e.dot(e);
-    size_t nrCameras = cameras.size();
-    for (size_t i = 0; i < nrCameras; i++)
-      overallError += noise_.at(i)->distance(b.segment<ZDim>(i * ZDim));
-    return 0.5 * overallError;
-  }
-
-  /**
-   * Compute whitenedError, returning only the derivative E, i.e.,
-   * the stacked ZDim*3 derivatives of project with respect to the point.
-   * Assumes non-degenerate ! TODO explain this
-   */
-  Vector whitenedError(const Cameras& cameras, const Point3& point,
-      Matrix& E) const {
-
-    // Project into CameraSet, calculating derivatives
-    std::vector<Z> predicted;
-    try {
-      using boost::none;
-      predicted = cameras.project(point, none, E, none);
-    } catch (CheiralityException&) {
-      std::cout << "whitenedError(E): Cheirality exception " << std::endl;
-      exit(EXIT_FAILURE); // TODO: throw exception
-    }
-
-    // if needed, whiten
-    size_t m = keys_.size();
-    Vector b(ZDim * m);
-    for (size_t i = 0, row = 0; i < m; i++, row += ZDim) {
-
-      // Calculate error
-      const Z& zi = measured_.at(i);
-      Vector bi = (zi - predicted[i]).vector();
-
-      // if needed, whiten
-      SharedNoiseModel model = noise_.at(i);
-      if (model) {
-        // TODO: re-factor noiseModel to take any block/fixed vector/matrix
-        Vector dummy;
-        Matrix Ei = E.block<ZDim, 3>(row, 0);
-        model->WhitenSystem(Ei, dummy);
-        E.block<ZDim, 3>(row, 0) = Ei;
-      }
-      b.segment<ZDim>(row) = bi;
-    }
-    return b;
-  }
-
-  /**
-   *  Compute F, E, and optionally H, where F and E are the stacked derivatives
-   *  with respect to the cameras, point, and calibration, respectively.
-   *  The value of cameras/point are passed as parameters.
-   *  Returns error vector b
-   *  TODO: the treatment of body_P_sensor_ is weird: the transformation
-   *  is applied in the caller, but the derivatives are computed here.
-   */
-  Vector whitenedError(const Cameras& cameras, const Point3& point, Matrix& F,
-      Matrix& E, boost::optional<Matrix&> G = boost::none) const {
-
-    // Project into CameraSet, calculating derivatives
-    std::vector<Z> predicted;
-    try {
-      predicted = cameras.project(point, F, E, G);
-    } catch (CheiralityException&) {
-      std::cout << "whitenedError(E,F): Cheirality exception " << std::endl;
-      exit(EXIT_FAILURE); // TODO: throw exception
-    }
-
-    // Calculate error and whiten derivatives if needed
-    size_t m = keys_.size();
-    Vector b(ZDim * m);
-    for (size_t i = 0, row = 0; i < m; i++, row += ZDim) {
-
-      // Calculate error
-      const Z& zi = measured_.at(i);
-      Vector bi = (zi - predicted[i]).vector();
-
-      // if we have a sensor offset, correct camera derivatives
-      if (body_P_sensor_) {
-        // TODO: no simpler way ??
-        const Pose3& pose_i = cameras[i].pose();
-        Pose3 w_Pose_body = pose_i.compose(body_P_sensor_->inverse());
-        Matrix66 J;
-        Pose3 world_P_body = w_Pose_body.compose(*body_P_sensor_, J);
-        F.block<ZDim, 6>(row, 0) *= J;
-      }
-
-      // if needed, whiten
-      SharedNoiseModel model = noise_.at(i);
-      if (model) {
-        // TODO, refactor noiseModel so we can take blocks
-        Matrix Fi = F.block<ZDim, 6>(row, 0);
-        Matrix Ei = E.block<ZDim, 3>(row, 0);
-        if (!G)
-          model->WhitenSystem(Fi, Ei, bi);
-        else {
-          Matrix Gi = G->block<ZDim, D - 6>(row, 0);
-          model->WhitenSystem(Fi, Ei, Gi, bi);
-          G->block<ZDim, D - 6>(row, 0) = Gi;
-        }
-        F.block<ZDim, 6>(row, 0) = Fi;
-        E.block<ZDim, 3>(row, 0) = Ei;
-      }
-      b.segment<ZDim>(row) = bi;
-    }
-    return b;
   }
 
   /// Computes Point Covariance P from E
-  static Matrix3 PointCov(Matrix& E) {
+  static Matrix PointCov(Matrix& E) {
     return (E.transpose() * E).inverse();
   }
 
-  /// Computes Point Covariance P, with lambda parameter
-  static Matrix3 PointCov(Matrix& E, double lambda,
-      bool diagonalDamping = false) {
-
-    Matrix3 EtE = E.transpose() * E;
-
-    if (diagonalDamping) { // diagonal of the hessian
-      EtE(0, 0) += lambda * EtE(0, 0);
-      EtE(1, 1) += lambda * EtE(1, 1);
-      EtE(2, 2) += lambda * EtE(2, 2);
-    } else {
-      EtE(0, 0) += lambda;
-      EtE(1, 1) += lambda;
-      EtE(2, 2) += lambda;
-    }
-
-    return (EtE).inverse();
-  }
-
-  /// Assumes non-degenerate !
-  void computeEP(Matrix& E, Matrix& P, const Cameras& cameras,
-      const Point3& point) const {
-    whitenedError(cameras, point, E);
-    P = PointCov(E);
-  }
-
   /**
    *  Compute F, E, and b (called below in both vanilla and SVD versions), where
    *  F is a vector of derivatives wrpt the cameras, and E the stacked derivatives
    *  with respect to the point. The value of cameras/point are passed as parameters.
+   *  TODO: Kill this obsolete method
    */
-  double computeJacobians(std::vector<KeyMatrix2D>& Fblocks, Matrix& E,
+  template<class POINT>
-      Vector& b, const Cameras& cameras, const Point3& point) const {
+  void computeJacobians(std::vector<MatrixZD>& Fblocks, Matrix& E, Vector& b,
-
+      const Cameras& cameras, const POINT& point) const {
     // Project into Camera set and calculate derivatives
-    // TODO: if D==6 we optimize only camera pose. That is fairly hacky!
+    // As in expressionFactor, RHS vector b = - (h(x_bar) - z) = z-h(x_bar)
-    Matrix F, G;
+    // Indeed, nonlinear error |h(x_bar+dx)-z| ~ |h(x_bar) + A*dx - z|
-    using boost::none;
+    //                                         = |A*dx - (z-h(x_bar))|
-    boost::optional<Matrix&> optionalG(G);
+    b = -unwhitenedError(cameras, point, Fblocks, E);
-    b = whitenedError(cameras, point, F, E, D == 6 ? none : optionalG);
-
-    // Now calculate f and divide up the F derivatives into Fblocks
-    double f = 0.0;
-    size_t m = keys_.size();
-    for (size_t i = 0, row = 0; i < m; i++, row += ZDim) {
-
-      // Accumulate normalized error
-      f += b.segment<ZDim>(row).squaredNorm();
-
-      // Get piece of F and possibly G
-      Matrix2D Fi;
-      if (D == 6)
-        Fi << F.block<ZDim, 6>(row, 0);
-      else
-        Fi << F.block<ZDim, 6>(row, 0), G.block<ZDim, D - 6>(row, 0);
-
-      // Push it onto Fblocks
-      Fblocks.push_back(KeyMatrix2D(keys_[i], Fi));
-    }
-    return f;
-  }
-
-  /// Create BIG block-diagonal matrix F from Fblocks
-  static void FillDiagonalF(const std::vector<KeyMatrix2D>& Fblocks, Matrix& F) {
-    size_t m = Fblocks.size();
-    F.resize(ZDim * m, D * m);
-    F.setZero();
-    for (size_t i = 0; i < m; ++i)
-      F.block<This::ZDim, D>(This::ZDim * i, D * i) = Fblocks.at(i).second;
-  }
-
-  /**
-   *  Compute F, E, and b, where F and E are the stacked derivatives
-   *  with respect to the point. The value of cameras/point are passed as parameters.
-   */
-  double computeJacobians(Matrix& F, Matrix& E, Vector& b,
-      const Cameras& cameras, const Point3& point) const {
-    std::vector<KeyMatrix2D> Fblocks;
-    double f = computeJacobians(Fblocks, E, b, cameras, point);
-    FillDiagonalF(Fblocks, F);
-    return f;
   }
 
   /// SVD version
-  double computeJacobiansSVD(std::vector<KeyMatrix2D>& Fblocks, Matrix& Enull,
+  template<class POINT>
-      Vector& b, const Cameras& cameras, const Point3& point) const {
+  void computeJacobiansSVD(std::vector<MatrixZD>& Fblocks, Matrix& Enull,
+      Vector& b, const Cameras& cameras, const POINT& point) const {
 
     Matrix E;
-    double f = computeJacobians(Fblocks, E, b, cameras, point);
+    computeJacobians(Fblocks, E, b, cameras, point);
+
+    static const int N = FixedDimension<POINT>::value; // 2 (Unit3) or 3 (Point3)
 
     // Do SVD on A
     Eigen::JacobiSVD<Matrix> svd(E, Eigen::ComputeFullU);
     Vector s = svd.singularValues();
     size_t m = this->keys_.size();
-    // Enull = zeros(ZDim * m, ZDim * m - 3);
+    Enull = svd.matrixU().block(0, N, ZDim * m, ZDim * m - N); // last ZDim*m-N columns
-    Enull = svd.matrixU().block(0, 3, ZDim * m, ZDim * m - 3); // last ZDim*m-3 columns
-
-    return f;
   }
 
-  /// Matrix version of SVD
+  /// Linearize to a Hessianfactor
-  // TODO, there should not be a Matrix version, really
+  boost::shared_ptr<RegularHessianFactor<Dim> > createHessianFactor(
-  double computeJacobiansSVD(Matrix& F, Matrix& Enull, Vector& b,
-      const Cameras& cameras, const Point3& point) const {
-    std::vector<KeyMatrix2D> Fblocks;
-    double f = computeJacobiansSVD(Fblocks, Enull, b, cameras, point);
-    FillDiagonalF(Fblocks, F);
-    return f;
-  }
-
-  /**
-   * Linearize returns a Hessianfactor that is an approximation of error(p)
-   */
-  boost::shared_ptr<RegularHessianFactor<D> > createHessianFactor(
       const Cameras& cameras, const Point3& point, const double lambda = 0.0,
       bool diagonalDamping = false) const {
 
-    int numKeys = this->keys_.size();
+    std::vector<MatrixZD> Fblocks;
-
-    std::vector<KeyMatrix2D> Fblocks;
     Matrix E;
     Vector b;
-    double f = computeJacobians(Fblocks, E, b, cameras, point);
+    computeJacobians(Fblocks, E, b, cameras, point);
-    Matrix3 P = PointCov(E, lambda, diagonalDamping);
 
-//#define HESSIAN_BLOCKS // slower, as internally the Hessian factor will transform the blocks into SymmetricBlockMatrix
+    // build augmented hessian
-#ifdef HESSIAN_BLOCKS
+    SymmetricBlockMatrix augmentedHessian = Cameras::SchurComplement(Fblocks, E,
-    // Create structures for Hessian Factors
+        b);
-    std::vector < Matrix > Gs(numKeys * (numKeys + 1) / 2);
-    std::vector < Vector > gs(numKeys);
 
-    sparseSchurComplement(Fblocks, E, P, b, Gs, gs);
+    return boost::make_shared<RegularHessianFactor<Dim> >(keys_,
-    // schurComplement(Fblocks, E, P, b, Gs, gs);
-
-    //std::vector < Matrix > Gs2(Gs.begin(), Gs.end());
-    //std::vector < Vector > gs2(gs.begin(), gs.end());
-
-    return boost::make_shared < RegularHessianFactor<D> > (this->keys_, Gs, gs, f);
-#else // we create directly a SymmetricBlockMatrix
-    size_t n1 = D * numKeys + 1;
-    std::vector<DenseIndex> dims(numKeys + 1); // this also includes the b term
-    std::fill(dims.begin(), dims.end() - 1, D);
-    dims.back() = 1;
-
-    SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(n1, n1)); // for 10 cameras, size should be (10*D+1 x 10*D+1)
-    sparseSchurComplement(Fblocks, E, P, b, augmentedHessian); // augmentedHessian.matrix().block<D,D> (i1,i2) = ...
-    augmentedHessian(numKeys, numKeys)(0, 0) = f;
-    return boost::make_shared<RegularHessianFactor<D> >(this->keys_,
         augmentedHessian);
-#endif
-  }
-
-  /**
-   * Do Schur complement, given Jacobian as F,E,P.
-   * Slow version - works on full matrices
-   */
-  void schurComplement(const std::vector<KeyMatrix2D>& Fblocks, const Matrix& E,
-      const Matrix3& P, const Vector& b,
-      /*output ->*/std::vector<Matrix>& Gs, std::vector<Vector>& gs) const {
-    // Schur complement trick
-    // Gs = F' * F - F' * E * inv(E'*E) * E' * F
-    // gs = F' * (b - E * inv(E'*E) * E' * b)
-    // This version uses full matrices
-
-    int numKeys = this->keys_.size();
-
-    /// Compute Full F ????
-    Matrix F;
-    FillDiagonalF(Fblocks, F);
-
-    Matrix H(D * numKeys, D * numKeys);
-    Vector gs_vector;
-
-    H.noalias() = F.transpose() * (F - (E * (P * (E.transpose() * F))));
-    gs_vector.noalias() = F.transpose() * (b - (E * (P * (E.transpose() * b))));
-
-    // Populate Gs and gs
-    int GsCount2 = 0;
-    for (DenseIndex i1 = 0; i1 < numKeys; i1++) { // for each camera
-      DenseIndex i1D = i1 * D;
-      gs.at(i1) = gs_vector.segment<D>(i1D);
-      for (DenseIndex i2 = 0; i2 < numKeys; i2++) {
-        if (i2 >= i1) {
-          Gs.at(GsCount2) = H.block<D, D>(i1D, i2 * D);
-          GsCount2++;
-        }
-      }
-    }
-  }
-
-  /**
-   * Do Schur complement, given Jacobian as F,E,P, return SymmetricBlockMatrix
-   * Fast version - works on with sparsity
-   */
-  void sparseSchurComplement(const std::vector<KeyMatrix2D>& Fblocks,
-      const Matrix& E, const Matrix3& P /*Point Covariance*/, const Vector& b,
-      /*output ->*/SymmetricBlockMatrix& augmentedHessian) const {
-    // Schur complement trick
-    // Gs = F' * F - F' * E * P * E' * F
-    // gs = F' * (b - E * P * E' * b)
-
-    // a single point is observed in numKeys cameras
-    size_t numKeys = this->keys_.size();
-
-    // Blockwise Schur complement
-    for (size_t i1 = 0; i1 < numKeys; i1++) { // for each camera
-
-      const Matrix2D& Fi1 = Fblocks.at(i1).second;
-      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<ZDim>(ZDim * i1) // F' * b
-      - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1)
-
-      // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
-      augmentedHessian(i1, i1) = 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
-        const Matrix2D& Fi2 = Fblocks.at(i2).second;
-
-        // (DxD) = (Dx2) * ( (2x2) * (2xD) )
-        augmentedHessian(i1, i2) = -Fi1.transpose()
-            * (Ei1_P * E.block<ZDim, 3>(ZDim * i2, 0).transpose() * Fi2);
-      }
-    } // end of for over cameras
-  }
-
-  /**
-   * Do Schur complement, given Jacobian as F,E,P, return Gs/gs
-   * Fast version - works on with sparsity
-   */
-  void sparseSchurComplement(const std::vector<KeyMatrix2D>& Fblocks,
-      const Matrix& E, const Matrix3& P /*Point Covariance*/, const Vector& b,
-      /*output ->*/std::vector<Matrix>& Gs, std::vector<Vector>& gs) const {
-    // Schur complement trick
-    // Gs = F' * F - F' * E * P * E' * F
-    // gs = F' * (b - E * P * E' * b)
-
-    // a single point is observed in numKeys cameras
-    size_t numKeys = this->keys_.size();
-
-    int GsIndex = 0;
-    // Blockwise Schur complement
-    for (size_t i1 = 0; i1 < numKeys; i1++) { // for each camera
-      // GsIndex points to the upper triangular blocks
-      // 0  1  2  3  4
-      // X  5  6  7  8
-      // X  X  9 10 11
-      // X  X  X 12 13
-      // X  X  X X  14
-      const Matrix2D& Fi1 = Fblocks.at(i1).second;
-
-      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<ZDim>(ZDim * i1) // F' * b
-        - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1)
-
-        // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
-        Gs.at(GsIndex) = Fi1.transpose()
-            * (Fi1 - Ei1_P * E.block<ZDim, 3>(ZDim * i1, 0).transpose() * Fi1);
-        GsIndex++;
-      }
-      // upper triangular part of the hessian
-      for (size_t i2 = i1 + 1; i2 < numKeys; i2++) { // for each camera
-        const Matrix2D& Fi2 = Fblocks.at(i2).second;
-
-        // (DxD) = (Dx2) * ( (2x2) * (2xD) )
-        Gs.at(GsIndex) = -Fi1.transpose()
-            * (Ei1_P * E.block<ZDim, 3>(ZDim * i2, 0).transpose() * Fi2);
-        GsIndex++;
-      }
-    } // end of for over cameras
-  }
-
-  /**
-   * Applies Schur complement (exploiting block structure) to get a smart factor on cameras,
-   * and adds the contribution of the smart factor to a pre-allocated augmented Hessian.
-   */
-  void updateSparseSchurComplement(const std::vector<KeyMatrix2D>& Fblocks,
-      const Matrix& E, const Matrix3& P /*Point Covariance*/, const Vector& b,
-      const double f, const FastVector<Key> allKeys,
-      /*output ->*/SymmetricBlockMatrix& augmentedHessian) const {
-    // Schur complement trick
-    // Gs = F' * F - F' * E * P * E' * F
-    // gs = F' * (b - E * P * E' * b)
-
-    MatrixDD matrixBlock;
-
-    FastMap<Key, size_t> KeySlotMap;
-    for (size_t slot = 0; slot < allKeys.size(); slot++)
-      KeySlotMap.insert(std::make_pair(allKeys[slot], slot));
-
-    // a single point is observed in numKeys cameras
-    size_t numKeys = this->keys_.size(); // cameras observing current point
-    size_t aug_numKeys = (augmentedHessian.rows() - 1) / D; // all cameras in the group
-
-    // Blockwise Schur complement
-    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<ZDim, 3>(ZDim * i1, 0) * P;
-
-      // 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];
-      DenseIndex aug_i1 = KeySlotMap[this->keys_[i1]];
-
-      // information vector - store previous vector
-      // 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<ZDim>(ZDim * i1) // F' * b
-      - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1)
-
-      // (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<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
-        const Matrix2D& Fi2 = Fblocks.at(i2).second;
-
-        //Key cameraKey_i2 = this->keys_[i2];
-        DenseIndex aug_i2 = KeySlotMap[this->keys_[i2]];
-
-        // (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<ZDim, 3>(ZDim * i2, 0).transpose() * Fi2);
-      }
-    } // end of for over cameras
-
-    augmentedHessian(aug_numKeys, aug_numKeys)(0, 0) += f;
   }
 
   /**
@@ -712,73 +321,100 @@ public:
       const double lambda, bool diagonalDamping,
       SymmetricBlockMatrix& augmentedHessian,
       const FastVector<Key> allKeys) const {
-
-    // int numKeys = this->keys_.size();
-
-    std::vector<KeyMatrix2D> Fblocks;
     Matrix E;
     Vector b;
-    double f = computeJacobians(Fblocks, E, b, cameras, point);
+    std::vector<MatrixZD> Fblocks;
-    Matrix3 P = PointCov(E, lambda, diagonalDamping);
+    computeJacobians(Fblocks, E, b, cameras, point);
-    updateSparseSchurComplement(Fblocks, E, P, b, f, allKeys, augmentedHessian); // augmentedHessian.matrix().block<D,D> (i1,i2) = ...
+    Cameras::UpdateSchurComplement(Fblocks, E, b, allKeys, keys_,
+        augmentedHessian);
   }
 
-  /**
+  /// Whiten the Jacobians computed by computeJacobians using noiseModel_
-   * Return Jacobians as RegularImplicitSchurFactor with raw access
+  void whitenJacobians(std::vector<MatrixZD>& F, Matrix& E, Vector& b) const {
-   */
+    noiseModel_->WhitenSystem(E, b);
-  boost::shared_ptr<RegularImplicitSchurFactor<D> > createRegularImplicitSchurFactor(
+    // TODO make WhitenInPlace work with any dense matrix type
-      const Cameras& cameras, const Point3& point, double lambda = 0.0,
+    for (size_t i = 0; i < F.size(); i++)
-      bool diagonalDamping = false) const {
+      F[i] = noiseModel_->Whiten(F[i]);
-    typename boost::shared_ptr<RegularImplicitSchurFactor<D> > f(
+  }
-        new RegularImplicitSchurFactor<D>());
+
-    computeJacobians(f->Fblocks(), f->E(), f->b(), cameras, point);
+  /// Return Jacobians as RegularImplicitSchurFactor with raw access
-    f->PointCovariance() = PointCov(f->E(), lambda, diagonalDamping);
+  boost::shared_ptr<RegularImplicitSchurFactor<CAMERA> > //
-    f->initKeys();
+  createRegularImplicitSchurFactor(const Cameras& cameras, const Point3& point,
-    return f;
+      double lambda = 0.0, bool diagonalDamping = false) const {
+    Matrix E;
+    Vector b;
+    std::vector<MatrixZD> F;
+    computeJacobians(F, E, b, cameras, point);
+    whitenJacobians(F, E, b);
+    Matrix P = Cameras::PointCov(E, lambda, diagonalDamping);
+    return boost::make_shared<RegularImplicitSchurFactor<CAMERA> >(keys_, F, E,
+        P, b);
   }
 
   /**
    * Return Jacobians as JacobianFactorQ
    */
-  boost::shared_ptr<JacobianFactorQ<D, ZDim> > createJacobianQFactor(
+  boost::shared_ptr<JacobianFactorQ<Dim, ZDim> > createJacobianQFactor(
       const Cameras& cameras, const Point3& point, double lambda = 0.0,
       bool diagonalDamping = false) const {
-    std::vector<KeyMatrix2D> Fblocks;
     Matrix E;
     Vector b;
-    computeJacobians(Fblocks, E, b, cameras, point);
+    std::vector<MatrixZD> F;
-    Matrix3 P = PointCov(E, lambda, diagonalDamping);
+    computeJacobians(F, E, b, cameras, point);
-    return boost::make_shared<JacobianFactorQ<D, ZDim> >(Fblocks, E, P, b);
+    const size_t M = b.size();
+    Matrix P = Cameras::PointCov(E, lambda, diagonalDamping);
+    SharedIsotropic n = noiseModel::Isotropic::Sigma(M, noiseModel_->sigma());
+    return boost::make_shared<JacobianFactorQ<Dim, ZDim> >(keys_, F, E, P, b, n);
   }
 
   /**
-   * Return Jacobians as JacobianFactor
+   * Return Jacobians as JacobianFactorSVD
    * TODO lambda is currently ignored
    */
   boost::shared_ptr<JacobianFactor> createJacobianSVDFactor(
       const Cameras& cameras, const Point3& point, double lambda = 0.0) const {
-    size_t numKeys = this->keys_.size();
+    size_t m = this->keys_.size();
-    std::vector<KeyMatrix2D> Fblocks;
+    std::vector<MatrixZD> F;
     Vector b;
-    Matrix Enull(ZDim * numKeys, ZDim * numKeys - 3);
+    const size_t M = ZDim * m;
-    computeJacobiansSVD(Fblocks, Enull, b, cameras, point);
+    Matrix E0(M, M - 3);
-    return boost::make_shared<JacobianFactorSVD<6, ZDim> >(Fblocks, Enull, b);
+    computeJacobiansSVD(F, E0, b, cameras, point);
+    SharedIsotropic n = noiseModel::Isotropic::Sigma(M - 3,
+        noiseModel_->sigma());
+    return boost::make_shared<JacobianFactorSVD<Dim, ZDim> >(keys_, F, E0, b, n);
+  }
+
+  /// Create BIG block-diagonal matrix F from Fblocks
+  static void FillDiagonalF(const std::vector<MatrixZD>& Fblocks, Matrix& F) {
+    size_t m = Fblocks.size();
+    F.resize(ZDim * m, Dim * m);
+    F.setZero();
+    for (size_t i = 0; i < m; ++i)
+      F.block<ZDim, Dim>(ZDim * i, Dim * i) = Fblocks.at(i);
+  }
+
+
+  Pose3 body_P_sensor() const{
+    if(body_P_sensor_)
+      return *body_P_sensor_;
+    else
+      return Pose3(); // if unspecified, the transformation is the identity
   }
 
 private:
 
-  /// Serialization function
+/// 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(measured_);
-    ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
   }
-}
+};
-;
+// end class SmartFactorBase
 
-template<class CAMERA, size_t D>
+// Definitions need to avoid link errors (above are only declarations)
-const int SmartFactorBase<CAMERA, D>::ZDim;
+template<class CAMERA> const int SmartFactorBase<CAMERA>::Dim;
+template<class CAMERA> const int SmartFactorBase<CAMERA>::ZDim;
 
 } // \ namespace gtsam

gtsam/slam/SmartProjectionFactor.h

@@ -22,7 +22,6 @@
 #include <gtsam/slam/SmartFactorBase.h>
 
 #include <gtsam/geometry/triangulation.h>
-#include <gtsam/geometry/Pose3.h>
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/slam/dataset.h>
 
@@ -32,108 +31,135 @@
 
 namespace gtsam {
 
-/**
+/// Linearization mode: what factor to linearize to
- * Structure for storing some state memory, used to speed up optimization
+enum LinearizationMode {
- * @addtogroup SLAM
+  HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD
- */
+};
-class SmartProjectionFactorState {
 
-protected:
+/// How to manage degeneracy
+enum DegeneracyMode {
+  IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY
+};
+
+/*
+ *  Parameters for the smart projection factors
+ */
+class GTSAM_EXPORT SmartProjectionParams {
 
 public:
 
-  SmartProjectionFactorState() {
+  LinearizationMode linearizationMode; ///< How to linearize the factor
-  }
+  DegeneracyMode degeneracyMode; ///< How to linearize the factor
-  // Hessian representation (after Schur complement)
-  bool calculatedHessian;
-  Matrix H;
-  Vector gs_vector;
-  std::vector<Matrix> Gs;
-  std::vector<Vector> gs;
-  double f;
-};
 
-enum LinearizationMode {
+  /// @name Parameters governing the triangulation
-  HESSIAN, JACOBIAN_SVD, JACOBIAN_Q
+  /// @{
+  mutable TriangulationParameters triangulation;
+  const 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)
+  const bool verboseCheirality; ///< If true, prints text for Cheirality exceptions (default: false)
+  /// @}
+
+  // Constructor
+  SmartProjectionParams(LinearizationMode linMode = HESSIAN,
+      DegeneracyMode degMode = IGNORE_DEGENERACY, bool throwCheirality = false,
+      bool verboseCheirality = false) :
+      linearizationMode(linMode), degeneracyMode(degMode), retriangulationThreshold(
+          1e-5), throwCheirality(throwCheirality), verboseCheirality(
+          verboseCheirality) {
+  }
+
+  virtual ~SmartProjectionParams() {
+  }
+
+  void print(const std::string& str) const {
+    std::cout << "linearizationMode: " << linearizationMode << "\n";
+    std::cout << "   degeneracyMode: " << degeneracyMode << "\n";
+    std::cout << triangulation << std::endl;
+  }
+
+  LinearizationMode getLinearizationMode() const {
+    return linearizationMode;
+  }
+  DegeneracyMode getDegeneracyMode() const {
+    return degeneracyMode;
+  }
+  TriangulationParameters getTriangulationParameters() const {
+    return triangulation;
+  }
+  bool getVerboseCheirality() const {
+    return verboseCheirality;
+  }
+  bool getThrowCheirality() const {
+    return throwCheirality;
+  }
+  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(bool landmarkDistanceThreshold) {
+    triangulation.landmarkDistanceThreshold = landmarkDistanceThreshold;
+  }
+  void setDynamicOutlierRejectionThreshold(bool dynOutRejectionThreshold) {
+    triangulation.dynamicOutlierRejectionThreshold = dynOutRejectionThreshold;
+  }
 };
 
 /**
  * SmartProjectionFactor: triangulates point and keeps an estimate of it around.
  */
-template<class CALIBRATION, size_t D>
+template<class CAMERA>
-class SmartProjectionFactor: public SmartFactorBase<PinholeCamera<CALIBRATION>,
+class SmartProjectionFactor: public SmartFactorBase<CAMERA> {
-    D> {
+
+public:
+
+private:
+  typedef SmartFactorBase<CAMERA> Base;
+  typedef SmartProjectionFactor<CAMERA> This;
+  typedef SmartProjectionFactor<CAMERA> SmartProjectionCameraFactor;
+
 protected:
 
-  // Some triangulation parameters
+  /// @name Parameters
-  const double rankTolerance_; ///< threshold to decide whether triangulation is degenerate_
+  /// @{
-  const double retriangulationThreshold_; ///< threshold to decide whether to re-triangulate
+  const SmartProjectionParams params_;
+  /// @}
+
+  /// @name Caching triangulation
+  /// @{
+  mutable TriangulationResult result_; ///< result from triangulateSafe
   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<SmartProjectionFactorState> state_;
-
-  /// shorthand for smart projection factor state variable
-  typedef boost::shared_ptr<SmartProjectionFactorState> SmartFactorStatePtr;
-
-  /// shorthand for base class type
-  typedef SmartFactorBase<PinholeCamera<CALIBRATION>, 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 SmartProjectionFactor<CALIBRATION, D> This;
 
 public:
 
   /// shorthand for a smart pointer to a factor
   typedef boost::shared_ptr<This> shared_ptr;
 
-  /// shorthand for a pinhole camera
+  /// shorthand for a set of cameras
-  typedef PinholeCamera<CALIBRATION> Camera;
+  typedef CameraSet<CAMERA> Cameras;
-  typedef CameraSet<Camera> Cameras;
 
   /**
    * Constructor
-   * @param rankTol tolerance used to check if point triangulation is degenerate
+   * @param body_P_sensor pose of the camera in the body frame
-   * @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
-   * @param body_P_sensor is the transform from sensor to body frame (default identity)
    */
-  SmartProjectionFactor(const double rankTol, const double linThreshold,
+  SmartProjectionFactor(
-      const bool manageDegeneracy, const bool enableEPI,
+      const boost::optional<Pose3> body_P_sensor = boost::none,
-      boost::optional<Pose3> body_P_sensor = boost::none,
+      const SmartProjectionParams& params = SmartProjectionParams()) :
-      double landmarkDistanceThreshold = 1e10,
+      Base(body_P_sensor), params_(params), //
-      double dynamicOutlierRejectionThreshold = -1, SmartFactorStatePtr state =
+      result_(TriangulationResult::Degenerate()) {
-          SmartFactorStatePtr(new SmartProjectionFactorState())) :
-      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 */
@@ -147,24 +173,33 @@ public:
    */
   void print(const std::string& s = "", const KeyFormatter& keyFormatter =
       DefaultKeyFormatter) const {
-    std::cout << s << "SmartProjectionFactor, z = \n";
+    std::cout << s << "SmartProjectionFactor\n";
-    std::cout << "rankTolerance_ = " << rankTolerance_ << std::endl;
+    std::cout << "linearizationMode:\n" << params_.linearizationMode
-    std::cout << "degenerate_ = " << degenerate_ << std::endl;
+        << std::endl;
-    std::cout << "cheiralityException_ = " << cheiralityException_ << std::endl;
+    std::cout << "triangulationParameters:\n" << params_.triangulation
+        << std::endl;
+    std::cout << "result:\n" << result_ << std::endl;
     Base::print("", keyFormatter);
   }
 
-  /// Check if the new linearization point_ is the same as the one used for previous triangulation
+  /// equals
+  virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
+    const This *e = dynamic_cast<const This*>(&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
+    // 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_
+    // 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 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;
@@ -172,7 +207,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;
         }
@@ -187,137 +222,34 @@ public:
         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
+    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 {
+  TriangulationResult triangulateSafe(const Cameras& cameras) 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
+    if (m < 2) // if we have a single pose the corresponding factor is uninformative
-      degenerate_ = true;
+      return TriangulationResult::Degenerate();
-      return m;
+
-    }
     bool retriangulate = decideIfTriangulate(cameras);
-
+    if (retriangulate)
-    if (retriangulate) {
+      result_ = gtsam::triangulateSafe(cameras, this->measured_,
-      // We triangulate the 3D position of the landmark
+          params_.triangulation);
-      try {
+    return result_;
-        // std::cout << "triangulatePoint3 i \n" << rankTolerance << std::endl;
-        point_ = triangulatePoint3<Camera>(cameras, this->measured_,
-            rankTolerance_, enableEPI_);
-        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 Point2& zi = this->measured_.at(i);
-          try {
-            Point2 reprojectionError(camera.project(point_) - zi);
-            totalReprojError += reprojectionError.vector().norm();
-          } catch (CheiralityException) {
-            cheiralityException_ = true;
-          }
-          i += 1;
-        }
-        // 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 {
-
+    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
-            << "createRegularImplicitSchurFactor: 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(
+  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;
@@ -331,264 +263,198 @@ public:
       exit(1);
     }
 
-    this->triangulateSafe(cameras);
+    triangulateSafe(cameras);
 
-    if (numKeys < 2
+    if (params_.degeneracyMode == ZERO_ON_DEGENERACY && !result_) {
-        || (!this->manageDegeneracy_
+      // failed: return"empty" Hessian
-            && (this->cheiralityException_ || this->degenerate_))) {
-      // std::cout << "In linearize: exception" << std::endl;
       BOOST_FOREACH(Matrix& m, Gs)
-        m = zeros(D, D);
+        m = zeros(Base::Dim, Base::Dim);
       BOOST_FOREACH(Vector& v, gs)
-        v = zero(D);
+        v = zero(Base::Dim);
-      return boost::make_shared<RegularHessianFactor<D> >(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<typename Base::MatrixZD> Fblocks;
-      this->degenerate_ = true;
+    Matrix E;
-    }
-
-    bool doLinearize = this->decideIfLinearize(cameras);
-
-    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;
     Vector b;
-    double f = computeJacobians(F, E, b, cameras);
+    computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras);
 
-    // 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(D * numKeys, D * numKeys);
-    Vector gs_vector;
 
-    Matrix3 P = Base::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)
-      std::cout << "gs_vector size " << gs_vector.size() << std::endl;
 
-    // Populate Gs and gs
+    return boost::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_,
-    int GsCount2 = 0;
+        augmentedHessian);
-    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<RegularImplicitSchurFactor<D> > createRegularImplicitSchurFactor(
+  boost::shared_ptr<RegularImplicitSchurFactor<CAMERA> > createRegularImplicitSchurFactor(
       const Cameras& cameras, double lambda) const {
     if (triangulateForLinearize(cameras))
-      return Base::createRegularImplicitSchurFactor(cameras, point_, lambda);
+      return Base::createRegularImplicitSchurFactor(cameras, *result_, lambda);
     else
-      return boost::shared_ptr<RegularImplicitSchurFactor<D> >();
+      // failed: return empty
+      return boost::shared_ptr<RegularImplicitSchurFactor<CAMERA> >();
   }
 
   /// create factor
-  boost::shared_ptr<JacobianFactorQ<D, 2> > createJacobianQFactor(
+  boost::shared_ptr<JacobianFactorQ<Base::Dim, 2> > 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<D, 2> >(this->keys_);
+      // failed: return empty
+      return boost::make_shared<JacobianFactorQ<Base::Dim, 2> >(this->keys_);
   }
 
   /// Create a factor, takes values
-  boost::shared_ptr<JacobianFactorQ<D, 2> > createJacobianQFactor(
+  boost::shared_ptr<JacobianFactorQ<Base::Dim, 2> > createJacobianQFactor(
       const Values& values, double lambda) const {
-    Cameras myCameras;
+    return createJacobianQFactor(this->cameras(values), lambda);
-    // TODO triangulate twice ??
-    bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras);
-    if (nonDegenerate)
-      return createJacobianQFactor(myCameras, lambda);
-    else
-      return boost::make_shared<JacobianFactorQ<D, 2> >(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<D, 2> >(this->keys_);
+      // failed: return empty
+      return boost::make_shared<JacobianFactorSVD<Base::Dim, 2> >(this->keys_);
   }
 
-  /// Returns true if nonDegenerate
+  /// linearize to a Hessianfactor
-  bool computeCamerasAndTriangulate(const Values& values,
+  virtual boost::shared_ptr<RegularHessianFactor<Base::Dim> > linearizeToHessian(
-      Cameras& myCameras) 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<CAMERA> > linearizeToImplicit(
-      valuesFactor.insert(key, values.at(key));
+      const Values& values, double lambda = 0.0) const {
+    return createRegularImplicitSchurFactor(this->cameras(values), lambda);
+  }
 
-    myCameras = this->cameras(valuesFactor);
+  /// linearize to a JacobianfactorQ
-    size_t nrCameras = this->triangulateSafe(myCameras);
+  virtual boost::shared_ptr<JacobianFactorQ<Base::Dim, 2> > linearizeToJacobian(
+      const Values& values, double lambda = 0.0) const {
+    return createJacobianQFactor(this->cameras(values), lambda);
+  }
 
-    if (nrCameras < 2
+  /**
-        || (!this->manageDegeneracy_
+   * Linearize to Gaussian Factor
-            && (this->cheiralityException_ || this->degenerate_)))
+   * @param values Values structure which must contain camera poses for this factor
-      return false;
+   * @return a Gaussian factor
-
+   */
-    // instead, if we want to manage the exception..
+  boost::shared_ptr<GaussianFactor> linearizeDamped(const Cameras& cameras,
-    if (this->cheiralityException_ || this->degenerate_) // if we want to manage the exceptions with rotation-only factors
+      const double lambda = 0.0) const {
-      this->degenerate_ = true;
+    // depending on flag set on construction we may linearize to different linear factors
-
+    switch (params_.linearizationMode) {
-    if (this->degenerate_) {
+    case HESSIAN:
-      std::cout << "SmartProjectionFactor: this is not ready" << std::endl;
+      return createHessianFactor(cameras, lambda);
-      std::cout << "this->cheiralityException_ " << this->cheiralityException_
+    case IMPLICIT_SCHUR:
-          << std::endl;
+      return createRegularImplicitSchurFactor(cameras, lambda);
-      std::cout << "this->degenerate_ " << this->degenerate_ << std::endl;
+    case JACOBIAN_SVD:
+      return createJacobianSVDFactor(cameras, lambda);
+    case JACOBIAN_Q:
+      return createJacobianQFactor(cameras, lambda);
+    default:
+      throw std::runtime_error("SmartFactorlinearize: unknown mode");
     }
-    return true;
   }
 
-  /// Assumes non-degenerate !
+  /**
-  void computeEP(Matrix& E, Matrix& P, const Cameras& cameras) const {
+   * Linearize to Gaussian Factor
-    return Base::computeEP(E, P, cameras, point_);
+   * @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);
   }
 
-  /// Takes values
+  /// linearize
-  bool computeEP(Matrix& E, Matrix& P, const Values& values) const {
+  virtual boost::shared_ptr<GaussianFactor> linearize(
-    Cameras myCameras;
+      const Values& values) const {
-    bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras);
+    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)
-      computeEP(E, P, myCameras);
+      cameras.project2(*result_, boost::none, E);
     return nonDegenerate;
   }
 
-  /// Version that takes values, and creates the point
+  /**
-  bool computeJacobians(std::vector<typename Base::KeyMatrix2D>& Fblocks,
+   * Triangulate and compute derivative of error with respect to point
-      Matrix& E, Vector& b, const Values& values) const {
+   * @return whether triangulation worked
-    Cameras myCameras;
+   */
-    bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras);
+  bool triangulateAndComputeE(Matrix& E, const Values& values) const {
-    if (nonDegenerate)
+    Cameras cameras = this->cameras(values);
-      computeJacobians(Fblocks, E, b, myCameras);
+    return triangulateAndComputeE(E, cameras);
-    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,
+  void computeJacobiansWithTriangulatedPoint(
-      Matrix& E, Vector& b, const Cameras& cameras) const {
+      std::vector<typename Base::MatrixZD>& Fblocks, Matrix& E, Vector& b,
+      const Cameras& cameras) const {
 
-    if (this->degenerate_) {
+    if (!result_) {
-      std::cout << "manage degeneracy " << manageDegeneracy_ << std::endl;
+      // Handle degeneracy
-      std::cout << "point " << point_ << std::endl;
+      // TODO check flag whether we should do this
-      std::cout
+      Unit3 backProjected = cameras[0].backprojectPointAtInfinity(
-          << "SmartProjectionFactor: Management of degeneracy is disabled - not ready to be used"
+          this->measured_.at(0));
-          << std::endl;
+      Base::computeJacobians(Fblocks, E, b, cameras, backProjected);
-      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
+      // valid result: just return Base version
-      return 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<typename 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 computeJacobiansSVD(std::vector<typename Base::KeyMatrix2D>& Fblocks,
+  bool triangulateAndComputeJacobiansSVD(
-      Matrix& Enull, Vector& b, const Values& values) const {
+      std::vector<typename Base::MatrixZD>& Fblocks, Matrix& Enull, Vector& b,
-    typename Base::Cameras myCameras;
+      const Values& values) const {
-    double good = computeCamerasAndTriangulate(values, myCameras);
+    Cameras cameras = this->cameras(values);
-    if (good)
+    bool nonDegenerate = triangulateForLinearize(cameras);
-      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, Vector& b,
-      const Cameras& cameras) const {
-    return Base::computeJacobians(F, E, b, cameras, point_);
-  }
-
-  /// 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);
+      Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, *result_);
+    return nonDegenerate;
+  }
+
+  /// Calculate vector of re-projection errors, before applying noise model
+  Vector reprojectionErrorAfterTriangulation(const Values& values) const {
+    Cameras cameras = this->cameras(values);
+    bool nonDegenerate = triangulateForLinearize(cameras);
+    if (nonDegenerate)
+      return Base::unwhitenedError(cameras, *result_);
     else
-      return zero(myCameras.size() * 2);
+      return zero(cameras.size() * 2);
   }
 
   /**
@@ -600,86 +466,57 @@ 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) {
+      // Otherwise, manage the exceptions with rotation-only factors
+      const Point2& 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
+  /// Calculate total reprojection error
-      std::cout
+  virtual double error(const Values& values) const {
-          << "SmartProjectionHessianFactor: cheirality exception (this should not happen if CheiralityException is disabled)!"
+    if (this->active(values)) {
-          << std::endl;
+      return totalReprojectionError(Base::cameras(values));
-      this->degenerate_ = true;
+    } else { // else of active flag
-    }
+      return 0.0;
-
-    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 Point2& zi = this->measured_.at(i);
-        if (i == 0) // first pose
-          this->point_ = camera.backprojectPointAtInfinity(zi); // 3D parametrization of point at infinity
-        Point2 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 {
+  TriangulationResult point() const {
-    return point_;
+    return result_;
   }
 
   /** 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);
+  }
+
+  /// Is result valid?
+  bool isValid() const {
+    return result_;
   }
 
   /** return the degenerate state */
-  inline bool isDegenerate() const {
+  bool isDegenerate() const {
-    return (cheiralityException_ || degenerate_);
+    return result_.degenerate();
   }
 
   /** return the cheirality status flag */
-  inline bool isPointBehindCamera() const {
+  bool isPointBehindCamera() const {
-    return cheiralityException_;
+    return result_.behindCamera();
-  }
-  /** return cheirality verbosity */
-  inline bool verboseCheirality() const {
-    return verboseCheirality_;
-  }
-
-  /** return flag for throwing cheirality exceptions */
-  inline bool throwCheirality() const {
-    return throwCheirality_;
   }
 
 private:
@@ -687,11 +524,18 @@ private:
   /// Serialization function
   friend class boost::serialization::access;
   template<class ARCHIVE>
-  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
+  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 CAMERA>
+struct traits<SmartProjectionFactor<CAMERA> > : public Testable<
+    SmartProjectionFactor<CAMERA> > {
 };
 
 } // \ namespace gtsam

gtsam/slam/SmartProjectionPoseFactor.h

@@ -38,87 +38,37 @@ namespace gtsam {
  * @addtogroup SLAM
  */
 template<class CALIBRATION>
-class SmartProjectionPoseFactor: public SmartProjectionFactor<CALIBRATION, 6> {
+class SmartProjectionPoseFactor: public SmartProjectionFactor<
+    PinholePose<CALIBRATION> > {
+
+private:
+  typedef PinholePose<CALIBRATION> Camera;
+  typedef SmartProjectionFactor<Camera> Base;
+  typedef SmartProjectionPoseFactor<CALIBRATION> This;
+
 protected:
 
-  LinearizationMode linearizeTo_;  ///< How to linearize the factor (HESSIAN, JACOBIAN_SVD, JACOBIAN_Q)
+  boost::shared_ptr<CALIBRATION> K_; ///< calibration object (one for all cameras)
-
-  std::vector<boost::shared_ptr<CALIBRATION> > K_all_; ///< shared pointer to calibration object (one for each camera)
 
 public:
 
-  /// shorthand for base class type
-  typedef SmartProjectionFactor<CALIBRATION, 6> Base;
-
-  /// shorthand for this class
-  typedef SmartProjectionPoseFactor<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 K (fixed) calibration, assumed to be the same for all cameras
-   * @param linThreshold threshold on relative pose changes used to decide whether to relinearize (selective relinearization)
+   * @param  body_P_sensor pose of the camera in the body frame
-   * @param manageDegeneracy is true, in presence of degenerate triangulation, the factor is converted to a rotation-only constraint,
+   * @param params internal parameters of the smart factors
-   * 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 sensor to body frame (default identity)
    */
-  SmartProjectionPoseFactor(const double rankTol = 1,
+  SmartProjectionPoseFactor(const boost::shared_ptr<CALIBRATION> K,
-      const double linThreshold = -1, const bool manageDegeneracy = false,
+      const boost::optional<Pose3> body_P_sensor = boost::none,
-      const bool enableEPI = false, boost::optional<Pose3> body_P_sensor = boost::none,
+      const SmartProjectionParams& params = SmartProjectionParams()) :
-      LinearizationMode linearizeTo = HESSIAN, double landmarkDistanceThreshold = 1e10,
+      Base(body_P_sensor, params), K_(K) {
-      double dynamicOutlierRejectionThreshold = -1) :
+  }
-        Base(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor,
-        landmarkDistanceThreshold, dynamicOutlierRejectionThreshold), linearizeTo_(linearizeTo) {}
 
   /** Virtual destructor */
-  virtual ~SmartProjectionPoseFactor() {}
+  virtual ~SmartProjectionPoseFactor() {
-
-  /**
-   * 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 Point2 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<Point2> 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<Point2> 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);
-    }
   }
 
   /**
@@ -129,59 +79,15 @@ public:
   void print(const std::string& s = "", const KeyFormatter& keyFormatter =
       DefaultKeyFormatter) const {
     std::cout << s << "SmartProjectionPoseFactor, 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);
   }
 
-  /**
-   * 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);
-      if(Base::body_P_sensor_)
-        pose = pose.compose(*(Base::body_P_sensor_));
-
-      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 :
-      return this->createJacobianQFactor(cameras(values), 0.0);
-      break;
-    default:
-      return this->createHessianFactor(cameras(values));
-      break;
-    }
-  }
-
   /**
    * error calculates the error of the factor.
    */
@@ -193,9 +99,28 @@ public:
     }
   }
 
-  /** return the calibration object */
+  /** return calibration shared pointers */
-  inline const std::vector<boost::shared_ptr<CALIBRATION> > calibration() const {
+  inline const boost::shared_ptr<CALIBRATION> calibration() const {
-    return K_all_;
+    return K_;
+  }
+
+  /**
+   * 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;
+    BOOST_FOREACH(const Key& k, this->keys_) {
+      Pose3 pose = values.at<Pose3>(k);
+      if (Base::body_P_sensor_)
+        pose = pose.compose(*(Base::body_P_sensor_));
+
+      Camera camera(pose, K_);
+      cameras.push_back(camera);
+    }
+    return cameras;
   }
 
 private:
@@ -205,10 +130,11 @@ private:
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
     ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
-    ar & BOOST_SERIALIZATION_NVP(K_all_);
+    ar & BOOST_SERIALIZATION_NVP(K_);
   }
 
-}; // end of class declaration
+};
+// end of class declaration
 
 /// traits
 template<class CALIBRATION>

gtsam/slam/TriangulationFactor.h

@@ -16,8 +16,7 @@
  */
 
 #include <gtsam/nonlinear/NonlinearFactor.h>
-#include <gtsam/geometry/SimpleCamera.h>
+#include <gtsam/geometry/CalibratedCamera.h>
-#include <boost/optional.hpp>
 #include <boost/make_shared.hpp>
 
 namespace gtsam {
@@ -27,18 +26,24 @@ namespace gtsam {
  * The calibration and pose are assumed known.
  * @addtogroup SLAM
  */
-template<class CALIBRATION = Cal3_S2>
+template<class CAMERA>
 class TriangulationFactor: public NoiseModelFactor1<Point3> {
 
 public:
 
-  /// Camera type
+  /// CAMERA type
-  typedef PinholeCamera<CALIBRATION> Camera;
+  typedef CAMERA Camera;
 
 protected:
 
+  /// shorthand for base class type
+  typedef NoiseModelFactor1<Point3> Base;
+
+  /// shorthand for this class
+  typedef TriangulationFactor<CAMERA> This;
+
   // Keep a copy of measurement and calibration for I/O
-  const Camera camera_; ///< Camera in which this landmark was seen
+  const CAMERA camera_; ///< CAMERA in which this landmark was seen
   const Point2 measured_; ///< 2D measurement
 
   // verbosity handling for Cheirality Exceptions
@@ -47,12 +52,6 @@ protected:
 
 public:
 
-  /// shorthand for base class type
-  typedef NoiseModelFactor1<Point3> Base;
-
-  /// shorthand for this class
-  typedef TriangulationFactor<CALIBRATION> This;
-
   /// shorthand for a smart pointer to a factor
   typedef boost::shared_ptr<This> shared_ptr;
 
@@ -70,7 +69,7 @@ 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 Point2& measured,
       const SharedNoiseModel& model, Key pointKey, bool throwCheirality = false,
       bool verboseCheirality = false) :
       Base(model, pointKey), camera_(camera), measured_(measured), throwCheirality_(
@@ -114,7 +113,7 @@ public:
   Vector evaluateError(const Point3& point, boost::optional<Matrix&> H2 =
       boost::none) const {
     try {
-      Point2 error(camera_.project(point, boost::none, H2, boost::none) - measured_);
+      Point2 error(camera_.project2(point, boost::none, H2) - measured_);
       return error.vector();
     } catch (CheiralityException& e) {
       if (H2)
@@ -154,7 +153,7 @@ public:
 
     // Would be even better if we could pass blocks to project
     const Point3& point = x.at<Point3>(key());
-    b = -(camera_.project(point, boost::none, A, boost::none) - measured_).vector();
+    b = -(camera_.project2(point, boost::none, A) - measured_).vector();
     if (noiseModel_)
       this->noiseModel_->WhitenSystem(A, b);
 

gtsam/slam/expressions.h

@@ -28,6 +28,7 @@ inline Point2_ transform_to(const Pose2_& x, const Point2_& p) {
 // 3D Geometry
 
 typedef Expression<Point3> Point3_;
+typedef Expression<Unit3> Unit3_;
 typedef Expression<Rot3> Rot3_;
 typedef Expression<Pose3> Pose3_;
 
@@ -40,33 +41,52 @@ inline Point3_ transform_to(const Pose3_& x, const Point3_& p) {
 typedef Expression<Cal3_S2> Cal3_S2_;
 typedef Expression<Cal3Bundler> Cal3Bundler_;
 
+/// Expression version of PinholeBase::Project
 inline Point2_ project(const Point3_& p_cam) {
-  return Point2_(PinholeCamera<Cal3_S2>::project_to_camera, p_cam);
+  Point2 (*f)(const Point3&, OptionalJacobian<2, 3>) = &PinholeBase::Project;
+  return Point2_(f, p_cam);
 }
 
-template <class CAMERA>
+inline Point2_ project(const Unit3_& p_cam) {
-Point2 project4(const CAMERA& camera, const Point3& p,
+  Point2 (*f)(const Unit3&, OptionalJacobian<2, 2>) = &PinholeBase::Project;
-    OptionalJacobian<2, CAMERA::dimension> Dcam, OptionalJacobian<2, 3> Dpoint) {
+  return Point2_(f, p_cam);
+}
+
+namespace internal {
+// Helper template for project2 expression below
+template<class CAMERA, class POINT>
+Point2 project4(const CAMERA& camera, const POINT& p,
+    OptionalJacobian<2, CAMERA::dimension> Dcam,
+    OptionalJacobian<2, FixedDimension<POINT>::value> Dpoint) {
   return camera.project2(p, Dcam, Dpoint);
 }
-
-template <class CAMERA>
-Point2_ project2(const Expression<CAMERA>& camera_, const Point3_& p_) {
-  return Point2_(project4<CAMERA>, camera_, p_);
 }
 
+template<class CAMERA, class POINT>
+Point2_ project2(const Expression<CAMERA>& camera_,
+    const Expression<POINT>& p_) {
+  return Point2_(internal::project4<CAMERA, POINT>, camera_, p_);
+}
+
+namespace internal {
+// Helper template for project3 expression below
+template<class CALIBRATION, class POINT>
 inline Point2 project6(const Pose3& x, const Point3& p, const Cal3_S2& K,
-    OptionalJacobian<2, 6> Dpose, OptionalJacobian<2, 3> Dpoint, OptionalJacobian<2, 5> Dcal) {
+    OptionalJacobian<2, 6> Dpose, OptionalJacobian<2, 3> Dpoint,
+    OptionalJacobian<2, 5> Dcal) {
   return PinholeCamera<Cal3_S2>(x, K).project(p, Dpose, Dpoint, Dcal);
 }
-
-inline Point2_ project3(const Pose3_& x, const Point3_& p, const Cal3_S2_& K) {
-  return Point2_(project6, x, p, K);
 }
 
-template<class CAL>
+template<class CALIBRATION, class POINT>
-Point2_ uncalibrate(const Expression<CAL>& K, const Point2_& xy_hat) {
+inline Point2_ project3(const Pose3_& x, const Expression<POINT>& p,
-  return Point2_(K, &CAL::uncalibrate, xy_hat);
+    const Expression<CALIBRATION>& K) {
+  return Point2_(internal::project6<CALIBRATION, POINT>, x, p, K);
+}
+
+template<class CALIBRATION>
+Point2_ uncalibrate(const Expression<CALIBRATION>& K, const Point2_& xy_hat) {
+  return Point2_(K, &CALIBRATION::uncalibrate, xy_hat);
 }
 
 } // \namespace gtsam

gtsam/slam/tests/smartFactorScenarios.h

@@ -0,0 +1,145 @@
+/* ----------------------------------------------------------------------------
+
+ * 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  SmartFactorScenarios.h
+ *  @brief Scenarios for testSmart*.cpp
+ *  @author Frank Dellaert
+ *  @date   Feb 2015
+ */
+
+#pragma once
+#include <gtsam/slam/SmartProjectionPoseFactor.h>
+#include <gtsam/slam/SmartProjectionFactor.h>
+#include <gtsam/slam/GeneralSFMFactor.h>
+#include <gtsam/geometry/Cal3_S2.h>
+#include <gtsam/geometry/Cal3Bundler.h>
+
+using namespace std;
+using namespace gtsam;
+
+// three landmarks ~5 meters infront of camera
+Point3 landmark1(5, 0.5, 1.2);
+Point3 landmark2(5, -0.5, 1.2);
+Point3 landmark3(3, 0, 3.0);
+Point3 landmark4(10, 0.5, 1.2);
+Point3 landmark5(10, -0.5, 1.2);
+
+// First camera pose, looking along X-axis, 1 meter above ground plane (x-y)
+Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
+// Second camera 1 meter to the right of first camera
+Pose3 pose_right = level_pose * Pose3(Rot3(), Point3(1, 0, 0));
+// Third camera 1 meter above the first camera
+Pose3 pose_above = level_pose * Pose3(Rot3(), Point3(0, -1, 0));
+
+// Create a noise unit2 for the pixel error
+static SharedNoiseModel unit2(noiseModel::Unit::Create(2));
+
+static double fov = 60; // degrees
+static size_t w = 640, h = 480;
+
+/* ************************************************************************* */
+// default Cal3_S2 cameras
+namespace vanilla {
+typedef PinholeCamera<Cal3_S2> Camera;
+typedef SmartProjectionFactor<Camera> SmartFactor;
+static Cal3_S2 K(fov, w, h);
+static Cal3_S2 K2(1500, 1200, 0, w, h);
+Camera level_camera(level_pose, K2);
+Camera level_camera_right(pose_right, K2);
+Point2 level_uv = level_camera.project(landmark1);
+Point2 level_uv_right = level_camera_right.project(landmark1);
+Camera cam1(level_pose, K2);
+Camera cam2(pose_right, K2);
+Camera cam3(pose_above, K2);
+typedef GeneralSFMFactor<Camera, Point3> SFMFactor;
+SmartProjectionParams params;
+}
+
+/* ************************************************************************* */
+// default Cal3_S2 poses
+namespace vanillaPose {
+typedef PinholePose<Cal3_S2> Camera;
+typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor;
+static Cal3_S2::shared_ptr sharedK(new Cal3_S2(fov, w, h));
+Camera level_camera(level_pose, sharedK);
+Camera level_camera_right(pose_right, sharedK);
+Camera cam1(level_pose, sharedK);
+Camera cam2(pose_right, sharedK);
+Camera cam3(pose_above, sharedK);
+}
+
+/* ************************************************************************* */
+// default Cal3_S2 poses
+namespace vanillaPose2 {
+typedef PinholePose<Cal3_S2> Camera;
+typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor;
+static Cal3_S2::shared_ptr sharedK2(new Cal3_S2(1500, 1200, 0, 640, 480));
+Camera level_camera(level_pose, sharedK2);
+Camera level_camera_right(pose_right, sharedK2);
+Camera cam1(level_pose, sharedK2);
+Camera cam2(pose_right, sharedK2);
+Camera cam3(pose_above, sharedK2);
+}
+
+/* *************************************************************************/
+// Cal3Bundler cameras
+namespace bundler {
+typedef PinholeCamera<Cal3Bundler> Camera;
+typedef SmartProjectionFactor<Camera> SmartFactor;
+static Cal3Bundler K(500, 1e-3, 1e-3, 0, 0);
+Camera level_camera(level_pose, K);
+Camera level_camera_right(pose_right, K);
+Point2 level_uv = level_camera.project(landmark1);
+Point2 level_uv_right = level_camera_right.project(landmark1);
+Pose3 pose1 = level_pose;
+Camera cam1(level_pose, K);
+Camera cam2(pose_right, K);
+Camera cam3(pose_above, K);
+typedef GeneralSFMFactor<Camera, Point3> SFMFactor;
+}
+/* *************************************************************************/
+// Cal3Bundler poses
+namespace bundlerPose {
+typedef PinholePose<Cal3Bundler> Camera;
+typedef SmartProjectionPoseFactor<Cal3Bundler> SmartFactor;
+static boost::shared_ptr<Cal3Bundler> sharedBundlerK(
+    new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000));
+Camera level_camera(level_pose, sharedBundlerK);
+Camera level_camera_right(pose_right, sharedBundlerK);
+Camera cam1(level_pose, sharedBundlerK);
+Camera cam2(pose_right, sharedBundlerK);
+Camera cam3(pose_above, sharedBundlerK);
+}
+/* *************************************************************************/
+
+template<class CAMERA>
+CAMERA perturbCameraPose(const CAMERA& camera) {
+  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10),
+      Point3(0.5, 0.1, 0.3));
+  Pose3 cameraPose = camera.pose();
+  Pose3 perturbedCameraPose = cameraPose.compose(noise_pose);
+  return CAMERA(perturbedCameraPose, camera.calibration());
+}
+
+template<class CAMERA>
+void projectToMultipleCameras(const CAMERA& cam1, const CAMERA& cam2,
+    const CAMERA& cam3, Point3 landmark, vector<Point2>& measurements_cam) {
+  Point2 cam1_uv1 = cam1.project(landmark);
+  Point2 cam2_uv1 = cam2.project(landmark);
+  Point2 cam3_uv1 = cam3.project(landmark);
+  measurements_cam.push_back(cam1_uv1);
+  measurements_cam.push_back(cam2_uv1);
+  measurements_cam.push_back(cam3_uv1);
+}
+
+/* ************************************************************************* */
+

gtsam/slam/tests/testEssentialMatrixFactor.cpp

@@ -160,7 +160,7 @@ TEST (EssentialMatrixFactor2, factor) {
 
     // Check evaluation
     Point3 P1 = data.tracks[i].p, P2 = data.cameras[1].pose().transform_to(P1);
-    const Point2 pi = SimpleCamera::project_to_camera(P2);
+    const Point2 pi = PinholeBase::Project(P2);
     Point2 reprojectionError(pi - pB(i));
     Vector expected = reprojectionError.vector();
 

gtsam/slam/tests/testRegularImplicitSchurFactor.cpp

@@ -19,11 +19,13 @@
 #include <gtsam/slam/JacobianFactorQ.h>
 #include <gtsam/slam/JacobianFactorQR.h>
 #include <gtsam/slam/RegularImplicitSchurFactor.h>
+#include <gtsam/geometry/CalibratedCamera.h>
+#include <gtsam/geometry/Point2.h>
 
-#include <gtsam/base/timing.h>
 #include <gtsam/linear/VectorValues.h>
 #include <gtsam/linear/NoiseModel.h>
 #include <gtsam/linear/GaussianFactor.h>
+#include <gtsam/base/timing.h>
 
 #include <boost/assign/list_of.hpp>
 #include <boost/assign/std/vector.hpp>
@@ -39,8 +41,8 @@ using namespace gtsam;
 const Matrix26 F0 = Matrix26::Ones();
 const Matrix26 F1 = 2 * Matrix26::Ones();
 const Matrix26 F3 = 3 * Matrix26::Ones();
-const vector<pair<Key, Matrix26> > Fblocks = list_of<pair<Key, Matrix> > //
+const vector<Matrix26> FBlocks = list_of<Matrix26>(F0)(F1)(F3);
-    (make_pair(0, F0))(make_pair(1, F1))(make_pair(3, F3));
+const FastVector<Key> keys = list_of<Key>(0)(1)(3);
 // RHS and sigmas
 const Vector b = (Vector(6) << 1., 2., 3., 4., 5., 6.).finished();
 
@@ -51,7 +53,7 @@ TEST( regularImplicitSchurFactor, creation ) {
   E.block<2,2>(0, 0) = eye(2);
   E.block<2,3>(2, 0) = 2 * ones(2, 3);
   Matrix3 P = (E.transpose() * E).inverse();
-  RegularImplicitSchurFactor<6> expected(Fblocks, E, P, b);
+  RegularImplicitSchurFactor<CalibratedCamera> expected(keys, FBlocks, E, P, b);
   Matrix expectedP = expected.getPointCovariance();
   EXPECT(assert_equal(expectedP, P));
 }
@@ -84,15 +86,15 @@ TEST( regularImplicitSchurFactor, addHessianMultiply ) {
   F << F0, zeros(2, d * 3), zeros(2, d), F1, zeros(2, d*2), zeros(2, d * 3), F3;
 
   // Calculate expected result F'*alpha*(I - E*P*E')*F*x
-  FastVector<Key> keys;
+  FastVector<Key> keys2;
-  keys += 0,1,2,3;
+  keys2 += 0,1,2,3;
-  Vector x = xvalues.vector(keys);
+  Vector x = xvalues.vector(keys2);
   Vector expected = zero(24);
-  RegularImplicitSchurFactor<6>::multiplyHessianAdd(F, E, P, alpha, x, expected);
+  RegularImplicitSchurFactor<CalibratedCamera>::multiplyHessianAdd(F, E, P, alpha, x, expected);
-  EXPECT(assert_equal(expected, yExpected.vector(keys), 1e-8));
+  EXPECT(assert_equal(expected, yExpected.vector(keys2), 1e-8));
 
   // Create ImplicitSchurFactor
-  RegularImplicitSchurFactor<6> implicitFactor(Fblocks, E, P, b);
+  RegularImplicitSchurFactor<CalibratedCamera> implicitFactor(keys, FBlocks, E, P, b);
 
   VectorValues zero = 0 * yExpected;// quick way to get zero w right structure
   { // First Version
@@ -122,32 +124,34 @@ TEST( regularImplicitSchurFactor, addHessianMultiply ) {
 
   // Create JacobianFactor with same error
   const SharedDiagonal model;
-  JacobianFactorQ<6, 2> jf(Fblocks, E, P, b, model);
+  JacobianFactorQ<6, 2> jfQ(keys, FBlocks, E, P, b, model);
 
-  { // error
+  // error
-    double expectedError = jf.error(xvalues);
+  double expectedError = 11875.083333333334;
-    double actualError = implicitFactor.errorJF(xvalues);
+  {
-    DOUBLES_EQUAL(expectedError,actualError,1e-7)
+    EXPECT_DOUBLES_EQUAL(expectedError,jfQ.error(xvalues),1e-7)
+    EXPECT_DOUBLES_EQUAL(expectedError,implicitFactor.errorJF(xvalues),1e-7)
+    EXPECT_DOUBLES_EQUAL(11903.500000000007,implicitFactor.error(xvalues),1e-7)
   }
 
-  { // JacobianFactor with same error
+  {
     VectorValues yActual = zero;
-    jf.multiplyHessianAdd(alpha, xvalues, yActual);
+    jfQ.multiplyHessianAdd(alpha, xvalues, yActual);
     EXPECT(assert_equal(yExpected, yActual, 1e-8));
-    jf.multiplyHessianAdd(alpha, xvalues, yActual);
+    jfQ.multiplyHessianAdd(alpha, xvalues, yActual);
     EXPECT(assert_equal(2 * yExpected, yActual, 1e-8));
-    jf.multiplyHessianAdd(-1, xvalues, yActual);
+    jfQ.multiplyHessianAdd(-1, xvalues, yActual);
     EXPECT(assert_equal(zero, yActual, 1e-8));
   }
 
   { // check hessian Diagonal
-    VectorValues diagExpected = jf.hessianDiagonal();
+    VectorValues diagExpected = jfQ.hessianDiagonal();
     VectorValues diagActual = implicitFactor.hessianDiagonal();
     EXPECT(assert_equal(diagExpected, diagActual, 1e-8));
   }
 
   { // check hessian Block Diagonal
-    map<Key,Matrix> BD = jf.hessianBlockDiagonal();
+    map<Key,Matrix> BD = jfQ.hessianBlockDiagonal();
     map<Key,Matrix> actualBD = implicitFactor.hessianBlockDiagonal();
     LONGS_EQUAL(3,actualBD.size());
     EXPECT(assert_equal(BD[0],actualBD[0]));
@@ -157,40 +161,46 @@ TEST( regularImplicitSchurFactor, addHessianMultiply ) {
 
   { // Raw memory Version
     std::fill(y, y + 24, 0);// zero y !
-    jf.multiplyHessianAdd(alpha, xdata, y);
+    jfQ.multiplyHessianAdd(alpha, xdata, y);
     EXPECT(assert_equal(expected, XMap(y), 1e-8));
-    jf.multiplyHessianAdd(alpha, xdata, y);
+    jfQ.multiplyHessianAdd(alpha, xdata, y);
     EXPECT(assert_equal(Vector(2 * expected), XMap(y), 1e-8));
-    jf.multiplyHessianAdd(-1, xdata, y);
+    jfQ.multiplyHessianAdd(-1, xdata, y);
     EXPECT(assert_equal(Vector(0 * expected), XMap(y), 1e-8));
   }
 
+  VectorValues expectedVV;
+  expectedVV.insert(0,-3.5*ones(6));
+  expectedVV.insert(1,10*ones(6)/3);
+  expectedVV.insert(3,-19.5*ones(6));
   { // Check gradientAtZero
-    VectorValues expected = jf.gradientAtZero();
     VectorValues actual = implicitFactor.gradientAtZero();
-    EXPECT(assert_equal(expected, actual, 1e-8));
+    EXPECT(assert_equal(expectedVV, jfQ.gradientAtZero(), 1e-8));
+    EXPECT(assert_equal(expectedVV, implicitFactor.gradientAtZero(), 1e-8));
   }
 
   // Create JacobianFactorQR
-  JacobianFactorQR<6, 2> jfq(Fblocks, E, P, b, model);
+  JacobianFactorQR<6, 2> jfQR(keys, FBlocks, E, P, b, model);
+    EXPECT_DOUBLES_EQUAL(expectedError, jfQR.error(xvalues),1e-7)
+  EXPECT(assert_equal(expectedVV,  jfQR.gradientAtZero(), 1e-8));
   {
     const SharedDiagonal model;
     VectorValues yActual = zero;
-    jfq.multiplyHessianAdd(alpha, xvalues, yActual);
+    jfQR.multiplyHessianAdd(alpha, xvalues, yActual);
     EXPECT(assert_equal(yExpected, yActual, 1e-8));
-    jfq.multiplyHessianAdd(alpha, xvalues, yActual);
+    jfQR.multiplyHessianAdd(alpha, xvalues, yActual);
     EXPECT(assert_equal(2 * yExpected, yActual, 1e-8));
-    jfq.multiplyHessianAdd(-1, xvalues, yActual);
+    jfQR.multiplyHessianAdd(-1, xvalues, yActual);
     EXPECT(assert_equal(zero, yActual, 1e-8));
   }
 
   { // Raw memory Version
     std::fill(y, y + 24, 0);// zero y !
-    jfq.multiplyHessianAdd(alpha, xdata, y);
+    jfQR.multiplyHessianAdd(alpha, xdata, y);
     EXPECT(assert_equal(expected, XMap(y), 1e-8));
-    jfq.multiplyHessianAdd(alpha, xdata, y);
+    jfQR.multiplyHessianAdd(alpha, xdata, y);
     EXPECT(assert_equal(Vector(2 * expected), XMap(y), 1e-8));
-    jfq.multiplyHessianAdd(-1, xdata, y);
+    jfQR.multiplyHessianAdd(-1, xdata, y);
     EXPECT(assert_equal(Vector(0 * expected), XMap(y), 1e-8));
   }
   delete [] y;
@@ -214,7 +224,7 @@ TEST(regularImplicitSchurFactor, hessianDiagonal)
   E.block<2,3>(2, 0) << 1,2,3,4,5,6;
   E.block<2,3>(4, 0) << 0.5,1,2,3,4,5;
   Matrix3 P = (E.transpose() * E).inverse();
-  RegularImplicitSchurFactor<6> factor(Fblocks, E, P, b);
+  RegularImplicitSchurFactor<CalibratedCamera> factor(keys, FBlocks, E, P, b);
 
   // hessianDiagonal
   VectorValues expected;
@@ -255,6 +265,18 @@ TEST(regularImplicitSchurFactor, hessianDiagonal)
   EXPECT(assert_equal(F0t*(I2-E0*P*E0.transpose())*F0,actualBD[0]));
   EXPECT(assert_equal(F1.transpose()*F1-FtE1*P*FtE1.transpose(),actualBD[1]));
   EXPECT(assert_equal(F3.transpose()*F3-FtE3*P*FtE3.transpose(),actualBD[3]));
+
+  // augmentedInformation (test just checks diagonals)
+  Matrix actualInfo = factor.augmentedInformation();
+  EXPECT(assert_equal(actualBD[0],actualInfo.block<6,6>(0,0)));
+  EXPECT(assert_equal(actualBD[1],actualInfo.block<6,6>(6,6)));
+  EXPECT(assert_equal(actualBD[3],actualInfo.block<6,6>(12,12)));
+
+  // information (test just checks diagonals)
+  Matrix actualInfo2 = factor.information();
+  EXPECT(assert_equal(actualBD[0],actualInfo2.block<6,6>(0,0)));
+  EXPECT(assert_equal(actualBD[1],actualInfo2.block<6,6>(6,6)));
+  EXPECT(assert_equal(actualBD[3],actualInfo2.block<6,6>(12,12)));
 }
 
 /* ************************************************************************* */

gtsam/slam/tests/testSmartFactorBase.cpp

@@ -26,7 +26,7 @@ using namespace gtsam;
 /* ************************************************************************* */
 #include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/geometry/Cal3Bundler.h>
-class PinholeFactor: public SmartFactorBase<PinholeCamera<Cal3Bundler>, 9> {
+class PinholeFactor: public SmartFactorBase<PinholeCamera<Cal3Bundler> > {
   virtual double error(const Values& values) const {
     return 0.0;
   }
@@ -45,7 +45,7 @@ TEST(SmartFactorBase, Pinhole) {
 
 /* ************************************************************************* */
 #include <gtsam/geometry/StereoCamera.h>
-class StereoFactor: public SmartFactorBase<StereoCamera, 9> {
+class StereoFactor: public SmartFactorBase<StereoCamera> {
   virtual double error(const Values& values) const {
     return 0.0;
   }

gtsam/slam/tests/testSmartProjectionCameraFactor.cpp

@@ -0,0 +1,856 @@
+/* ----------------------------------------------------------------------------
+
+ * 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  testSmartProjectionCameraFactor.cpp
+ *  @brief Unit tests for SmartProjectionCameraFactor Class
+ *  @author Chris Beall
+ *  @author Luca Carlone
+ *  @author Zsolt Kira
+ *  @author Frank Dellaert
+ *  @date   Sept 2013
+ */
+
+#include "smartFactorScenarios.h"
+#include <gtsam/slam/SmartProjectionFactor.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <CppUnitLite/TestHarness.h>
+#include <boost/assign/std/map.hpp>
+#include <iostream>
+
+using namespace boost::assign;
+
+static bool isDebugTest = false;
+
+// Convenience for named keys
+using symbol_shorthand::X;
+using symbol_shorthand::L;
+
+static Key x1(1);
+Symbol l1('l', 1), l2('l', 2), l3('l', 3);
+Key c1 = 1, c2 = 2, c3 = 3;
+
+static Point2 measurement1(323.0, 240.0);
+
+static double rankTol = 1.0;
+
+template<class CALIBRATION>
+PinholeCamera<CALIBRATION> perturbCameraPoseAndCalibration(
+    const PinholeCamera<CALIBRATION>& camera) {
+  GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION)
+  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10),
+      Point3(0.5, 0.1, 0.3));
+  Pose3 cameraPose = camera.pose();
+  Pose3 perturbedCameraPose = cameraPose.compose(noise_pose);
+  typename gtsam::traits<CALIBRATION>::TangentVector d;
+  d.setRandom();
+  d *= 0.1;
+  CALIBRATION perturbedCalibration = camera.calibration().retract(d);
+  return PinholeCamera<CALIBRATION>(perturbedCameraPose, perturbedCalibration);
+}
+
+/* ************************************************************************* */
+TEST( SmartProjectionCameraFactor, perturbCameraPose) {
+  using namespace vanilla;
+  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10),
+      Point3(0.5, 0.1, 0.3));
+  Pose3 perturbed_level_pose = level_pose.compose(noise_pose);
+  Camera actualCamera(perturbed_level_pose, K2);
+
+  Camera expectedCamera = perturbCameraPose(level_camera);
+  CHECK(assert_equal(expectedCamera, actualCamera));
+}
+
+/* ************************************************************************* */
+TEST( SmartProjectionCameraFactor, Constructor) {
+  using namespace vanilla;
+  SmartFactor::shared_ptr factor1(new SmartFactor());
+}
+
+/* ************************************************************************* */
+TEST( SmartProjectionCameraFactor, Constructor2) {
+  using namespace vanilla;
+  params.setRankTolerance(rankTol);
+  SmartFactor factor1(boost::none, params);
+}
+
+/* ************************************************************************* */
+TEST( SmartProjectionCameraFactor, Constructor3) {
+  using namespace vanilla;
+  SmartFactor::shared_ptr factor1(new SmartFactor());
+  factor1->add(measurement1, x1, unit2);
+}
+
+/* ************************************************************************* */
+TEST( SmartProjectionCameraFactor, Constructor4) {
+  using namespace vanilla;
+  params.setRankTolerance(rankTol);
+  SmartFactor factor1(boost::none, params);
+  factor1.add(measurement1, x1, unit2);
+}
+
+/* ************************************************************************* */
+TEST( SmartProjectionCameraFactor, Equals ) {
+  using namespace vanilla;
+  SmartFactor::shared_ptr factor1(new SmartFactor());
+  factor1->add(measurement1, x1, unit2);
+
+  SmartFactor::shared_ptr factor2(new SmartFactor());
+  factor2->add(measurement1, x1, unit2);
+}
+
+/* *************************************************************************/
+TEST( SmartProjectionCameraFactor, noiseless ) {
+  using namespace vanilla;
+
+  Values values;
+  values.insert(c1, level_camera);
+  values.insert(c2, level_camera_right);
+
+  SmartFactor::shared_ptr factor1(new SmartFactor());
+  factor1->add(level_uv, c1, unit2);
+  factor1->add(level_uv_right, c2, unit2);
+
+  double expectedError = 0.0;
+  DOUBLES_EQUAL(expectedError, factor1->error(values), 1e-7);
+  CHECK(
+      assert_equal(zero(4),
+          factor1->reprojectionErrorAfterTriangulation(values), 1e-7));
+}
+
+/* *************************************************************************/
+TEST( SmartProjectionCameraFactor, noisy ) {
+
+  using namespace vanilla;
+
+  // Project one landmark into two cameras and add noise on first
+  Point2 level_uv = level_camera.project(landmark1) + Point2(0.2, 0.2);
+  Point2 level_uv_right = level_camera_right.project(landmark1);
+
+  Values values;
+  values.insert(c1, level_camera);
+  Camera perturbed_level_camera_right = perturbCameraPose(level_camera_right);
+  values.insert(c2, perturbed_level_camera_right);
+
+  SmartFactor::shared_ptr factor1(new SmartFactor());
+  factor1->add(level_uv, c1, unit2);
+  factor1->add(level_uv_right, c2, unit2);
+
+  // Point is now uninitialized before a triangulation event
+  EXPECT(!factor1->point());
+
+  double expectedError = 58640;
+  double actualError1 = factor1->error(values);
+  EXPECT_DOUBLES_EQUAL(expectedError, actualError1, 1);
+
+  // Check triangulated point
+  CHECK(factor1->point());
+  EXPECT(
+      assert_equal(Point3(13.7587, 1.43851, -1.14274), *factor1->point(), 1e-4));
+
+  // Check whitened errors
+  Vector expected(4);
+  expected << -7, 235, 58, -242;
+  SmartFactor::Cameras cameras1 = factor1->cameras(values);
+  Point3 point1 = *factor1->point();
+  Vector actual = factor1->whitenedError(cameras1, point1);
+  EXPECT(assert_equal(expected, actual, 1));
+
+  SmartFactor::shared_ptr factor2(new SmartFactor());
+  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);
+
+  double actualError2 = factor2->error(values);
+  EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1);
+}
+
+/* *************************************************************************/
+TEST( SmartProjectionCameraFactor, perturbPoseAndOptimize ) {
+
+  using namespace vanilla;
+
+  // Project three landmarks into three cameras
+  vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;
+  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
+  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
+  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
+
+  // Create and fill smartfactors
+  SmartFactor::shared_ptr smartFactor1(new SmartFactor());
+  SmartFactor::shared_ptr smartFactor2(new SmartFactor());
+  SmartFactor::shared_ptr smartFactor3(new SmartFactor());
+  vector<Key> views;
+  views.push_back(c1);
+  views.push_back(c2);
+  views.push_back(c3);
+  smartFactor1->add(measurements_cam1, views, unit2);
+  smartFactor2->add(measurements_cam2, views, unit2);
+  smartFactor3->add(measurements_cam3, views, unit2);
+
+  // Create factor graph and add priors on two cameras
+  NonlinearFactorGraph graph;
+  graph.push_back(smartFactor1);
+  graph.push_back(smartFactor2);
+  graph.push_back(smartFactor3);
+  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6 + 5, 1e-5);
+  graph.push_back(PriorFactor<Camera>(c1, cam1, noisePrior));
+  graph.push_back(PriorFactor<Camera>(c2, cam2, noisePrior));
+
+  // Create initial estimate
+  Values initial;
+  initial.insert(c1, cam1);
+  initial.insert(c2, cam2);
+  initial.insert(c3, perturbCameraPose(cam3));
+  if (isDebugTest)
+    initial.at<Camera>(c3).print("Smart: Pose3 before optimization: ");
+
+  // Points are now uninitialized before a triangulation event
+  EXPECT(!smartFactor1->point());
+  EXPECT(!smartFactor2->point());
+  EXPECT(!smartFactor3->point());
+
+  EXPECT_DOUBLES_EQUAL(75711, smartFactor1->error(initial), 1);
+  EXPECT_DOUBLES_EQUAL(58524, smartFactor2->error(initial), 1);
+  EXPECT_DOUBLES_EQUAL(77564, smartFactor3->error(initial), 1);
+
+  // Error should trigger this and initialize the points, abort if not so
+  CHECK(smartFactor1->point());
+  CHECK(smartFactor2->point());
+  CHECK(smartFactor3->point());
+
+  EXPECT(
+      assert_equal(Point3(2.57696, -0.182566, 1.04085), *smartFactor1->point(),
+          1e-4));
+  EXPECT(
+      assert_equal(Point3(2.80114, -0.702153, 1.06594), *smartFactor2->point(),
+          1e-4));
+  EXPECT(
+      assert_equal(Point3(1.82593, -0.289569, 2.13438), *smartFactor3->point(),
+          1e-4));
+
+  // Check whitened errors
+  Vector expected(6);
+  expected << 256, 29, -26, 29, -206, -202;
+  Point3 point1 = *smartFactor1->point();
+  SmartFactor::Cameras cameras1 = smartFactor1->cameras(initial);
+  Vector reprojectionError = cameras1.reprojectionError(point1,
+      measurements_cam1);
+  EXPECT(assert_equal(expected, reprojectionError, 1));
+  Vector actual = smartFactor1->whitenedError(cameras1, point1);
+  EXPECT(assert_equal(expected, actual, 1));
+
+  // Optimize
+  LevenbergMarquardtParams lmParams;
+  if (isDebugTest) {
+    lmParams.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
+    lmParams.verbosity = NonlinearOptimizerParams::ERROR;
+  }
+  LevenbergMarquardtOptimizer optimizer(graph, initial, lmParams);
+  Values result = optimizer.optimize();
+
+  EXPECT(assert_equal(landmark1, *smartFactor1->point(), 1e-7));
+  EXPECT(assert_equal(landmark2, *smartFactor2->point(), 1e-7));
+  EXPECT(assert_equal(landmark3, *smartFactor3->point(), 1e-7));
+
+  //  GaussianFactorGraph::shared_ptr GFG = graph.linearize(initial);
+  //  VectorValues delta = GFG->optimize();
+
+  if (isDebugTest)
+    result.at<Camera>(c3).print("Smart: Pose3 after optimization: ");
+  EXPECT(assert_equal(cam1, result.at<Camera>(c1)));
+  EXPECT(assert_equal(cam2, result.at<Camera>(c2)));
+  EXPECT(assert_equal(result.at<Camera>(c3).pose(), cam3.pose(), 1e-4));
+  EXPECT(
+      assert_equal(result.at<Camera>(c3).calibration(), cam3.calibration(), 2));
+  if (isDebugTest)
+    tictoc_print_();
+}
+
+/* *************************************************************************/
+TEST( SmartProjectionCameraFactor, perturbPoseAndOptimizeFromSfM_tracks ) {
+
+  using namespace vanilla;
+
+  // Project three landmarks into three cameras
+  vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;
+  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
+  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
+  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
+
+  vector<Key> views;
+  views.push_back(c1);
+  views.push_back(c2);
+  views.push_back(c3);
+
+  SfM_Track track1;
+  for (size_t i = 0; i < 3; ++i) {
+    SfM_Measurement measures;
+    measures.first = i + 1; // cameras are from 1 to 3
+    measures.second = measurements_cam1.at(i);
+    track1.measurements.push_back(measures);
+  }
+  SmartFactor::shared_ptr smartFactor1(new SmartFactor());
+  smartFactor1->add(track1, unit2);
+
+  SfM_Track track2;
+  for (size_t i = 0; i < 3; ++i) {
+    SfM_Measurement measures;
+    measures.first = i + 1; // cameras are from 1 to 3
+    measures.second = measurements_cam2.at(i);
+    track2.measurements.push_back(measures);
+  }
+  SmartFactor::shared_ptr smartFactor2(new SmartFactor());
+  smartFactor2->add(track2, unit2);
+
+  SmartFactor::shared_ptr smartFactor3(new SmartFactor());
+  smartFactor3->add(measurements_cam3, views, unit2);
+
+  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6 + 5, 1e-5);
+
+  NonlinearFactorGraph graph;
+  graph.push_back(smartFactor1);
+  graph.push_back(smartFactor2);
+  graph.push_back(smartFactor3);
+  graph.push_back(PriorFactor<Camera>(c1, cam1, noisePrior));
+  graph.push_back(PriorFactor<Camera>(c2, cam2, noisePrior));
+
+  Values values;
+  values.insert(c1, cam1);
+  values.insert(c2, cam2);
+  values.insert(c3, perturbCameraPose(cam3));
+  if (isDebugTest)
+    values.at<Camera>(c3).print("Smart: Pose3 before optimization: ");
+
+  LevenbergMarquardtParams lmParams;
+  if (isDebugTest)
+    lmParams.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
+  if (isDebugTest)
+    lmParams.verbosity = NonlinearOptimizerParams::ERROR;
+
+  Values result;
+  LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
+  result = optimizer.optimize();
+
+  //  GaussianFactorGraph::shared_ptr GFG = graph.linearize(values);
+  //  VectorValues delta = GFG->optimize();
+
+  if (isDebugTest)
+    result.at<Camera>(c3).print("Smart: Pose3 after optimization: ");
+  EXPECT(assert_equal(cam1, result.at<Camera>(c1)));
+  EXPECT(assert_equal(cam2, result.at<Camera>(c2)));
+  EXPECT(assert_equal(result.at<Camera>(c3).pose(), cam3.pose(), 1e-4));
+  EXPECT(
+      assert_equal(result.at<Camera>(c3).calibration(), cam3.calibration(), 2));
+  if (isDebugTest)
+    tictoc_print_();
+}
+
+/* *************************************************************************/
+TEST( SmartProjectionCameraFactor, perturbCamerasAndOptimize ) {
+
+  using namespace vanilla;
+
+  vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3,
+      measurements_cam4, measurements_cam5;
+
+  // 1. Project three landmarks into three cameras and triangulate
+  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
+  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
+  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
+  projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4);
+  projectToMultipleCameras(cam1, cam2, cam3, landmark5, measurements_cam5);
+
+  vector<Key> views;
+  views.push_back(c1);
+  views.push_back(c2);
+  views.push_back(c3);
+
+  SmartFactor::shared_ptr smartFactor1(new SmartFactor());
+  smartFactor1->add(measurements_cam1, views, unit2);
+
+  SmartFactor::shared_ptr smartFactor2(new SmartFactor());
+  smartFactor2->add(measurements_cam2, views, unit2);
+
+  SmartFactor::shared_ptr smartFactor3(new SmartFactor());
+  smartFactor3->add(measurements_cam3, views, unit2);
+
+  SmartFactor::shared_ptr smartFactor4(new SmartFactor());
+  smartFactor4->add(measurements_cam4, views, unit2);
+
+  SmartFactor::shared_ptr smartFactor5(new SmartFactor());
+  smartFactor5->add(measurements_cam5, views, unit2);
+
+  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6 + 5, 1e-5);
+
+  NonlinearFactorGraph graph;
+  graph.push_back(smartFactor1);
+  graph.push_back(smartFactor2);
+  graph.push_back(smartFactor3);
+  graph.push_back(smartFactor4);
+  graph.push_back(smartFactor5);
+  graph.push_back(PriorFactor<Camera>(c1, cam1, noisePrior));
+  graph.push_back(PriorFactor<Camera>(c2, cam2, noisePrior));
+
+  Values values;
+  values.insert(c1, cam1);
+  values.insert(c2, cam2);
+  values.insert(c3, perturbCameraPoseAndCalibration(cam3));
+  if (isDebugTest)
+    values.at<Camera>(c3).print("Smart: Pose3 before optimization: ");
+
+  LevenbergMarquardtParams lmParams;
+  lmParams.relativeErrorTol = 1e-8;
+  lmParams.absoluteErrorTol = 0;
+  lmParams.maxIterations = 20;
+  if (isDebugTest)
+    lmParams.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
+  if (isDebugTest)
+    lmParams.verbosity = NonlinearOptimizerParams::ERROR;
+
+  Values result;
+  LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
+  result = optimizer.optimize();
+
+  //  GaussianFactorGraph::shared_ptr GFG = graph.linearize(values);
+  //  VectorValues delta = GFG->optimize();
+
+  if (isDebugTest)
+    result.at<Camera>(c3).print("Smart: Pose3 after optimization: ");
+  EXPECT(assert_equal(cam1, result.at<Camera>(c1)));
+  EXPECT(assert_equal(cam2, result.at<Camera>(c2)));
+  EXPECT(assert_equal(result.at<Camera>(c3).pose(), cam3.pose(), 1e-1));
+  EXPECT(
+      assert_equal(result.at<Camera>(c3).calibration(), cam3.calibration(), 20));
+  if (isDebugTest)
+    tictoc_print_();
+}
+
+/* *************************************************************************/
+TEST( SmartProjectionCameraFactor, Cal3Bundler ) {
+
+  using namespace bundler;
+
+  vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3,
+      measurements_cam4, measurements_cam5;
+
+  // 1. Project three landmarks into three cameras and triangulate
+  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
+  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
+  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
+  projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4);
+  projectToMultipleCameras(cam1, cam2, cam3, landmark5, measurements_cam5);
+
+  vector<Key> views;
+  views.push_back(c1);
+  views.push_back(c2);
+  views.push_back(c3);
+
+  SmartFactor::shared_ptr smartFactor1(new SmartFactor());
+  smartFactor1->add(measurements_cam1, views, unit2);
+
+  SmartFactor::shared_ptr smartFactor2(new SmartFactor());
+  smartFactor2->add(measurements_cam2, views, unit2);
+
+  SmartFactor::shared_ptr smartFactor3(new SmartFactor());
+  smartFactor3->add(measurements_cam3, views, unit2);
+
+  SmartFactor::shared_ptr smartFactor4(new SmartFactor());
+  smartFactor4->add(measurements_cam4, views, unit2);
+
+  SmartFactor::shared_ptr smartFactor5(new SmartFactor());
+  smartFactor5->add(measurements_cam5, views, unit2);
+
+  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(9, 1e-6);
+
+  NonlinearFactorGraph graph;
+  graph.push_back(smartFactor1);
+  graph.push_back(smartFactor2);
+  graph.push_back(smartFactor3);
+  graph.push_back(PriorFactor<Camera>(c1, cam1, noisePrior));
+  graph.push_back(PriorFactor<Camera>(c2, cam2, noisePrior));
+
+  Values values;
+  values.insert(c1, cam1);
+  values.insert(c2, cam2);
+  // initialize third pose with some noise, we expect it to move back to original pose3
+  values.insert(c3, perturbCameraPose(cam3));
+  if (isDebugTest)
+    values.at<Camera>(c3).print("Smart: Pose3 before optimization: ");
+
+  LevenbergMarquardtParams lmParams;
+  lmParams.relativeErrorTol = 1e-8;
+  lmParams.absoluteErrorTol = 0;
+  lmParams.maxIterations = 20;
+  if (isDebugTest)
+    lmParams.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
+  if (isDebugTest)
+    lmParams.verbosity = NonlinearOptimizerParams::ERROR;
+
+  Values result;
+  LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
+  result = optimizer.optimize();
+
+  if (isDebugTest)
+    result.at<Camera>(c3).print("Smart: Pose3 after optimization: ");
+  EXPECT(assert_equal(cam1, result.at<Camera>(c1), 1e-4));
+  EXPECT(assert_equal(cam2, result.at<Camera>(c2), 1e-4));
+  EXPECT(assert_equal(result.at<Camera>(c3).pose(), cam3.pose(), 1e-1));
+  EXPECT(
+      assert_equal(result.at<Camera>(c3).calibration(), cam3.calibration(), 1));
+  if (isDebugTest)
+    tictoc_print_();
+}
+
+/* *************************************************************************/
+TEST( SmartProjectionCameraFactor, Cal3Bundler2 ) {
+
+  using namespace bundler;
+
+  vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3,
+      measurements_cam4, measurements_cam5;
+
+  // 1. Project three landmarks into three cameras and triangulate
+  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
+  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
+  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
+  projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4);
+  projectToMultipleCameras(cam1, cam2, cam3, landmark5, measurements_cam5);
+
+  vector<Key> views;
+  views.push_back(c1);
+  views.push_back(c2);
+  views.push_back(c3);
+
+  SmartFactor::shared_ptr smartFactor1(new SmartFactor());
+  smartFactor1->add(measurements_cam1, views, unit2);
+
+  SmartFactor::shared_ptr smartFactor2(new SmartFactor());
+  smartFactor2->add(measurements_cam2, views, unit2);
+
+  SmartFactor::shared_ptr smartFactor3(new SmartFactor());
+  smartFactor3->add(measurements_cam3, views, unit2);
+
+  SmartFactor::shared_ptr smartFactor4(new SmartFactor());
+  smartFactor4->add(measurements_cam4, views, unit2);
+
+  SmartFactor::shared_ptr smartFactor5(new SmartFactor());
+  smartFactor5->add(measurements_cam5, views, unit2);
+
+  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(9, 1e-6);
+
+  NonlinearFactorGraph graph;
+  graph.push_back(smartFactor1);
+  graph.push_back(smartFactor2);
+  graph.push_back(smartFactor3);
+  graph.push_back(PriorFactor<Camera>(c1, cam1, noisePrior));
+  graph.push_back(PriorFactor<Camera>(c2, cam2, noisePrior));
+
+  Values values;
+  values.insert(c1, cam1);
+  values.insert(c2, cam2);
+  // initialize third pose with some noise, we expect it to move back to original pose3
+  values.insert(c3, perturbCameraPoseAndCalibration(cam3));
+  if (isDebugTest)
+    values.at<Camera>(c3).print("Smart: Pose3 before optimization: ");
+
+  LevenbergMarquardtParams lmParams;
+  lmParams.relativeErrorTol = 1e-8;
+  lmParams.absoluteErrorTol = 0;
+  lmParams.maxIterations = 20;
+  if (isDebugTest)
+    lmParams.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
+  if (isDebugTest)
+    lmParams.verbosity = NonlinearOptimizerParams::ERROR;
+
+  Values result;
+  LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
+  result = optimizer.optimize();
+
+  if (isDebugTest)
+    result.at<Camera>(c3).print("Smart: Pose3 after optimization: ");
+  EXPECT(assert_equal(cam1, result.at<Camera>(c1), 1e-4));
+  EXPECT(assert_equal(cam2, result.at<Camera>(c2), 1e-4));
+  EXPECT(assert_equal(result.at<Camera>(c3).pose(), cam3.pose(), 1e-1));
+  EXPECT(
+      assert_equal(result.at<Camera>(c3).calibration(), cam3.calibration(), 2));
+  if (isDebugTest)
+    tictoc_print_();
+}
+
+/* *************************************************************************/
+TEST( SmartProjectionCameraFactor, noiselessBundler ) {
+
+  using namespace bundler;
+  Values values;
+  values.insert(c1, level_camera);
+  values.insert(c2, level_camera_right);
+
+  SmartFactor::shared_ptr factor1(new SmartFactor());
+  factor1->add(level_uv, c1, unit2);
+  factor1->add(level_uv_right, c2, unit2);
+
+  double actualError = factor1->error(values);
+
+  double expectedError = 0.0;
+  DOUBLES_EQUAL(expectedError, actualError, 1e-3);
+
+  Point3 oldPoint; // this takes the point stored in the factor (we are not interested in this)
+  if (factor1->point())
+    oldPoint = *(factor1->point());
+
+  Point3 expectedPoint;
+  if (factor1->point(values))
+    expectedPoint = *(factor1->point(values));
+
+  EXPECT(assert_equal(expectedPoint, landmark1, 1e-3));
+}
+
+/* *************************************************************************/
+TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor ) {
+
+  using namespace bundler;
+  Values values;
+  values.insert(c1, level_camera);
+  values.insert(c2, level_camera_right);
+
+  NonlinearFactorGraph smartGraph;
+  SmartFactor::shared_ptr factor1(new SmartFactor());
+  factor1->add(level_uv, c1, unit2);
+  factor1->add(level_uv_right, c2, unit2);
+  smartGraph.push_back(factor1);
+  double expectedError = factor1->error(values);
+  double expectedErrorGraph = smartGraph.error(values);
+  Point3 expectedPoint;
+  if (factor1->point())
+    expectedPoint = *(factor1->point());
+  // cout << "expectedPoint " << expectedPoint.vector() << endl;
+
+  // COMMENTS:
+  // 1) triangulation introduces small errors, then for a fair comparison we use expectedPoint as
+  // value in the generalGrap
+  NonlinearFactorGraph generalGraph;
+  SFMFactor sfm1(level_uv, unit2, c1, l1);
+  SFMFactor sfm2(level_uv_right, unit2, c2, l1);
+  generalGraph.push_back(sfm1);
+  generalGraph.push_back(sfm2);
+  values.insert(l1, expectedPoint); // note: we get rid of possible errors in the triangulation
+  Vector e1 = sfm1.evaluateError(values.at<Camera>(c1), values.at<Point3>(l1));
+  Vector e2 = sfm2.evaluateError(values.at<Camera>(c2), values.at<Point3>(l1));
+  double actualError = 0.5
+      * (norm_2(e1) * norm_2(e1) + norm_2(e2) * norm_2(e2));
+  double actualErrorGraph = generalGraph.error(values);
+
+  DOUBLES_EQUAL(expectedErrorGraph, actualErrorGraph, 1e-7);
+  DOUBLES_EQUAL(expectedErrorGraph, expectedError, 1e-7);
+  DOUBLES_EQUAL(actualErrorGraph, actualError, 1e-7);
+  DOUBLES_EQUAL(expectedError, actualError, 1e-7);
+}
+
+/* *************************************************************************/
+TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor1 ) {
+
+  using namespace bundler;
+  Values values;
+  values.insert(c1, level_camera);
+  values.insert(c2, level_camera_right);
+
+  NonlinearFactorGraph smartGraph;
+  SmartFactor::shared_ptr factor1(new SmartFactor());
+  factor1->add(level_uv, c1, unit2);
+  factor1->add(level_uv_right, c2, unit2);
+  smartGraph.push_back(factor1);
+  Matrix expectedHessian = smartGraph.linearize(values)->hessian().first;
+  Vector expectedInfoVector = smartGraph.linearize(values)->hessian().second;
+  Point3 expectedPoint;
+  if (factor1->point())
+    expectedPoint = *(factor1->point());
+
+  // COMMENTS:
+  // 1) triangulation introduces small errors, then for a fair comparison we use expectedPoint as
+  // value in the generalGrap
+  NonlinearFactorGraph generalGraph;
+  SFMFactor sfm1(level_uv, unit2, c1, l1);
+  SFMFactor sfm2(level_uv_right, unit2, c2, l1);
+  generalGraph.push_back(sfm1);
+  generalGraph.push_back(sfm2);
+  values.insert(l1, expectedPoint); // note: we get rid of possible errors in the triangulation
+  Matrix actualFullHessian = generalGraph.linearize(values)->hessian().first;
+  Matrix actualFullInfoVector = generalGraph.linearize(values)->hessian().second;
+  Matrix actualHessian = actualFullHessian.block(0, 0, 18, 18)
+      - actualFullHessian.block(0, 18, 18, 3)
+          * (actualFullHessian.block(18, 18, 3, 3)).inverse()
+          * actualFullHessian.block(18, 0, 3, 18);
+  Vector actualInfoVector = actualFullInfoVector.block(0, 0, 18, 1)
+      - actualFullHessian.block(0, 18, 18, 3)
+          * (actualFullHessian.block(18, 18, 3, 3)).inverse()
+          * actualFullInfoVector.block(18, 0, 3, 1);
+
+  EXPECT(assert_equal(expectedHessian, actualHessian, 1e-7));
+  EXPECT(assert_equal(expectedInfoVector, actualInfoVector, 1e-7));
+}
+
+/* *************************************************************************/
+// Have to think about how to compare multiplyHessianAdd in generalSfMFactor and smartFactors
+//TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor2 ){
+//
+//  Values values;
+//  values.insert(c1, level_camera);
+//  values.insert(c2, level_camera_right);
+//
+//  NonlinearFactorGraph smartGraph;
+//  SmartFactor::shared_ptr factor1(new SmartFactor());
+//  factor1->add(level_uv, c1, unit2);
+//  factor1->add(level_uv_right, c2, unit2);
+//  smartGraph.push_back(factor1);
+//  GaussianFactorGraph::shared_ptr gfgSmart = smartGraph.linearize(values);
+//
+//  Point3 expectedPoint;
+//  if(factor1->point())
+//    expectedPoint = *(factor1->point());
+//
+//  // COMMENTS:
+//  // 1) triangulation introduces small errors, then for a fair comparison we use expectedPoint as
+//  // value in the generalGrap
+//  NonlinearFactorGraph generalGraph;
+//  SFMFactor sfm1(level_uv, unit2, c1, l1);
+//  SFMFactor sfm2(level_uv_right, unit2, c2, l1);
+//  generalGraph.push_back(sfm1);
+//  generalGraph.push_back(sfm2);
+//  values.insert(l1, expectedPoint); // note: we get rid of possible errors in the triangulation
+//  GaussianFactorGraph::shared_ptr gfg = generalGraph.linearize(values);
+//
+//  double alpha = 1.0;
+//
+//  VectorValues yExpected, yActual, ytmp;
+//  VectorValues xtmp = map_list_of
+//      (c1, (Vec(9) << 0,0,0,0,0,0,0,0,0))
+//      (c2, (Vec(9) << 0,0,0,0,0,0,0,0,0))
+//      (l1, (Vec(3) << 5.5, 0.5, 1.2));
+//  gfg ->multiplyHessianAdd(alpha, xtmp, ytmp);
+//
+//  VectorValues x = map_list_of
+//      (c1, (Vec(9) << 1,2,3,4,5,6,7,8,9))
+//      (c2, (Vec(9) << 11,12,13,14,15,16,17,18,19))
+//      (l1, (Vec(3) << 5.5, 0.5, 1.2));
+//
+//  gfgSmart->multiplyHessianAdd(alpha, ytmp + x, yActual);
+//  gfg ->multiplyHessianAdd(alpha, x, yExpected);
+//
+//  EXPECT(assert_equal(yActual,yExpected, 1e-7));
+//}
+/* *************************************************************************/
+TEST( SmartProjectionCameraFactor, computeImplicitJacobian ) {
+
+  using namespace bundler;
+  Values values;
+  values.insert(c1, level_camera);
+  values.insert(c2, level_camera_right);
+
+  SmartFactor::shared_ptr factor1(new SmartFactor());
+  factor1->add(level_uv, c1, unit2);
+  factor1->add(level_uv_right, c2, unit2);
+  Matrix expectedE;
+  Vector expectedb;
+
+  CameraSet<Camera> cameras;
+  cameras.push_back(level_camera);
+  cameras.push_back(level_camera_right);
+
+  factor1->error(values); // this is important to have a triangulation of the point
+  Point3 point;
+  if (factor1->point())
+    point = *(factor1->point());
+  vector<Matrix29> Fblocks;
+  factor1->computeJacobians(Fblocks, expectedE, expectedb, cameras, point);
+
+  NonlinearFactorGraph generalGraph;
+  SFMFactor sfm1(level_uv, unit2, c1, l1);
+  SFMFactor sfm2(level_uv_right, unit2, c2, l1);
+  generalGraph.push_back(sfm1);
+  generalGraph.push_back(sfm2);
+  values.insert(l1, point); // note: we get rid of possible errors in the triangulation
+  Matrix actualFullHessian = generalGraph.linearize(values)->hessian().first;
+  Matrix actualVinv = (actualFullHessian.block(18, 18, 3, 3)).inverse();
+
+  Matrix3 expectedVinv = factor1->PointCov(expectedE);
+  EXPECT(assert_equal(expectedVinv, actualVinv, 1e-7));
+}
+
+/* *************************************************************************/
+TEST( SmartProjectionCameraFactor, implicitJacobianFactor ) {
+
+  using namespace bundler;
+
+  Values values;
+  values.insert(c1, level_camera);
+  values.insert(c2, level_camera_right);
+  double rankTol = 1;
+  bool useEPI = false;
+
+  // Hessian version
+  SmartProjectionParams params;
+  params.setRankTolerance(rankTol);
+  params.setLinearizationMode(gtsam::HESSIAN);
+  params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY);
+  params.setEnableEPI(useEPI);
+
+  SmartFactor::shared_ptr explicitFactor(
+      new SmartFactor(boost::none, params));
+  explicitFactor->add(level_uv, c1, unit2);
+  explicitFactor->add(level_uv_right, c2, unit2);
+
+  GaussianFactor::shared_ptr gaussianHessianFactor = explicitFactor->linearize(
+      values);
+  HessianFactor& hessianFactor =
+      dynamic_cast<HessianFactor&>(*gaussianHessianFactor);
+
+  // Implicit Schur version
+  params.setLinearizationMode(gtsam::IMPLICIT_SCHUR);
+  SmartFactor::shared_ptr implicitFactor(
+      new SmartFactor(boost::none, params));
+  implicitFactor->add(level_uv, c1, unit2);
+  implicitFactor->add(level_uv_right, c2, unit2);
+  GaussianFactor::shared_ptr gaussianImplicitSchurFactor =
+      implicitFactor->linearize(values);
+  CHECK(gaussianImplicitSchurFactor);
+  typedef RegularImplicitSchurFactor<Camera> Implicit9;
+  Implicit9& implicitSchurFactor =
+      dynamic_cast<Implicit9&>(*gaussianImplicitSchurFactor);
+
+  VectorValues x = map_list_of(c1,
+      (Vector(9) << 1, 2, 3, 4, 5, 6, 7, 8, 9).finished())(c2,
+      (Vector(9) << 11, 12, 13, 14, 15, 16, 17, 18, 19).finished());
+
+  VectorValues yExpected, yActual;
+  double alpha = 1.0;
+  hessianFactor.multiplyHessianAdd(alpha, x, yActual);
+  implicitSchurFactor.multiplyHessianAdd(alpha, x, yExpected);
+  EXPECT(assert_equal(yActual, yExpected, 1e-7));
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */
+

gtsam/slam/tests/testTriangulationFactor.cpp

@@ -17,6 +17,7 @@
  */
 
 #include <gtsam/geometry/triangulation.h>
+#include <gtsam/geometry/SimpleCamera.h>
 #include <gtsam/geometry/Cal3Bundler.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <CppUnitLite/TestHarness.h>
@@ -35,7 +36,7 @@ static const boost::shared_ptr<Cal3_S2> sharedCal = //
 // Looking along X-axis, 1 meter above ground plane (x-y)
 static const Rot3 upright = Rot3::ypr(-M_PI / 2, 0., -M_PI / 2);
 static const Pose3 pose1 = Pose3(upright, gtsam::Point3(0, 0, 1));
-PinholeCamera<Cal3_S2> camera1(pose1, *sharedCal);
+SimpleCamera camera1(pose1, *sharedCal);
 
 // landmark ~5 meters infront of camera
 static const Point3 landmark(5, 0.5, 1.2);
@@ -48,7 +49,7 @@ TEST( triangulation, TriangulationFactor ) {
   // Create the factor with a measurement that is 3 pixels off in x
   Key pointKey(1);
   SharedNoiseModel model;
-  typedef TriangulationFactor<> Factor;
+  typedef TriangulationFactor<SimpleCamera> Factor;
   Factor factor(camera1, z1, model, pointKey);
 
   // Use the factor to calculate the Jacobians

gtsam_unstable/examples/SmartProjectionFactorExample.cpp

@@ -26,16 +26,14 @@
  *  -makes monocular observations of many landmarks
  */
 
-#include <gtsam/geometry/Pose3.h>
+#include <gtsam/slam/SmartProjectionPoseFactor.h>
+#include <gtsam/slam/dataset.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/slam/SmartProjectionPoseFactor.h>
 
 #include <string>
 #include <fstream>
@@ -46,6 +44,7 @@ using namespace gtsam;
 
 int main(int argc, char** argv){
 
+  typedef PinholePose<Cal3_S2> Camera;
   typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor;
 
   Values initial_estimate;
@@ -68,18 +67,17 @@ int main(int argc, char** argv){
   cout << "Reading camera poses" << endl;
   ifstream pose_file(pose_loc.c_str());
 
-  int pose_id;
+  int i;
   MatrixRowMajor m(4,4);
   //read camera pose parameters and use to make initial estimates of camera poses
-  while (pose_file >> pose_id) {
+  while (pose_file >> i) {
-    for (int i = 0; i < 16; i++) {
+    for (int i = 0; i < 16; i++)
       pose_file >> m.data()[i];
-    }
+    initial_estimate.insert(i, Pose3(m));
-    initial_estimate.insert(Symbol('x', pose_id), Pose3(m));
   }
   
-  // camera and landmark keys
+  // landmark keys
-  size_t x, l;
+  size_t 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
@@ -89,24 +87,24 @@ 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(K));
   size_t current_l = 3;   // hardcoded landmark ID from first measurement
 
-  while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) {
+  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());
+      factor = SmartFactor::shared_ptr(new SmartFactor(K));
       current_l = l;
     }
-    factor->add(Point2(uL,v), Symbol('x',x), model, K);
+    factor->add(Point2(uL,v), i, model);
   }
 
-  Pose3 first_pose = initial_estimate.at<Pose3>(Symbol('x',1));
+  Pose3 firstPose = initial_estimate.at<Pose3>(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));
+  graph.push_back(NonlinearEquality<Pose3>(1,firstPose));
 
   LevenbergMarquardtParams params;
   params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;

gtsam_unstable/slam/SmartStereoProjectionFactor.h

@@ -33,49 +33,25 @@
 
 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
  */
-template<class CALIBRATION, size_t D>
+template<class CALIBRATION>
-class SmartStereoProjectionFactor: public SmartFactorBase<StereoCamera, D> {
+class SmartStereoProjectionFactor: public SmartFactorBase<StereoCamera> {
 protected:
 
-  // Some triangulation parameters
+  /// @name Caching triangulation
-  const double rankTolerance_; ///< threshold to decide whether triangulation is degenerate_
+  /// @{
+  const TriangulationParameters parameters_;
+  // 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 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
 
@@ -84,29 +60,22 @@ protected:
   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<StereoCamera, D> Base;
+  typedef SmartFactorBase<StereoCamera> 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<CALIBRATION, D> This;
+  typedef SmartStereoProjectionFactor<CALIBRATION> This;
 
-  enum {ZDim = 3};    ///< Dimension trait of measurement type
+  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:
 
@@ -128,18 +97,15 @@ public:
    * @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,
+  SmartStereoProjectionFactor(const double rankTolerance,
-      const bool manageDegeneracy, const bool enableEPI,
+      const double linThreshold, const bool manageDegeneracy,
-      boost::optional<Pose3> body_P_sensor = boost::none,
+      const bool enableEPI, double landmarkDistanceThreshold = 1e10,
-      double landmarkDistanceThreshold = 1e10,
+      double dynamicOutlierRejectionThreshold = -1) :
-      double dynamicOutlierRejectionThreshold = -1,
+      parameters_(rankTolerance, enableEPI, landmarkDistanceThreshold,
-      SmartFactorStatePtr state = SmartFactorStatePtr(new SmartStereoProjectionFactorState())) :
+          dynamicOutlierRejectionThreshold), //
-      Base(body_P_sensor), rankTolerance_(rankTol), retriangulationThreshold_(
+      retriangulationThreshold_(1e-5), manageDegeneracy_(manageDegeneracy), linearizationThreshold_(
-          1e-5), manageDegeneracy_(manageDegeneracy), enableEPI_(enableEPI), linearizationThreshold_(
           linThreshold), degenerate_(false), cheiralityException_(false), throwCheirality_(
-          false), verboseCheirality_(false), state_(state),
+          false), verboseCheirality_(false) {
-          landmarkDistanceThreshold_(landmarkDistanceThreshold),
-          dynamicOutlierRejectionThreshold_(dynamicOutlierRejectionThreshold) {
   }
 
   /** Virtual destructor */
@@ -153,11 +119,12 @@ public:
    */
   void print(const std::string& s = "", const KeyFormatter& keyFormatter =
       DefaultKeyFormatter) const {
-    std::cout << s << "SmartStereoProjectionFactor, z = \n";
+    std::cout << s << "SmartStereoProjectionFactor\n";
-    std::cout << "rankTolerance_ = " << rankTolerance_ << std::endl;
+    std::cout << "triangulationParameters:\n" << parameters_ << std::endl;
     std::cout << "degenerate_ = " << degenerate_ << std::endl;
     std::cout << "cheiralityException_ = " << cheiralityException_ << std::endl;
-    std::cout << "linearizationThreshold_ = " << linearizationThreshold_ << std::endl;
+    std::cout << "linearizationThreshold_ = " << linearizationThreshold_
+        << std::endl;
     Base::print("", keyFormatter);
   }
 
@@ -197,40 +164,6 @@ public:
     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));
@@ -268,7 +201,7 @@ public:
           mono_cameras.push_back(PinholeCamera<Cal3_S2>(pose, K));
         }
         point_ = triangulatePoint3<PinholeCamera<Cal3_S2> >(mono_cameras, mono_measurements,
-            rankTolerance_, enableEPI_);
+            parameters_.rankTolerance, parameters_.enableEPI);
 
         // // // End temporary hack
 
@@ -281,11 +214,11 @@ public:
 
         // Check landmark distance and reprojection errors to avoid outliers
         double totalReprojError = 0.0;
-        size_t i=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_){
+          if (cameraTranslation.distance(point_) > parameters_.landmarkDistanceThreshold) {
             degenerate_ = true;
             break;
           }
@@ -300,8 +233,8 @@ public:
         }
         //std::cout << "totalReprojError error: " << totalReprojError << std::endl;
         // we discard smart factors that have large reprojection error
-        if(dynamicOutlierRejectionThreshold_ > 0 &&
+        if (parameters_.dynamicOutlierRejectionThreshold > 0
-            totalReprojError/m > dynamicOutlierRejectionThreshold_)
+            && totalReprojError / m > parameters_.dynamicOutlierRejectionThreshold)
           degenerate_ = true;
 
       } catch (TriangulationUnderconstrainedException&) {
@@ -345,15 +278,15 @@ public:
   }
 
   /// linearize returns a Hessianfactor that is an approximation of error(p)
-  boost::shared_ptr<RegularHessianFactor<D> > createHessianFactor(
+  boost::shared_ptr<RegularHessianFactor<Base::Dim> > 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<Key> js;
-    std::vector < Matrix > Gs(numKeys * (numKeys + 1) / 2);
+    std::vector<Matrix> Gs(numKeys * (numKeys + 1) / 2);
-    std::vector < Vector > gs(numKeys);
+    std::vector<Vector> gs(numKeys);
 
     if (this->measured_.size() != cameras.size()) {
       std::cout
@@ -362,138 +295,123 @@ public:
       exit(1);
     }
 
-    this->triangulateSafe(cameras);
+    triangulateSafe(cameras);
-    if (isDebug) std::cout << "point_ = " << point_ << std::endl;
+    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;
+      if (isDebug)
+        std::cout << "In linearize: exception" << std::endl;
       BOOST_FOREACH(Matrix& m, Gs)
-        m = zeros(D, D);
+        m = zeros(Base::Dim, Base::Dim);
       BOOST_FOREACH(Vector& v, gs)
-        v = zero(D);
+        v = zero(Base::Dim);
-      return boost::make_shared<RegularHessianFactor<D> >(this->keys_, Gs, gs,
+      return boost::make_shared<RegularHessianFactor<Base::Dim> >(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;
+      if (isDebug)
+        std::cout << "degenerate_ = true" << std::endl;
     }
 
-    bool doLinearize = this->decideIfLinearize(cameras);
+    if (this->linearizationThreshold_ >= 0) // if we apply selective relinearization and we need to relinearize
-
-    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);
-    }
-
     // ==================================================================
+    std::vector<typename Base::MatrixZD> Fblocks;
     Matrix F, E;
     Vector b;
-    double f = computeJacobians(F, E, b, cameras);
+    computeJacobians(Fblocks, E, b, cameras);
+    Base::FillDiagonalF(Fblocks, F); // expensive !!!
 
     // 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);
+    Matrix H(Base::Dim * numKeys, Base::Dim * numKeys);
     Vector gs_vector;
 
-    Matrix3 P = Base::PointCov(E,lambda);
+    Matrix3 P = Cameras::PointCov(E, lambda);
     H.noalias() = F.transpose() * (F - (E * (P * (E.transpose() * F))));
     gs_vector.noalias() = F.transpose() * (b - (E * (P * (E.transpose() * b))));
 
-    if (isDebug) std::cout << "gs_vector size " << gs_vector.size() << std::endl;
+    if (isDebug)
-    if (isDebug) std::cout << "H:\n" << H << std::endl;
+      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
+    for (DenseIndex i1 = 0; i1 < (DenseIndex) numKeys; i1++) { // for each camera
-      DenseIndex i1D = i1 * D;
+      DenseIndex i1D = i1 * Base::Dim;
-      gs.at(i1) = gs_vector.segment < D > (i1D);
+      gs.at(i1) = gs_vector.segment<Base::Dim>(i1D);
-      for (DenseIndex i2 = 0; i2 < (DenseIndex)numKeys; i2++) {
+      for (DenseIndex i2 = 0; i2 < (DenseIndex) numKeys; i2++) {
         if (i2 >= i1) {
-          Gs.at(GsCount2) = H.block < D, D > (i1D, i2 * D);
+          Gs.at(GsCount2) = H.block<Base::Dim, Base::Dim>(i1D, i2 * Base::Dim);
           GsCount2++;
         }
       }
     }
     // ==================================================================
-    if (this->linearizationThreshold_ >= 0) { // if we do not use selective relinearization we don't need to store these variables
+    double f = b.squaredNorm();
-      this->state_->Gs = Gs;
+    return boost::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_, Gs, gs, f);
-      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(
+//  boost::shared_ptr<ImplicitSchurFactor<Base::Dim> > createImplicitSchurFactor(
 //      const Cameras& cameras, double lambda) const {
 //    if (triangulateForLinearize(cameras))
 //      return Base::createImplicitSchurFactor(cameras, point_, lambda);
 //    else
-//      return boost::shared_ptr<ImplicitSchurFactor<D> >();
+//      return boost::shared_ptr<ImplicitSchurFactor<Base::Dim> >();
 //  }
 //
 //  /// create factor
-//  boost::shared_ptr<JacobianFactorQ<D> > createJacobianQFactor(
+//  boost::shared_ptr<JacobianFactorQ<Base::Dim> > 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<Base::Dim> >(this->keys_);
 //  }
 //
 //  /// Create a factor, takes values
-//  boost::shared_ptr<JacobianFactorQ<D> > createJacobianQFactor(
+//  boost::shared_ptr<JacobianFactorQ<Base::Dim> > createJacobianQFactor(
 //      const Values& values, double lambda) const {
-//    Cameras myCameras;
+//    Cameras cameras;
 //    // TODO triangulate twice ??
-//    bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras);
+//    bool nonDegenerate = computeCamerasAndTriangulate(values, cameras);
 //    if (nonDegenerate)
-//      return createJacobianQFactor(myCameras, lambda);
+//      return createJacobianQFactor(cameras, lambda);
 //    else
-//      return boost::make_shared< JacobianFactorQ<D> >(this->keys_);
+//      return boost::make_shared< JacobianFactorQ<Base::Dim> >(this->keys_);
 //  }
 //
   /// different (faster) way to compute Jacobian factor
-  boost::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras& cameras,
+  boost::shared_ptr<JacobianFactor> createJacobianSVDFactor(
-      double lambda) const {
+      const Cameras& cameras, double lambda) const {
     if (triangulateForLinearize(cameras))
       return Base::createJacobianSVDFactor(cameras, point_, lambda);
     else
-      return boost::make_shared< JacobianFactorSVD<D, ZDim> >(this->keys_);
+      return boost::make_shared<JacobianFactorSVD<Base::Dim, ZDim> >(this->keys_);
   }
 
   /// Returns true if nonDegenerate
   bool computeCamerasAndTriangulate(const Values& values,
-      Cameras& myCameras) const {
+      Cameras& cameras) const {
     Values valuesFactor;
 
     // Select only the cameras
     BOOST_FOREACH(const Key key, this->keys_)
       valuesFactor.insert(key, values.at(key));
 
-    myCameras = this->cameras(valuesFactor);
+    cameras = this->cameras(valuesFactor);
-    size_t nrCameras = this->triangulateSafe(myCameras);
+    size_t nrCameras = this->triangulateSafe(cameras);
 
     if (nrCameras < 2
         || (!this->manageDegeneracy_
@@ -505,7 +423,8 @@ public:
       this->degenerate_ = true;
 
     if (this->degenerate_) {
-      std::cout << "SmartStereoProjectionFactor: this is not ready" << std::endl;
+      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;
@@ -513,34 +432,32 @@ public:
     return true;
   }
 
-  /// Assumes non-degenerate !
+  /**
-  void computeEP(Matrix& E, Matrix& PointCov, const Cameras& cameras) const {
+   * Triangulate and compute derivative of error with respect to point
-    Base::computeEP(E, PointCov, cameras, point_);
+   * @return whether triangulation worked
-  }
+   */
-
+  bool triangulateAndComputeE(Matrix& E, const Values& values) const {
-  /// Takes values
+    Cameras cameras;
-  bool computeEP(Matrix& E, Matrix& PointCov, const Values& values) const {
+    bool nonDegenerate = computeCamerasAndTriangulate(values, cameras);
-    Cameras myCameras;
-    bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras);
     if (nonDegenerate)
-      computeEP(E, PointCov, myCameras);
+      cameras.project2(point_, boost::none, E);
     return nonDegenerate;
   }
 
   /// Version that takes values, and creates the point
-  bool computeJacobians(std::vector<typename Base::KeyMatrix2D>& Fblocks,
+  bool computeJacobians(std::vector<typename Base::MatrixZD>& Fblocks,
       Matrix& E, Vector& b, const Values& values) const {
-    Cameras myCameras;
+    Cameras cameras;
-    bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras);
+    bool nonDegenerate = computeCamerasAndTriangulate(values, cameras);
     if (nonDegenerate)
-      computeJacobians(Fblocks, E, b, myCameras, 0.0);
+      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
-  double computeJacobians(std::vector<typename Base::KeyMatrix2D>& Fblocks,
+  void computeJacobians(std::vector<typename Base::MatrixZD>& Fblocks,
       Matrix& E, Vector& b, const Cameras& cameras) const {
     if (this->degenerate_) {
       throw("FIXME: computeJacobians degenerate case commented out!");
@@ -571,73 +488,36 @@ public:
 //
 //        this->noise_.at(i)->WhitenSystem(Fi, Ei, bi);
 //        f += bi.squaredNorm();
-//        Fblocks.push_back(typename Base::KeyMatrix2D(this->keys_[i], Fi));
+//        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
-      return Base::computeJacobians(Fblocks, E, b, cameras, point_);
+      Base::computeJacobians(Fblocks, E, b, cameras, point_);
     } // end else
   }
 
-//  /// Version that computes PointCov, with optional lambda parameter
+  /// takes values
-//  double computeJacobians(std::vector<typename Base::KeyMatrix2D>& Fblocks,
+  bool triangulateAndComputeJacobiansSVD(
-//      Matrix& E, Matrix& PointCov, Vector& b, const Cameras& cameras,
+      std::vector<typename Base::MatrixZD>& Fblocks, Matrix& Enull, Vector& b,
-//      const double lambda = 0.0) const {
+      const Values& values) const {
-//
+    typename Base::Cameras cameras;
-//    double f = computeJacobians(Fblocks, E, b, cameras);
+    double good = computeCamerasAndTriangulate(values, cameras);
-//
+    if (good)
-//    // Point covariance inv(E'*E)
+      return Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, point_);
-//    PointCov.noalias() = (E.transpose() * E + lambda * eye(E.cols())).inverse();
+    return true;
-//
-//    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, Vector& b,
-      const Cameras& cameras) const {
-    return Base::computeJacobians(F, E, b, cameras, point_);
   }
 
   /// Calculate vector of re-projection errors, before applying noise model
-  /// Assumes triangulation was done and degeneracy handled
+  Vector reprojectionErrorAfterTriangulation(const Values& values) const {
-  Vector reprojectionError(const Cameras& cameras) const {
+    Cameras cameras;
-    return Base::reprojectionError(cameras, point_);
+    bool nonDegenerate = computeCamerasAndTriangulate(values, cameras);
-  }
-
-  /// 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);
+      return Base::unwhitenedError(cameras, point_);
     else
-      return zero(myCameras.size() * 3);
+      return zero(cameras.size() * 3);
   }
 
   /**
@@ -675,7 +555,7 @@ public:
     }
 
     if (this->degenerate_) {
-       return 0.0; // TODO: this maybe should be zero?
+      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;
@@ -744,9 +624,9 @@ private:
 };
 
 /// traits
-template<class CALIBRATION, size_t D>
+template<class CALIBRATION>
-struct traits<SmartStereoProjectionFactor<CALIBRATION, D> > :
+struct traits<SmartStereoProjectionFactor<CALIBRATION> > : public Testable<
-    public Testable<SmartStereoProjectionFactor<CALIBRATION, D> > {
+    SmartStereoProjectionFactor<CALIBRATION> > {
 };
 
 } // \ namespace gtsam

gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h

@@ -15,6 +15,7 @@
  * @author Luca Carlone
  * @author Chris Beall
  * @author Zsolt Kira
+ * @author Frank Dellaert
  */
 
 #pragma once
@@ -38,7 +39,14 @@ namespace gtsam {
  * @addtogroup SLAM
  */
 template<class CALIBRATION>
-class SmartStereoProjectionPoseFactor: public SmartStereoProjectionFactor<CALIBRATION, 6> {
+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)
@@ -50,7 +58,7 @@ public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
   /// shorthand for base class type
-  typedef SmartStereoProjectionFactor<CALIBRATION, 6> Base;
+  typedef SmartStereoProjectionFactor<CALIBRATION> Base;
 
   /// shorthand for this class
   typedef SmartStereoProjectionPoseFactor<CALIBRATION> This;
@@ -65,15 +73,16 @@ public:
    * @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<Pose3> body_P_sensor = boost::none,
+      const bool enableEPI = false, LinearizationMode linearizeTo = HESSIAN,
-      LinearizationMode linearizeTo = HESSIAN, double landmarkDistanceThreshold = 1e10,
+      double landmarkDistanceThreshold = 1e10,
       double dynamicOutlierRejectionThreshold = -1) :
-        Base(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor,
+      Base(rankTol, linThreshold, manageDegeneracy, enableEPI,
-        landmarkDistanceThreshold, dynamicOutlierRejectionThreshold), linearizeTo_(linearizeTo) {}
+          landmarkDistanceThreshold, dynamicOutlierRejectionThreshold), linearizeTo_(
+          linearizeTo) {
+  }
 
   /** Virtual destructor */
   virtual ~SmartStereoProjectionPoseFactor() {}

gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp

@@ -18,8 +18,8 @@
  *  @date   Sept 2013
  */
 
+// TODO #include <gtsam/slam/tests/smartFactorScenarios.h>
 #include <gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h>
-
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <gtsam/slam/PoseTranslationPrior.h>
 #include <gtsam/slam/ProjectionFactor.h>
@@ -32,8 +32,6 @@ using namespace std;
 using namespace boost::assign;
 using namespace gtsam;
 
-static bool isDebugTest = false;
-
 // make a realistic calibration matrix
 static double fov = 60; // degrees
 static size_t w = 640, h = 480;
@@ -66,7 +64,6 @@ 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;
-typedef SmartStereoProjectionPoseFactor<Cal3Bundler> SmartFactorBundler;
 
 vector<StereoPoint2> stereo_projectToMultipleCameras(const StereoCamera& cam1,
     const StereoCamera& cam2, const StereoCamera& cam3, Point3 landmark) {
@@ -83,9 +80,10 @@ vector<StereoPoint2> stereo_projectToMultipleCameras(const StereoCamera& cam1,
   return measurements_cam;
 }
 
+LevenbergMarquardtParams params;
+
 /* ************************************************************************* */
 TEST( SmartStereoProjectionPoseFactor, Constructor) {
-  fprintf(stderr, "Test 1 Complete");
   SmartFactor::shared_ptr factor1(new SmartFactor());
 }
 
@@ -106,15 +104,6 @@ TEST( SmartStereoProjectionPoseFactor, Constructor4) {
   factor1.add(measurement1, poseKey1, model, K);
 }
 
-/* ************************************************************************* */
-TEST( SmartStereoProjectionPoseFactor, ConstructorWithTransform) {
-  bool manageDegeneracy = true;
-  bool enableEPI = false;
-  SmartFactor factor1(rankTol, linThreshold, manageDegeneracy, enableEPI,
-      body_P_sensor1);
-  factor1.add(measurement1, poseKey1, model, K);
-}
-
 /* ************************************************************************* */
 TEST( SmartStereoProjectionPoseFactor, Equals ) {
   SmartFactor::shared_ptr factor1(new SmartFactor());
@@ -128,8 +117,6 @@ TEST( SmartStereoProjectionPoseFactor, Equals ) {
 
 /* *************************************************************************/
 TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ) {
-  // cout << " ************************ SmartStereoProjectionPoseFactor: noisy ****************************" << endl;
-
   // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
   Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2),
       Point3(0, 0, 1));
@@ -169,8 +156,6 @@ TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ) {
 
 /* *************************************************************************/
 TEST( SmartStereoProjectionPoseFactor, noisy ) {
-  // cout << " ************************ SmartStereoProjectionPoseFactor: noisy ****************************" << endl;
-
   // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
   Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2),
       Point3(0, 0, 1));
@@ -226,10 +211,6 @@ TEST( SmartStereoProjectionPoseFactor, noisy ) {
 
 /* *************************************************************************/
 TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) {
-  cout
-      << " ************************ SmartStereoProjectionPoseFactor: 3 cams + 3 landmarks **********************"
-      << endl;
-
   // 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);
@@ -286,31 +267,31 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) {
   values.insert(x2, pose2);
   // initialize third pose with some noise, we expect it to move back to original pose3
   values.insert(x3, pose3 * noise_pose);
-  if (isDebugTest)
+  EXPECT(
-    values.at<Pose3>(x3).print("Smart: Pose3 before optimization: ");
+      assert_equal(
+          Pose3(
+              Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598,
+                  -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598),
+              Point3(0.1, -0.1, 1.9)), values.at<Pose3>(x3)));
 
-  LevenbergMarquardtParams params;
+  EXPECT_DOUBLES_EQUAL(1888864, graph.error(values), 1);
-  if (isDebugTest)
-    params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
-  if (isDebugTest)
-    params.verbosity = NonlinearOptimizerParams::ERROR;
 
   Values result;
-  gttic_ (SmartStereoProjectionPoseFactor);
+  gttic_(SmartStereoProjectionPoseFactor);
   LevenbergMarquardtOptimizer optimizer(graph, values, params);
   result = optimizer.optimize();
   gttoc_(SmartStereoProjectionPoseFactor);
   tictoc_finishedIteration_();
 
-//  GaussianFactorGraph::shared_ptr GFG = graph.linearize(values);
+  EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-6);
-//  VectorValues delta = GFG->optimize();
+
+  GaussianFactorGraph::shared_ptr GFG = graph.linearize(result);
+  VectorValues delta = GFG->optimize();
+  VectorValues expected = VectorValues::Zero(delta);
+  EXPECT(assert_equal(expected, delta, 1e-6));
 
   // result.print("results of 3 camera, 3 landmark optimization \n");
-  if (isDebugTest)
-    result.at<Pose3>(x3).print("Smart: Pose3 after optimization: ");
   EXPECT(assert_equal(pose3, result.at<Pose3>(x3)));
-  if (isDebugTest)
-    tictoc_print_();
 }
 
 /* *************************************************************************/
@@ -345,15 +326,15 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) {
       cam2, cam3, landmark3);
 
   SmartFactor::shared_ptr smartFactor1(
-      new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD));
+      new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD));
   smartFactor1->add(measurements_cam1, views, model, K);
 
   SmartFactor::shared_ptr smartFactor2(
-      new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD));
+      new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD));
   smartFactor2->add(measurements_cam2, views, model, K);
 
   SmartFactor::shared_ptr smartFactor3(
-      new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD));
+      new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD));
   smartFactor3->add(measurements_cam3, views, model, K);
 
   const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@@ -373,7 +354,6 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) {
   values.insert(x2, pose2);
   values.insert(x3, pose3 * noise_pose);
 
-  LevenbergMarquardtParams params;
   Values result;
   LevenbergMarquardtOptimizer optimizer(graph, values, params);
   result = optimizer.optimize();
@@ -414,17 +394,17 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) {
       cam2, cam3, landmark3);
 
   SmartFactor::shared_ptr smartFactor1(
-      new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD,
+      new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD,
           excludeLandmarksFutherThanDist));
   smartFactor1->add(measurements_cam1, views, model, K);
 
   SmartFactor::shared_ptr smartFactor2(
-      new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD,
+      new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD,
           excludeLandmarksFutherThanDist));
   smartFactor2->add(measurements_cam2, views, model, K);
 
   SmartFactor::shared_ptr smartFactor3(
-      new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD,
+      new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD,
           excludeLandmarksFutherThanDist));
   smartFactor3->add(measurements_cam3, views, model, K);
 
@@ -446,7 +426,6 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) {
   values.insert(x3, pose3 * noise_pose);
 
   // All factors are disabled and pose should remain where it is
-  LevenbergMarquardtParams params;
   Values result;
   LevenbergMarquardtOptimizer optimizer(graph, values, params);
   result = optimizer.optimize();
@@ -493,22 +472,22 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
   measurements_cam4.at(0) = measurements_cam4.at(0) + StereoPoint2(10, 10, 1); // add outlier
 
   SmartFactor::shared_ptr smartFactor1(
-      new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD,
+      new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD,
           excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
   smartFactor1->add(measurements_cam1, views, model, K);
 
   SmartFactor::shared_ptr smartFactor2(
-      new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD,
+      new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD,
           excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
   smartFactor2->add(measurements_cam2, views, model, K);
 
   SmartFactor::shared_ptr smartFactor3(
-      new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD,
+      new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD,
           excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
   smartFactor3->add(measurements_cam3, views, model, K);
 
   SmartFactor::shared_ptr smartFactor4(
-      new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD,
+      new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD,
           excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
   smartFactor4->add(measurements_cam4, views, model, K);
 
@@ -530,7 +509,6 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
   values.insert(x3, pose3);
 
   // All factors are disabled and pose should remain where it is
-  LevenbergMarquardtParams params;
   Values result;
   LevenbergMarquardtOptimizer optimizer(graph, values, params);
   result = optimizer.optimize();
@@ -567,13 +545,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, boost::none, JACOBIAN_Q));
+//  SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, JACOBIAN_Q));
 //  smartFactor1->add(measurements_cam1, views, model, K);
 //
-//  SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q));
+//  SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, JACOBIAN_Q));
 //  smartFactor2->add(measurements_cam2, views, model, K);
 //
-//  SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q));
+//  SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, JACOBIAN_Q));
 //  smartFactor3->add(measurements_cam3, views, model, K);
 //
 //  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@@ -592,8 +570,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
 //  values.insert(x2, pose2);
 //  values.insert(x3, pose3*noise_pose);
 //
-//  LevenbergMarquardtParams params;
+////  Values result;
-//  Values result;
 //  LevenbergMarquardtOptimizer optimizer(graph, values, params);
 //  result = optimizer.optimize();
 //  EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
@@ -601,7 +578,6 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
 //
 ///* *************************************************************************/
 //TEST( SmartStereoProjectionPoseFactor, 3poses_projection_factor ){
-//  //  cout << " ************************ Normal ProjectionFactor: 3 cams + 3 landmarks **********************" << endl;
 //
 //  vector<Key> views;
 //  views.push_back(x1);
@@ -653,15 +629,10 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
 //  values.insert(L(1), landmark1);
 //  values.insert(L(2), landmark2);
 //  values.insert(L(3), landmark3);
-//  if(isDebugTest)  values.at<Pose3>(x3).print("Pose3 before optimization: ");
 //
-//  LevenbergMarquardtParams params;
-//  if(isDebugTest)  params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
-//  if(isDebugTest)  params.verbosity = NonlinearOptimizerParams::ERROR;
 //  LevenbergMarquardtOptimizer optimizer(graph, values, params);
 //  Values result = optimizer.optimize();
 //
-//  if(isDebugTest)  result.at<Pose3>(x3).print("Pose3 after optimization: ");
 //  EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
 //}
 //
@@ -723,8 +694,6 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
   Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100),
       Point3(0.1, 0.1, 0.1)); // smaller noise
   values.insert(x3, pose3 * noise_pose);
-  if (isDebugTest)
-    values.at<Pose3>(x3).print("Smart: Pose3 before optimization: ");
 
   // TODO: next line throws Cheirality exception on Mac
   boost::shared_ptr<GaussianFactor> hessianFactor1 = smartFactor1->linearize(
@@ -749,7 +718,6 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
       + hessianFactor3->augmentedInformation();
 
   // Check Information vector
-  // cout << AugInformationMatrix.size() << endl;
   Vector InfoVector = AugInformationMatrix.block(0, 18, 18, 1); // 18x18 Hessian + information vector
 
   // Check Hessian
@@ -758,7 +726,6 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
 //
 ///* *************************************************************************/
 //TEST( SmartStereoProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_factor ){
-//  // cout << " ************************ SmartStereoProjectionPoseFactor: 3 cams + 2 landmarks: Rotation Only**********************" << endl;
 //
 //  vector<Key> views;
 //  views.push_back(x1);
@@ -811,11 +778,6 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
 //  values.insert(x2, pose2*noise_pose);
 //  // initialize third pose with some noise, we expect it to move back to original pose3
 //  values.insert(x3, pose3*noise_pose*noise_pose);
-//  if(isDebugTest) values.at<Pose3>(x3).print("Smart: Pose3 before optimization: ");
-//
-//  LevenbergMarquardtParams params;
-//  if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYDELTA;
-//  if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR;
 //
 //  Values result;
 //  gttic_(SmartStereoProjectionPoseFactor);
@@ -825,15 +787,11 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
 //  tictoc_finishedIteration_();
 //
 //  // result.print("results of 3 camera, 3 landmark optimization \n");
-//  if(isDebugTest) result.at<Pose3>(x3).print("Smart: Pose3 after optimization: ");
-//  cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << endl;
 //  // EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
-//  if(isDebugTest) tictoc_print_();
 //}
 //
 ///* *************************************************************************/
 //TEST( SmartStereoProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ){
-//  // cout << " ************************ SmartStereoProjectionPoseFactor: 3 cams + 3 landmarks: Rotation Only**********************" << endl;
 //
 //  vector<Key> views;
 //  views.push_back(x1);
@@ -894,11 +852,6 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
 //  values.insert(x2, pose2);
 //  // initialize third pose with some noise, we expect it to move back to original pose3
 //  values.insert(x3, pose3*noise_pose);
-//  if(isDebugTest) values.at<Pose3>(x3).print("Smart: Pose3 before optimization: ");
-//
-//  LevenbergMarquardtParams params;
-//  if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYDELTA;
-//  if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR;
 //
 //  Values result;
 //  gttic_(SmartStereoProjectionPoseFactor);
@@ -908,15 +861,11 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
 //  tictoc_finishedIteration_();
 //
 //  // result.print("results of 3 camera, 3 landmark optimization \n");
-//  if(isDebugTest) result.at<Pose3>(x3).print("Smart: Pose3 after optimization: ");
-//  cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << endl;
 //  // EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
-//  if(isDebugTest) tictoc_print_();
 //}
 //
 ///* *************************************************************************/
 //TEST( SmartStereoProjectionPoseFactor, Hessian ){
-//  // cout << " ************************ SmartStereoProjectionPoseFactor: Hessian **********************" << endl;
 //
 //  vector<Key> views;
 //  views.push_back(x1);
@@ -949,7 +898,6 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
 //  values.insert(x2, pose2);
 //
 //  boost::shared_ptr<GaussianFactor> hessianFactor = smartFactor1->linearize(values);
-//  if(isDebugTest) hessianFactor->print("Hessian factor \n");
 //
 //  // compute triangulation from linearization point
 //  // compute reprojection errors (sum squared)
@@ -960,8 +908,6 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
 
 /* *************************************************************************/
 TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) {
-  // cout << " ************************ SmartStereoProjectionPoseFactor: rotated Hessian **********************" << endl;
-
   vector<Key> views;
   views.push_back(x1);
   views.push_back(x2);
@@ -1031,8 +977,6 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) {
 
 /* *************************************************************************/
 TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) {
-  // cout << " ************************ SmartStereoProjectionPoseFactor: rotated Hessian (degenerate) **********************" << endl;
-
   vector<Key> views;
   views.push_back(x1);
   views.push_back(x2);
@@ -1063,8 +1007,6 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) {
 
   boost::shared_ptr<GaussianFactor> hessianFactor = smartFactor->linearize(
       values);
-  if (isDebugTest)
-    hessianFactor->print("Hessian factor \n");
 
   Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0));
 
@@ -1075,8 +1017,6 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) {
 
   boost::shared_ptr<GaussianFactor> hessianFactorRot = smartFactor->linearize(
       rotValues);
-  if (isDebugTest)
-    hessianFactorRot->print("Hessian factor \n");
 
   // Hessian is invariant to rotations in the nondegenerate case
   EXPECT(

tests/testExpressionFactor.cpp

@@ -37,6 +37,10 @@ using namespace gtsam;
 Point2 measured(-17, 30);
 SharedNoiseModel model = noiseModel::Unit::Create(2);
 
+// This deals with the overload problem and makes the expressions factor
+// understand that we work on Point3
+Point2 (*Project)(const Point3&, OptionalJacobian<2, 3>) = &PinholeBase::Project;
+
 namespace leaf {
 // Create some values
 struct MyValues: public Values {
@@ -313,7 +317,7 @@ TEST(ExpressionFactor, tree) {
 
   // Create expression tree
   Point3_ p_cam(x, &Pose3::transform_to, p);
-  Point2_ xy_hat(PinholeCamera<Cal3_S2>::project_to_camera, p_cam);
+  Point2_ xy_hat(Project, p_cam);
   Point2_ uv_hat(K, &Cal3_S2::uncalibrate, xy_hat);
 
   // Create factor and check value, dimension, linearization
@@ -331,8 +335,6 @@ TEST(ExpressionFactor, tree) {
   boost::shared_ptr<GaussianFactor> gf2 = f2.linearize(values);
   EXPECT( assert_equal(*expected, *gf2, 1e-9));
 
-  Expression<Point2>::TernaryFunction<Pose3, Point3, Cal3_S2>::type fff = project6;
-
   // Try ternary version
   ExpressionFactor<Point2> f3(model, measured, project3(x, p, K));
   EXPECT_DOUBLES_EQUAL(expected_error, f3.error(values), 1e-9);
@@ -489,7 +491,7 @@ TEST(ExpressionFactor, tree_finite_differences) {
 
   // Create expression tree
   Point3_ p_cam(x, &Pose3::transform_to, p);
-  Point2_ xy_hat(PinholeCamera<Cal3_S2>::project_to_camera, p_cam);
+  Point2_ xy_hat(Project, p_cam);
   Point2_ uv_hat(K, &Cal3_S2::uncalibrate, xy_hat);
 
   const double fd_step = 1e-5;

timing/DummyFactor.h

@@ -0,0 +1,56 @@
+/**
+ * @file    DummyFactor.h
+ * @brief   Just to help in timing overhead
+ * @author  Frank Dellaert
+ */
+
+#pragma once
+
+#include <gtsam/slam/RegularImplicitSchurFactor.h>
+
+namespace gtsam {
+
+/**
+ * DummyFactor
+ */
+template<size_t D> //
+class DummyFactor: public RegularImplicitSchurFactor<D> {
+
+public:
+
+  typedef Eigen::Matrix<double, 2, D> Matrix2D;
+  typedef std::pair<Key, Matrix2D> KeyMatrix2D;
+
+  DummyFactor() {
+  }
+
+  DummyFactor(const std::vector<KeyMatrix2D>& Fblocks, const Matrix& E,
+      const Matrix3& P, const Vector& b) :RegularImplicitSchurFactor<D>(Fblocks,E,P,b)
+       {
+  }
+
+  virtual ~DummyFactor() {
+  }
+
+public:
+
+  /**
+   * @brief Dummy version to measure overhead of key access
+   */
+  void multiplyHessian(double alpha, const VectorValues& x,
+      VectorValues& y) const {
+
+    BOOST_FOREACH(const KeyMatrix2D& Fi, this->Fblocks_) {
+      static const Vector empty;
+      Key key = Fi.first;
+      std::pair<VectorValues::iterator, bool> it = y.tryInsert(key, empty);
+      Vector& yi = it.first->second;
+      yi = x.at(key);
+    }
+  }
+
+};
+// DummyFactor
+
+}
+

timing/timeSchurFactors.cpp

@@ -0,0 +1,152 @@
+/**
+ * @file    timeSchurFactors.cpp
+ * @brief   Time various Schur-complement Jacobian factors
+ * @author  Frank Dellaert
+ * @date    Oct 27, 2013
+ */
+
+#include "DummyFactor.h"
+#include <gtsam/base/timing.h>
+
+#include <gtsam/slam/JacobianFactorQ.h>
+#include "gtsam/slam/JacobianFactorQR.h"
+#include <gtsam/slam/RegularImplicitSchurFactor.h>
+
+#include <boost/assign/list_of.hpp>
+#include <boost/assign/std/vector.hpp>
+#include <fstream>
+
+using namespace std;
+using namespace boost::assign;
+using namespace gtsam;
+
+#define SLOW
+#define RAW
+#define HESSIAN
+#define NUM_ITERATIONS 1000
+
+// Create CSV file for results
+ofstream os("timeSchurFactors.csv");
+
+/*************************************************************************************/
+template<size_t D>
+void timeAll(size_t m, size_t N) {
+
+  cout << m << endl;
+
+  // create F
+  typedef Eigen::Matrix<double, 2, D> Matrix2D;
+  typedef pair<Key, Matrix2D> KeyMatrix2D;
+  vector < pair<Key, Matrix2D> > Fblocks;
+  for (size_t i = 0; i < m; i++)
+    Fblocks.push_back(KeyMatrix2D(i, (i + 1) * Matrix::Ones(2, D)));
+
+  // create E
+  Matrix E(2 * m, 3);
+  for (size_t i = 0; i < m; i++)
+    E.block < 2, 3 > (2 * i, 0) = Matrix::Ones(2, 3);
+
+  // Calculate point covariance
+  Matrix P = (E.transpose() * E).inverse();
+
+  // RHS and sigmas
+  const Vector b = gtsam::repeat(2 * m, 1);
+  const SharedDiagonal model;
+
+  // parameters for multiplyHessianAdd
+  double alpha = 0.5;
+  VectorValues xvalues, yvalues;
+  for (size_t i = 0; i < m; i++)
+    xvalues.insert(i, gtsam::repeat(D, 2));
+
+  // Implicit
+  RegularImplicitSchurFactor<D> implicitFactor(Fblocks, E, P, b);
+  // JacobianFactor with same error
+  JacobianFactorQ<D, 2> jf(Fblocks, E, P, b, model);
+  // JacobianFactorQR with same error
+  JacobianFactorQR<D, 2> jqr(Fblocks, E, P, b, model);
+  // Hessian
+  HessianFactor hessianFactor(jqr);
+
+#define TIME(label,factor,xx,yy) {\
+     for (size_t t = 0; t < N; t++) \
+     factor.multiplyHessianAdd(alpha, xx, yy);\
+     gttic_(label);\
+     for (size_t t = 0; t < N; t++) {\
+       factor.multiplyHessianAdd(alpha, xx, yy);\
+     }\
+     gttoc_(label);\
+     tictoc_getNode(timer, label)\
+     os << timer->secs()/NUM_ITERATIONS << ", ";\
+   }
+
+#ifdef SLOW
+  TIME(Implicit, implicitFactor, xvalues, yvalues)
+  TIME(Jacobian, jf, xvalues, yvalues)
+  TIME(JacobianQR, jqr, xvalues, yvalues)
+#endif
+
+#ifdef HESSIAN
+  TIME(Hessian, hessianFactor, xvalues, yvalues)
+#endif
+
+#ifdef OVERHEAD
+  DummyFactor<D> dummy(Fblocks, E, P, b);
+  TIME(Overhead,dummy,xvalues,yvalues)
+#endif
+
+#ifdef RAW
+  { // Raw memory Version
+    FastVector < Key > keys;
+    for (size_t i = 0; i < m; i++)
+      keys += i;
+    Vector x = xvalues.vector(keys);
+    double* xdata = x.data();
+
+    // create a y
+    Vector y = zero(m * D);
+    TIME(RawImplicit, implicitFactor, xdata, y.data())
+    TIME(RawJacobianQ, jf, xdata, y.data())
+    TIME(RawJacobianQR, jqr, xdata, y.data())
+  }
+#endif
+
+  os << m << endl;
+
+} // timeAll
+
+/*************************************************************************************/
+int main(void) {
+#ifdef SLOW
+  os << "Implicit,";
+  os << "JacobianQ,";
+  os << "JacobianQR,";
+#endif
+#ifdef HESSIAN
+  os << "Hessian,";
+#endif
+#ifdef OVERHEAD
+  os << "Overhead,";
+#endif
+#ifdef RAW
+  os << "RawImplicit,";
+  os << "RawJacobianQ,";
+  os << "RawJacobianQR,";
+#endif
+  os << "m" << endl;
+  // define images
+  vector < size_t > ms;
+  //  ms += 2;
+  //  ms += 3, 4, 5, 6, 7, 8, 9, 10;
+  // ms += 11,12,13,14,15,16,17,18,19;
+  //  ms += 20, 30, 40, 50;
+  // ms += 20,30,40,50,60,70,80,90,100;
+  for (size_t m = 2; m <= 50; m += 2)
+    ms += m;
+  //for (size_t m=10;m<=100;m+=10) ms += m;
+  // loop over number of images
+  BOOST_FOREACH(size_t m,ms)
+    timeAll<6>(m, NUM_ITERATIONS);
+}
+
+//*************************************************************************************