lookat: a convenient function to set up camera pose

gtsam/geometry/PinholeCamera.h

@@ -161,6 +161,24 @@ namespace gtsam {
     	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);
+    }
+
     /// @}
     /// @name Transformations and measurement functions
     /// @{

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)
 {