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

---
 gtsam/geometry/PinholeCamera.h | 24 +++---------
 gtsam/geometry/PinholePose.h   | 68 ++++++----------------------------
 2 files changed, 17 insertions(+), 75 deletions(-)

diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h
index e900d5a85..4775e732f 100644
--- a/gtsam/geometry/PinholeCamera.h
+++ b/gtsam/geometry/PinholeCamera.h
@@ -200,26 +200,14 @@ public:
 
   typedef Eigen::Matrix<double, 2, DimK> Matrix2K;
 
-  /** project a point from world coordinate to the image, fixed Jacobians
+  /** project a point from world coordinate to the image (possibly a Unit3)
    *  @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 {
-    // We just call 3-derivative version in Base
-    Matrix26 Dpose;
-    Eigen::Matrix<double, 2, DimK> Dcal;
-    Point2 pi = Base::project(pw, Dcamera ? &Dpose : 0, Dpoint,
-        Dcamera ? &Dcal : 0);
-    if (Dcamera)
-      *Dcamera << Dpose, Dcal;
-    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 Unit3& pw, OptionalJacobian<2, dimension> Dcamera =
-      boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const {
+  template<class POINT>
+  Point2 project2(
+      const POINT& pw, //
+      OptionalJacobian<2, dimension> Dcamera = boost::none,
+      OptionalJacobian<2, FixedDimension<POINT>::value> Dpoint = boost::none) const {
     // We just call 3-derivative version in Base
     Matrix26 Dpose;
     Eigen::Matrix<double, 2, DimK> Dcal;
diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h
index c23cbd4f5..a2449871a 100644
--- a/gtsam/geometry/PinholePose.h
+++ b/gtsam/geometry/PinholePose.h
@@ -104,14 +104,15 @@ public:
     return calibration().uncalibrate(pn);
   }
 
-  /** project a point from world coordinate to the image
+  /** project a point (possibly at infinity) from world coordinate to the image
    *  @param pw is a point 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,
-      OptionalJacobian<2, 3> Dpoint = boost::none,
+  template <class POINT>
+  Point2 project(const POINT& pw, OptionalJacobian<2, 6> Dpose,
+      OptionalJacobian<2, FixedDimension<POINT>::value> Dpoint = boost::none,
       OptionalJacobian<2, DimK> Dcal = boost::none) const {
 
     // project to normalized coordinates
@@ -124,36 +125,9 @@ public:
 
     // If needed, apply chain rule
     if (Dpose)
-      *Dpose = Dpi_pn * *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 project(const Unit3& pw, OptionalJacobian<2, 6> Dpose,
-      OptionalJacobian<2, 2> Dpoint = boost::none,
-      OptionalJacobian<2, DimK> Dcal = boost::none) 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, Dcal,
-        Dpose || Dpoint ? &Dpi_pn : 0);
-
-    // If needed, apply chain rule
-    if (Dpose)
-      *Dpose = Dpi_pn * *Dpose;
-    if (Dpoint)
-      *Dpoint = Dpi_pn * *Dpoint;
+    *Dpoint = Dpi_pn * *Dpoint;
 
     return pi;
   }
@@ -344,35 +318,15 @@ public:
     return *K_;
   }
 
-  /** project a point from world coordinate to the image, w 2 derivatives
-   *  @param pw is a point in the world coordinates
-   */
-  Point2 project2(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
-      OptionalJacobian<2, 3> Dpoint = boost::none) 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,
-        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, 2 derivatives only
-   *  @param pw is a point in the world coordinate (it is pw = lambda*[pw_x  pw_y  pw_z] with lambda->inf)
+  /** project a point (possibly at infinity) 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 (realy only the pose)
    *  @param Dpoint is the Jacobian w.r.t. point3
    *  TODO should use Unit3
    */
-  Point2 project2(const Unit3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
-      OptionalJacobian<2, 2> Dpoint = boost::none) const {
+  template<class POINT>
+  Point2 project2(const POINT& pw, OptionalJacobian<2, 6> Dpose = boost::none,
+      OptionalJacobian<2, FixedDimension<POINT>::value> Dpoint = boost::none) const {
     return Base::project(pw, Dpose, Dpoint);
   }