add backproject

cpp/CalibratedCamera.cpp

@@ -23,6 +23,10 @@ namespace gtsam {
 		return Matrix_(2, 3, d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2);
 	}
 
+	Point3 backproject_from_camera(const Point2& p, const double scale) {
+		return Point3(p.x() * scale, p.y() * scale, scale);
+	}
+
 	/* ************************************************************************* */
 	// Methods
 	/* ************************************************************************* */

cpp/CalibratedCamera.h

@@ -26,6 +26,11 @@ namespace gtsam {
 	 */
 	Matrix Dproject_to_camera1(const Point3& cameraPoint); /*2by3 <--*/
 
+	/**
+	 * backproject a 2-dimensional point to a 3-dimension point
+	 */
+	Point3 backproject_from_camera(const Point2& p, const double scale);
+
 	/**
 	 * A Calibrated camera class [R|-R't], calibration K=I.
 	 * If calibration is known, it is more computationally efficient

cpp/SimpleCamera.cpp

@@ -38,6 +38,12 @@ namespace gtsam {
 		return projected.first;
 	}
 
+	Point3 SimpleCamera::backproject(const Point2& projection, const double scale) const {
+		Point2 intrinsic = K_.calibrate(projection);
+		Point3 cameraPoint = backproject_from_camera(intrinsic, scale);
+		return transform_from(calibrated_.pose(), cameraPoint);
+	}
+
 	SimpleCamera SimpleCamera::level(const Cal3_S2& K, const Pose2& pose2, double height) {
 		return SimpleCamera(K, CalibratedCamera::level(pose2, height));
 	}

cpp/SimpleCamera.h

@@ -48,6 +48,11 @@ namespace gtsam {
 		 */
 		Point2 project(const Point3& P) const;
 
+		/**
+		 * backproject a 2d point from the camera up to a given scale
+		 */
+		Point3 backproject(const Point2& projection, const double scale) const;
+
 		/**
 		 * Create a level camera at the given 2D pose and height
 		 * @param pose2 specifies the location and viewing direction

cpp/testSimpleCamera.cpp

@@ -60,6 +60,15 @@ TEST( SimpleCamera, project)
   CHECK(assert_equal( camera.project(point4), Point2( 100,  100) ));
 }
 
+/* ************************************************************************* */
+TEST( SimpleCamera, backproject)
+{
+  CHECK(assert_equal( camera.backproject(Point2(-100,  100), 0.5),  point1));
+  CHECK(assert_equal( camera.backproject(Point2(-100, -100), 0.5),  point2));
+  CHECK(assert_equal( camera.backproject(Point2( 100, -100), 0.5),  point3));
+  CHECK(assert_equal( camera.backproject(Point2( 100,  100), 0.5),  point4));
+}
+
 /* ************************************************************************* */
 Point2 project2(const Pose3& pose, const Point3& point) {
 	return project(SimpleCamera(K,pose), point);