SimpleCamera::level now works

cpp/SimpleCamera.cpp

@@ -30,7 +30,7 @@ namespace gtsam {
 		return projection;
 	}
 
-	SimpleCamera::SimpleCamera level(const Cal3_S2& K, const Pose2& pose2, double height) {
+	SimpleCamera SimpleCamera::level(const Cal3_S2& K, const Pose2& pose2, double height) {
 		return SimpleCamera(K, CalibratedCamera::level(pose2, height));
 	}
 

cpp/SimpleCamera.h

@@ -42,7 +42,7 @@ namespace gtsam {
 		/**
 		 * Create a level camera at the given 2D pose and height
 		 */
-		static CalibratedCamera level(const Cal3_S2& K, const Pose2& pose2, double height);
+		static SimpleCamera level(const Cal3_S2& K, const Pose2& pose2, double height);
 
 		// Friends
 		friend Matrix Dproject_pose(const SimpleCamera& camera,  const Point3& point);

cpp/testSimpleCamera.cpp

@@ -37,6 +37,20 @@ TEST( SimpleCamera, constructor)
   CHECK(assert_equal( camera.pose(), pose1));
 }
 
+/* ************************************************************************* */
+TEST( SimpleCamera, level2)
+{
+	// Create a level camera, looking in Y-direction
+	Pose2 pose2(400,300,M_PI_2);
+	SimpleCamera camera = SimpleCamera::level(K, pose2, 100);
+
+	// expected
+	Point3 x(1,0,0),y(0,0,-1),z(0,1,0);
+	Rot3 wRc(x,y,z);
+	Pose3 expected(wRc,Point3(400,300,100));
+  CHECK(assert_equal( camera.pose(), expected));
+}
+
 /* ************************************************************************* */
 TEST( SimpleCamera, project)
 {