From 6c72c29a563d960219005383c0724fdc7095b714 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Thu, 5 Mar 2015 22:47:28 -0800
Subject: [PATCH] Templated instead of two identical functions

---
 gtsam/geometry/CameraSet.h | 55 +++++++-------------------------------
 1 file changed, 10 insertions(+), 45 deletions(-)

diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h
index 123d45e1e..60af8beef 100644
--- a/gtsam/geometry/CameraSet.h
+++ b/gtsam/geometry/CameraSet.h
@@ -93,85 +93,50 @@ public:
   }
 
   /**
-   * Project a point, with derivatives in CameraSet and Point3
+   * Project a point (posibly 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> project2(const Point3& point, //
+  template<class POINT>
+  std::vector<Z> project2(const POINT& point, //
       boost::optional<FBlocks&> Fs = boost::none, //
       boost::optional<Matrix&> E = boost::none) const {
 
+    static const int N = FixedDimension<POINT>::value;
+
     // Allocate result
     size_t m = this->size();
     std::vector<Z> z(m);
 
     // Allocate derivatives
     if (E)
-      E->resize(ZDim * m, 3);
+      E->resize(ZDim * m, N);
     if (Fs)
       Fs->resize(m);
 
     // Project and fill derivatives
     for (size_t i = 0; i < m; i++) {
       MatrixZD Fi;
-      Eigen::Matrix<double, ZDim, 3> Ei;
+      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, 3>(ZDim * i, 0) = Ei;
-    }
-
-    return z;
-  }
-
-  /**
-   * Project a point at infinity, with derivatives in this, point, and calibration
-   * throws CheiralityException
-   */
-  std::vector<Z> project2(const Unit3& point, //
-      boost::optional<FBlocks&> Fs = boost::none, //
-      boost::optional<Matrix&> E = boost::none) const {
-
-    // Allocate result
-    size_t m = this->size();
-    std::vector<Z> z(m);
-
-    // Allocate derivatives
-    if (E)
-      E->resize(ZDim * m, 2);
-    if (Fs)
-      Fs->resize(m);
-
-    // Project and fill derivatives
-    for (size_t i = 0; i < m; i++) {
-      MatrixZD Fi;
-      Eigen::Matrix<double, ZDim, 2> Ei;
-      z[i] = this->at(i).project2(point, Fs ? &Fi : 0, E ? &Ei : 0);
-      if (Fs)
-        (*Fs)[i] = Fi;
-      if (E)
-        E->block<ZDim, 2>(ZDim * i, 0) = Ei;
+        E->block<ZDim, N>(ZDim * i, 0) = Ei;
     }
 
     return z;
   }
 
   /// Calculate vector [project2(point)-z] of re-projection errors
-  Vector reprojectionError(const Point3& point, const std::vector<Z>& measured,
+  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);
   }
 
-  /// Calculate vector [project2(point)-z] of re-projection errors, from point at infinity
-  Vector reprojectionError(const Unit3& 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