From 092093444c8f687b82d05d6f14d594b3190d1ae5 Mon Sep 17 00:00:00 2001
From: Duy-Nguyen Ta <thduynguyen@gmail.com>
Date: Wed, 6 Jun 2012 09:33:03 +0000
Subject: [PATCH] lookat: a convenient function to set up camera pose

---
 gtsam/geometry/PinholeCamera.h            | 32 ++++++++++++++++++-----
 gtsam/geometry/tests/testSimpleCamera.cpp | 13 +++++++++
 2 files changed, 38 insertions(+), 7 deletions(-)

diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h
index 587312062..acc27ae1c 100644
--- a/gtsam/geometry/PinholeCamera.h
+++ b/gtsam/geometry/PinholeCamera.h
@@ -149,16 +149,34 @@ namespace gtsam {
      * (theta 0 = looking in direction of positive X axis)
      */
     static PinholeCamera level(const Pose2& pose2, double height) {
-        return PinholeCamera::level(Calibration(), pose2, height);
+    	return PinholeCamera::level(Calibration(), pose2, height);
     }
 
     static PinholeCamera level(const Calibration &K, const Pose2& pose2, double height) {
-        const double st = sin(pose2.theta()), ct = cos(pose2.theta());
-        const Point3 x(st, -ct, 0), y(0, 0, -1), z(ct, st, 0);
-        const Rot3 wRc(x, y, z);
-        const Point3 t(pose2.x(), pose2.y(), height);
-        const Pose3 pose3(wRc, t);
-        return PinholeCamera(pose3, K);
+    	const double st = sin(pose2.theta()), ct = cos(pose2.theta());
+    	const Point3 x(st, -ct, 0), y(0, 0, -1), z(ct, st, 0);
+    	const Rot3 wRc(x, y, z);
+    	const Point3 t(pose2.x(), pose2.y(), height);
+    	const Pose3 pose3(wRc, t);
+    	return PinholeCamera(pose3, K);
+    }
+
+    /**
+     * Create a camera at the given eye position looking at a target point in the scene
+     * with the specified up direction vector.
+     * @param eye specifies the camera position
+     * @param target the point to look at
+     * @param upVector specifies the camera up direction vector,
+     * 				doesn't need to be on the image plane nor orthogonal to the viewing axis
+     * @param K optional calibration parameter
+     */
+    static PinholeCamera lookat(const Point3& eye, const Point3& target, const Point3& upVector, const Calibration& K = Calibration()) {
+    	Point3 zc = target-eye;
+    	zc = zc/zc.norm();
+    	Point3 xc = (-upVector).cross(zc);	// minus upVector since yc is pointing down
+    	Point3 yc = zc.cross(xc);
+    	Pose3 pose3(Rot3(xc,yc,zc), eye);
+    	return PinholeCamera(pose3, K);
     }
 
     /// @}
diff --git a/gtsam/geometry/tests/testSimpleCamera.cpp b/gtsam/geometry/tests/testSimpleCamera.cpp
index 1990ad00a..e918606a1 100644
--- a/gtsam/geometry/tests/testSimpleCamera.cpp
+++ b/gtsam/geometry/tests/testSimpleCamera.cpp
@@ -63,6 +63,19 @@ TEST( SimpleCamera, level2)
   CHECK(assert_equal( camera.pose(), expected));
 }
 
+/* ************************************************************************* */
+TEST( SimpleCamera, lookat)
+{
+	// Create a level camera, looking in Y-direction
+	Point3 C(10.0,0.0,0.0);
+	SimpleCamera camera = SimpleCamera::lookat(C, Point3(), Point3(0.0,0.0,1.0));
+
+	// expected
+	Point3 xc(0,1,0),yc(0,0,-1),zc(-1,0,0);
+	Pose3 expected(Rot3(xc,yc,zc),C);
+  CHECK(assert_equal( camera.pose(), expected));
+}
+
 /* ************************************************************************* */
 TEST( SimpleCamera, project)
 {