From b6783e8b89c3a9a8446555d8af9039bcc928541b Mon Sep 17 00:00:00 2001 From: Chris Beall Date: Tue, 29 Mar 2011 01:54:59 +0000 Subject: [PATCH] add backproject to StereoCamera --- gtsam/geometry/StereoCamera.h | 14 ++++++++++++++ gtsam/geometry/tests/testStereoCamera.cpp | 8 ++++++++ 2 files changed, 22 insertions(+) diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index 1998ce79d..579b07233 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/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; diff --git a/gtsam/geometry/tests/testStereoCamera.cpp b/gtsam/geometry/tests/testStereoCamera.cpp index 2aa76732e..77f890279 100644 --- a/gtsam/geometry/tests/testStereoCamera.cpp +++ b/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);} /* ************************************************************************* */