From f3f09b17a76079e9deb854a15eb514c250e27634 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Thu, 5 Mar 2015 20:26:47 -0800
Subject: [PATCH] project_to_camera for Unit3 (points at infinity)

---
 gtsam/geometry/CalibratedCamera.cpp           | 13 +++++++++++
 gtsam/geometry/CalibratedCamera.h             |  8 +++++++
 gtsam/geometry/tests/testCalibratedCamera.cpp | 22 ++++++++++++++++---
 3 files changed, 40 insertions(+), 3 deletions(-)

diff --git a/gtsam/geometry/CalibratedCamera.cpp b/gtsam/geometry/CalibratedCamera.cpp
index 33f2c84eb..213cae978 100644
--- a/gtsam/geometry/CalibratedCamera.cpp
+++ b/gtsam/geometry/CalibratedCamera.cpp
@@ -94,6 +94,19 @@ Point2 PinholeBase::project_to_camera(const Point3& pc,
   return Point2(u, v);
 }
 
+/* ************************************************************************* */
+Point2 PinholeBase::project_to_camera(const Unit3& pc,
+    OptionalJacobian<2, 2> Dpoint) {
+  if (Dpoint) {
+    Matrix32 Dpoint3_pc;
+    Matrix23 Duv_point3;
+    Point2 uv = project_to_camera(pc.point3(Dpoint3_pc), Duv_point3);
+    *Dpoint = Duv_point3 * Dpoint3_pc;
+    return uv;
+  } else
+    return project_to_camera(pc.point3());
+}
+
 /* ************************************************************************* */
 pair<Point2, bool> PinholeBase::projectSafe(const Point3& pw) const {
   const Point3 pc = pose().transform_to(pw);
diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h
index 35789053e..56a42f3de 100644
--- a/gtsam/geometry/CalibratedCamera.h
+++ b/gtsam/geometry/CalibratedCamera.h
@@ -167,6 +167,14 @@ public:
   static Point2 project_to_camera(const Point3& pc, //
       OptionalJacobian<2, 3> Dpoint = boost::none);
 
+  /**
+   * Project from 3D point at infinity in camera coordinates into image
+   * Does *not* throw a CheiralityException, even if pc behind image plane
+   * @param pc point in camera coordinates
+   */
+  static Point2 project_to_camera(const Unit3& pc, //
+      OptionalJacobian<2, 2> Dpoint = boost::none);
+
   /// Project a point into the image and check depth
   std::pair<Point2, bool> projectSafe(const Point3& pw) const;
 
diff --git a/gtsam/geometry/tests/testCalibratedCamera.cpp b/gtsam/geometry/tests/testCalibratedCamera.cpp
index b1e265266..3b2f35031 100644
--- a/gtsam/geometry/tests/testCalibratedCamera.cpp
+++ b/gtsam/geometry/tests/testCalibratedCamera.cpp
@@ -88,12 +88,28 @@ TEST( CalibratedCamera, project)
 }
 
 /* ************************************************************************* */
+static Point2 project_to_camera1(const Point3& point) {
+  return PinholeBase::project_to_camera(point);
+}
+
 TEST( CalibratedCamera, Dproject_to_camera1) {
-  Point3 pp(155,233,131);
+  Point3 pp(155, 233, 131);
   Matrix test1;
   CalibratedCamera::project_to_camera(pp, test1);
-  Matrix test2 = numericalDerivative11<Point2,Point3>(
-      boost::bind(CalibratedCamera::project_to_camera, _1, boost::none), pp);
+  Matrix test2 = numericalDerivative11<Point2, Point3>(project_to_camera1, pp);
+  CHECK(assert_equal(test1, test2));
+}
+
+/* ************************************************************************* */
+static Point2 project_to_camera2(const Unit3& point) {
+  return PinholeBase::project_to_camera(point);
+}
+
+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);
   CHECK(assert_equal(test1, test2));
 }