using overloading rather than templates to manage projection of Point3 and Unit3 (the templates worked on mac, but had issues compiling in ubuntu)

gtsam/geometry/PinholeCamera.h

@@ -145,7 +145,7 @@ public:
   const Pose3& getPose(OptionalJacobian<6, dimension> H) const {
     if (H) {
       H->setZero();
-      H->template block<6,6>(0, 0) = I_6x6;
+      H->template block<6, 6>(0, 0) = I_6x6;
     }
     return Base::pose();
   }
@@ -199,14 +199,12 @@ public:
 
   typedef Eigen::Matrix<double, 2, DimK> Matrix2K;
 
-  /** project a point from world coordinate to the image (possibly a Unit3)
+  /** Templated projection of a 3D point or a point at infinity into the image
-   *  @param pw is a point in the world coordinate
+   *  @param pw either a Point3 or a Unit3, in world coordinates
    */
   template<class POINT>
-  Point2 project2(
+  Point2 _project2(const POINT& pw, OptionalJacobian<2, dimension> Dcamera,
-      const POINT& pw, //
+      OptionalJacobian<2, FixedDimension<POINT>::value> Dpoint) const {
-      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;
@@ -217,6 +215,18 @@ public:
     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
@@ -264,7 +274,7 @@ public:
     if (Dother) {
       Dother->resize(1, 6 + CalibrationB::dimension);
       Dother->setZero();
-      Dother->block<1,6>(0, 0) = Dother_;
+      Dother->block<1, 6>(0, 0) = Dother_;
     }
     return result;
   }

gtsam/geometry/PinholePose.h

@@ -87,12 +87,8 @@ public:
    *  @param pw is a point in the world coordinates
    */
   Point2 project(const Point3& pw) const {
-
+    const Point2 pn = PinholeBase::project2(pw); // project to normalized coordinates
-    // project to normalized coordinates
+    return calibration().uncalibrate(pn); // uncalibrate to pixel coordinates
-    const Point2 pn = PinholeBase::project2(pw);
-
-    // uncalibrate to pixel coordinates
-    return calibration().uncalibrate(pn);
   }
 
   /** project a point from world coordinate to the image
@@ -100,20 +96,20 @@ public:
    */
   Point2 project(const Unit3& pw) const {
     const Unit3 pc = pose().rotation().unrotate(pw); // convert to camera frame
-    const Point2 pn = PinholeBase::Project(pc);
+    const Point2 pn = PinholeBase::Project(pc); // project to normalized coordinates
-    return calibration().uncalibrate(pn);
+    return calibration().uncalibrate(pn);  // uncalibrate to pixel coordinates
   }
 
-  /** project a point (possibly at infinity) from world coordinate to the image
+  /** Templated projection of a point (possibly at infinity) from world coordinate to the image
-   *  @param pw is a point in world coordinates
+   *  @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,
+  Point2 _project(const POINT& pw, OptionalJacobian<2, 6> Dpose,
-      OptionalJacobian<2, FixedDimension<POINT>::value> Dpoint = boost::none,
+      OptionalJacobian<2, FixedDimension<POINT>::value> Dpoint,
-      OptionalJacobian<2, DimK> Dcal = boost::none) const {
+      OptionalJacobian<2, DimK> Dcal) const {
 
     // project to normalized coordinates
     const Point2 pn = PinholeBase::project2(pw, Dpose, Dpoint);
@@ -132,6 +128,20 @@ public:
     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);
@@ -318,15 +328,19 @@ public:
     return *K_;
   }
 
-  /** project a point (possibly at infinity) from world coordinate to the image, 2 derivatives only
+  /** 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 (realy only the pose)
+   *  @param Dpose is the Jacobian w.r.t. the whole camera (really only the pose)
    *  @param Dpoint is the Jacobian w.r.t. point3
-   *  TODO should use Unit3
    */
-  template<class POINT>
+  Point2 project2(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
-  Point2 project2(const POINT& pw, OptionalJacobian<2, 6> Dpose = boost::none,
+      OptionalJacobian<2, 3> Dpoint = boost::none) const {
-      OptionalJacobian<2, FixedDimension<POINT>::value> 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);
   }