PinholeBaseK::project and PinholePose::project2 of Unit3

gtsam/geometry/PinholePose.h

@@ -95,45 +95,66 @@ public:
     return calibration().uncalibrate(pn);
   }
 
+  /** 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_to_camera(pc);
+    return calibration().uncalibrate(pn);
+  }
+
+  /** project a point 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,
+      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;
+
+    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 =
+  Point2 project(const Unit3& pw, OptionalJacobian<2, 6> Dpose,
-      boost::none, OptionalJacobian<2, 2> Dpoint = boost::none,
+      OptionalJacobian<2, 2> Dpoint = boost::none,
       OptionalJacobian<2, DimK> Dcal = boost::none) const {
 
-    if (!Dpose && !Dpoint && !Dcal) {
+    // project to normalized coordinates
-      const Point3 pc = this->pose().rotation().unrotate(pw); // get direction in camera frame (translation does not matter)
+    const Point2 pn = PinholeBase::project2(pw, Dpose, Dpoint);
-      const Point2 pn = PinholeBase::project_to_camera(pc);// project the point to the camera
-      return calibration().uncalibrate(pn);
-    }
 
-    // world to camera coordinate
+    // uncalibrate to pixel coordinates
-    Matrix3 Dpc_rot, Dpc_point;
+    Matrix2 Dpi_pn;
-    const Point3 pc = this->pose().rotation().unrotate(pw, Dpc_rot, Dpc_point);
+    const Point2 pi = calibration().uncalibrate(pn, Dcal,
+        Dpose || Dpoint ? &Dpi_pn : 0);
 
-    // only rotation is important
+    // If needed, apply chain rule
-    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 = PinholeBase::project_to_camera(pc, Dpn_pc);
-
-    // uncalibration
-    Matrix2 Dpi_pn;// 2*2
-    const Point2 pi = calibration().uncalibrate(pn, Dcal, Dpi_pn);
-
-    // chain the Jacobian matrices
-    const Matrix23 Dpi_pc = Dpi_pn * Dpn_pc;
     if (Dpose)
-    *Dpose = Dpi_pc * Dpc_pose;
+      *Dpose = Dpi_pn * *Dpose;
     if (Dpoint)
-    *Dpoint = (Dpi_pc * Dpc_point).leftCols<2>();// only 2dof are important for the point (direction-only)
+      *Dpoint = Dpi_pn * *Dpoint;
+
     return pi;
   }
 
@@ -144,9 +165,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);
   }
 
@@ -344,6 +365,17 @@ public:
     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)
+   *  @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 {
+    return Base::project(pw, Dpose, Dpoint);
+  }
+
   /// @}
   /// @name Manifold
   /// @{

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));
 }
 
 /* ************************************************************************* */