static members for creating "level" cameras as we find on idealized robots

cpp/CalibratedCamera.cpp

@@ -22,6 +22,8 @@ namespace gtsam {
 		return Matrix_(2, 3, d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2);
 	}
 
+	/* ************************************************************************* */
+	// Methods
 	/* ************************************************************************* */
 
 	CalibratedCamera::CalibratedCamera(const Pose3& pose) :
@@ -31,6 +33,15 @@ namespace gtsam {
 	CalibratedCamera::~CalibratedCamera() {
 	}
 
+	CalibratedCamera CalibratedCamera::level(const Pose2& pose2, double height) {
+		double st = sin(pose2.theta()), ct = cos(pose2.theta());
+		Point3 x(st, -ct, 0), y(0, 0, -1), z(ct, st, 0);
+		Rot3 wRc(x, y, z);
+		Point3 t(pose2.x(), pose2.y(), height);
+		Pose3 pose3(wRc, t);
+		return CalibratedCamera(pose3);
+	}
+
 	Point2 CalibratedCamera::project(const Point3 & P) const {
 		Point3 cameraPoint = transform_to(pose_, P);
 		Point2 intrinsic = project_to_camera(cameraPoint);
@@ -38,6 +49,9 @@ namespace gtsam {
 	}
 
 	/* ************************************************************************* */
+	// measurement functions and derivatives
+	/* ************************************************************************* */
+
 	Point2 project(const CalibratedCamera& camera, const Point3& point) {
 		return camera.project(point);
 	}

cpp/CalibratedCamera.h

@@ -9,6 +9,7 @@
 #define CalibratedCAMERA_H_
 
 #include "Point2.h"
+#include "Pose2.h"
 #include "Pose3.h"
 
 namespace gtsam {
@@ -41,6 +42,11 @@ namespace gtsam {
 			return pose_;
 		}
 
+		/**
+		 * Create a level camera at the given 2D pose and height
+		 */
+		static CalibratedCamera level(const Pose2& pose2, double height);
+
 		Point2 project(const Point3& P) const;
 	};
 

cpp/Pose2.h

@@ -42,6 +42,11 @@ namespace gtsam {
 			x_(t.x()), y_(t.y()), theta_(theta) {
 		}
 
+    /** get functions for x, y, theta */
+		double x()     const { return x_;}
+		double y()     const { return y_;}
+		double theta() const { return theta_;}
+
 		/** print with optional string */
 		void print(const std::string& s = "") const;
 

cpp/SimpleCamera.cpp

@@ -12,6 +12,11 @@ namespace gtsam {
 
 	/* ************************************************************************* */
 
+	SimpleCamera::SimpleCamera(const Cal3_S2& K,
+			const CalibratedCamera& calibrated) :
+		calibrated_(calibrated), K_(K) {
+	}
+
 	SimpleCamera::SimpleCamera(const Cal3_S2& K, const Pose3& pose) :
 		calibrated_(pose), K_(K) {
 	}
@@ -25,7 +30,14 @@ namespace gtsam {
 		return projection;
 	}
 
+	SimpleCamera::SimpleCamera level(const Cal3_S2& K, const Pose2& pose2, double height) {
+		return SimpleCamera(K, CalibratedCamera::level(pose2, height));
+	}
+
 	/* ************************************************************************* */
+	// measurement functions and derivatives
+	/* ************************************************************************* */
+
 	Point2 project(const SimpleCamera& camera, const Point3& point) {
 		return camera.project(point);
 	}

cpp/SimpleCamera.h

@@ -25,6 +25,7 @@ namespace gtsam {
 		Cal3_S2 K_; // Calibration
 
 	public:
+		SimpleCamera(const Cal3_S2& K, const CalibratedCamera& calibrated);
 		SimpleCamera(const Cal3_S2& K, const Pose3& pose);
 		virtual ~SimpleCamera();
 
@@ -38,6 +39,11 @@ namespace gtsam {
 
 		Point2 project(const Point3& P) const;
 
+		/**
+		 * Create a level camera at the given 2D pose and height
+		 */
+		static CalibratedCamera level(const Cal3_S2& K, const Pose2& pose2, double height);
+
 		// Friends
 		friend Matrix Dproject_pose(const SimpleCamera& camera,  const Point3& point);
 		friend Matrix Dproject_point(const SimpleCamera& camera, const Point3& point);

cpp/testCalibratedCamera.cpp

@@ -33,6 +33,34 @@ TEST( CalibratedCamera, constructor)
   CHECK(assert_equal( camera.pose(), pose1));
 }
 
+/* ************************************************************************* */
+TEST( CalibratedCamera, level1)
+{
+	// Create a level camera, looking in X-direction
+	Pose2 pose2(100,200,0);
+	CalibratedCamera camera = CalibratedCamera::level(pose2, 300);
+
+	// expected
+	Point3 x(0,-1,0),y(0,0,-1),z(1,0,0);
+	Rot3 wRc(x,y,z);
+	Pose3 expected(wRc,Point3(100,200,300));
+  CHECK(assert_equal( camera.pose(), expected));
+}
+
+/* ************************************************************************* */
+TEST( CalibratedCamera, level2)
+{
+	// Create a level camera, looking in Y-direction
+	Pose2 pose2(400,300,M_PI_2);
+	CalibratedCamera camera = CalibratedCamera::level(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( CalibratedCamera, project)
 {

cpp/testSimpleCamera.cpp

@@ -4,6 +4,7 @@
  * based on testVSLAMFactor.cpp
  */
 
+#include <math.h>
 #include <iostream>
 
 #include <CppUnitLite/TestHarness.h>