add backproject to StereoCamera

gtsam/geometry/StereoCamera.h

@@ -80,6 +80,20 @@ namespace gtsam {
 			return pose().transform_from(cameraPoint);
 		}
 
+		Point3 backproject(const StereoPoint2& z) {
+			Vector measured = z.vector();
+//			std::cout << K_.baseline() << std::endl;
+//			std::cout << K_.fx() << std::endl;
+//			std:: cout << measured[0] << std::endl;
+//			std:: cout << measured[1] << std::endl;
+
+			double Z = K_.baseline()*K_.fx()/(measured[0]-measured[1]);
+			std::cout << "Z " << Z;
+			double X = Z *(measured[0]- K_.px()) / K_.fx();
+			double Y = Z *(measured[2]- K_.py()) / K_.fy();
+			return Point3(X, Y, Z);
+		}
+
 		/** Dimensionality of the tangent space */
 		inline size_t dim() const {
 			return 6;

gtsam/geometry/tests/testStereoCamera.cpp

@@ -100,6 +100,14 @@ TEST( StereoCamera, Dproject_stereo_point)
 	CHECK(assert_equal(expected,actual,1e-8));
 }
 
+TEST( StereoCamera, backproject)
+{
+	Point3 expected(1.2, 2.3, 4.5);
+	StereoPoint2 stereo_point = stereoCam.project(expected);
+	Point3 actual = stereoCam.backproject(stereo_point);
+	CHECK(assert_equal(expected,actual,1e-8));
+}
+
 /* ************************************************************************* */
 	int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
 /* ************************************************************************* */