From 3f94791b3bba015d49da7cac8679665b6451fbf1 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Thu, 5 Mar 2015 22:10:30 -0800
Subject: [PATCH] project2 of point at infinity

---
 gtsam/geometry/CalibratedCamera.cpp           | 29 ++++++++++++++-
 gtsam/geometry/CalibratedCamera.h             |  9 +++++
 gtsam/geometry/tests/testCalibratedCamera.cpp | 37 +++++++++++++++++--
 3 files changed, 71 insertions(+), 4 deletions(-)

diff --git a/gtsam/geometry/CalibratedCamera.cpp b/gtsam/geometry/CalibratedCamera.cpp
index 213cae978..fb518f2e3 100644
--- a/gtsam/geometry/CalibratedCamera.cpp
+++ b/gtsam/geometry/CalibratedCamera.cpp
@@ -122,7 +122,7 @@ Point2 PinholeBase::project2(const Point3& point, OptionalJacobian<2, 6> Dpose,
   const Point3 q = pose().transform_to(point, boost::none, Dpoint ? &Rt : 0);
 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
   if (q.z() <= 0)
-  throw CheiralityException();
+    throw CheiralityException();
 #endif
   const Point2 pn = project_to_camera(q);
 
@@ -136,6 +136,33 @@ Point2 PinholeBase::project2(const Point3& point, OptionalJacobian<2, 6> Dpose,
   return pn;
 }
 
+/* ************************************************************************* */
+Point2 PinholeBase::project2(const Unit3& pw, OptionalJacobian<2, 6> Dpose,
+    OptionalJacobian<2, 2> Dpoint) const {
+
+  // world to camera coordinate
+  Matrix23 Dpc_rot;
+  Matrix2 Dpc_point;
+  const Unit3 pc = pose().rotation().unrotate(pw, Dpose ? &Dpc_rot : 0,
+      Dpose ? &Dpc_point : 0);
+
+  // camera to normalized image coordinate
+  Matrix2 Dpn_pc;
+  const Point2 pn = PinholeBase::project_to_camera(pc,
+      Dpose || Dpoint ? &Dpn_pc : 0);
+
+  // chain the Jacobian matrices
+  if (Dpose) {
+    // only rotation is important
+    Matrix26 Dpc_pose;
+    Dpc_pose.setZero();
+    Dpc_pose.leftCols<3>() = Dpc_rot;
+    *Dpose = Dpn_pc * Dpc_pose; // 2x2 * 2x6
+  }
+  if (Dpoint)
+    *Dpoint = Dpn_pc * Dpc_point; // 2x2 * 2*2
+  return pn;
+}
 /* ************************************************************************* */
 Point3 PinholeBase::backproject_from_camera(const Point2& p,
     const double depth) {
diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h
index 56a42f3de..e43cafcc2 100644
--- a/gtsam/geometry/CalibratedCamera.h
+++ b/gtsam/geometry/CalibratedCamera.h
@@ -187,6 +187,15 @@ public:
   Point2 project2(const Point3& point, OptionalJacobian<2, 6> Dpose =
       boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const;
 
+  /**
+   * Project point at infinity into the image
+   * Throws a CheiralityException if point behind image plane iff GTSAM_THROW_CHEIRALITY_EXCEPTION
+   * @param point 3D point in world coordinates
+   * @return the intrinsic coordinates of the projected point
+   */
+  Point2 project2(const Unit3& point, OptionalJacobian<2, 6> Dpose =
+      boost::none, OptionalJacobian<2, 2> Dpoint = boost::none) const;
+
   /// backproject a 2-dimensional point to a 3-dimensional point at given depth
   static Point3 backproject_from_camera(const Point2& p, const double depth);
 
diff --git a/gtsam/geometry/tests/testCalibratedCamera.cpp b/gtsam/geometry/tests/testCalibratedCamera.cpp
index 3b2f35031..af4c65127 100644
--- a/gtsam/geometry/tests/testCalibratedCamera.cpp
+++ b/gtsam/geometry/tests/testCalibratedCamera.cpp
@@ -105,11 +105,12 @@ static Point2 project_to_camera2(const Unit3& point) {
   return PinholeBase::project_to_camera(point);
 }
 
+Unit3 pointAtInfinity(0, 0, 1000);
 TEST( CalibratedCamera, Dproject_to_cameraInfinity) {
-  Unit3 pp(0, 0, 1000);
   Matrix test1;
-  CalibratedCamera::project_to_camera(pp, test1);
-  Matrix test2 = numericalDerivative11<Point2, Unit3>(project_to_camera2, pp);
+  CalibratedCamera::project_to_camera(pointAtInfinity, test1);
+  Matrix test2 = numericalDerivative11<Point2, Unit3>(project_to_camera2,
+      pointAtInfinity);
   CHECK(assert_equal(test1, test2));
 }
 
@@ -144,6 +145,36 @@ TEST( CalibratedCamera, Dproject_point_pose2)
   CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
 }
 
+/* ************************************************************************* */
+static Point2 projectAtInfinity(const CalibratedCamera& camera, const Unit3& point) {
+  return camera.project2(point);
+}
+
+TEST( CalibratedCamera, Dproject_point_pose_infinity)
+{
+  Matrix Dpose, Dpoint;
+  Point2 result = camera.project2(pointAtInfinity, Dpose, Dpoint);
+  Matrix numerical_pose  = numericalDerivative21(projectAtInfinity, camera, pointAtInfinity);
+  Matrix numerical_point = numericalDerivative22(projectAtInfinity, camera, pointAtInfinity);
+  CHECK(assert_equal(Point2(), result));
+  CHECK(assert_equal(numerical_pose,  Dpose, 1e-7));
+  CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
+}
+
+/* ************************************************************************* */
+// Add a test with more arbitrary rotation
+TEST( CalibratedCamera, Dproject_point_pose2_infinity)
+{
+  static const Pose3 pose1(Rot3::ypr(0.1, -0.1, 0.4), Point3(0, 0, -10));
+  static const CalibratedCamera camera(pose1);
+  Matrix Dpose, Dpoint;
+  camera.project2(pointAtInfinity, Dpose, Dpoint);
+  Matrix numerical_pose  = numericalDerivative21(projectAtInfinity, camera, pointAtInfinity);
+  Matrix numerical_point = numericalDerivative22(projectAtInfinity, camera, pointAtInfinity);
+  CHECK(assert_equal(numerical_pose,  Dpose, 1e-7));
+  CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */