Moved project2 to PinholeBase

gtsam/geometry/CalibratedCamera.cpp

@@ -96,6 +96,24 @@ Point2 PinholeBase::project_to_camera(const Point3& P,
   return Point2(u, v);
 }
 
+/* ************************************************************************* */
+Point2 PinholeBase::project2(const Point3& point, OptionalJacobian<2, 6> Dpose,
+    OptionalJacobian<2, 3> Dpoint) const {
+
+  Matrix3 Rt; // calculated by transform_to if needed
+  const Point3 q = pose().transform_to(point, boost::none, Dpoint ? &Rt : 0);
+  Point2 pn = project_to_camera(q);
+
+  if (Dpose || Dpoint) {
+    const double z = q.z(), d = 1.0 / z;
+    if (Dpose)
+      *Dpose = PinholeBase::Dpose(pn, d);
+    if (Dpoint)
+      *Dpoint = PinholeBase::Dpoint(pn, d, Rt.transpose()); // TODO transpose
+  }
+  return pn;
+}
+
 /* ************************************************************************* */
 Point3 PinholeBase::backproject_from_camera(const Point2& p,
     const double depth) {
@@ -116,19 +134,7 @@ CalibratedCamera CalibratedCamera::Lookat(const Point3& eye,
 /* ************************************************************************* */
 Point2 CalibratedCamera::project(const Point3& point,
     OptionalJacobian<2, 6> Dcamera, OptionalJacobian<2, 3> Dpoint) const {
-
+  return project2(point, Dcamera, Dpoint);
-  Matrix3 Rt; // calculated by transform_to if needed
-  const Point3 q = pose().transform_to(point, boost::none, Dpoint ? &Rt : 0);
-  Point2 pn = project_to_camera(q);
-
-  if (Dcamera || Dpoint) {
-    const double z = q.z(), d = 1.0 / z;
-    if (Dcamera)
-      *Dcamera = PinholeBase::Dpose(pn, d);
-    if (Dpoint)
-      *Dpoint = PinholeBase::Dpoint(pn, d, Rt.transpose()); // TODO transpose
-  }
-  return pn;
 }
 
 /* ************************************************************************* */

gtsam/geometry/CalibratedCamera.h

@@ -147,10 +147,20 @@ public:
       OptionalJacobian<2, 3> Dpoint = boost::none);
 
   /**
-   * backproject a 2-dimensional point to a 3-dimension point
+   * backproject a 2-dimensional point to a 3-dimensional point at given depth
    */
   static Point3 backproject_from_camera(const Point2& p, const double depth);
 
+  /**
+   * Project point into the image
+   * @param point 3D point in world coordinates
+   * @param Dpose the optionally computed Jacobian with respect to camera
+   * @param Dpoint the optionally computed Jacobian with respect to the 3D point
+   * @return the intrinsic coordinates of the projected point
+   */
+  Point2 project2(const Point3& point, OptionalJacobian<2, 6> Dpose =
+      boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const;
+
 private:
 
   /** Serialization function */
@@ -253,16 +263,13 @@ public:
   /// @}
   /// @name Transformations and mesaurement functions
   /// @{
+
   /**
-   * This function receives the camera pose and the landmark location and
+   * @deprecated
-   * returns the location the point is supposed to appear in the image
+   * Use project2, which is more consistently named across Pinhole cameras
-   * @param point a 3D point to be projected
-   * @param Dpose the optionally computed Jacobian with respect to pose
-   * @param Dpoint the optionally computed Jacobian with respect to the 3D point
-   * @return the intrinsic coordinates of the projected point
    */
   Point2 project(const Point3& point,
-      OptionalJacobian<2, 6> Dpose = boost::none,
+      OptionalJacobian<2, 6> Dcamera = boost::none,
       OptionalJacobian<2, 3> Dpoint = boost::none) const;
 
   /**