From 48d159120ddd95fece32938bc66a18e05638a7c2 Mon Sep 17 00:00:00 2001 From: zhaoyang Date: Thu, 11 Jun 2015 11:40:40 -0400 Subject: [PATCH] create a backproject2, with optional Jacobians --- gtsam/geometry/StereoCamera.cpp | 23 +++++++++++++ gtsam/geometry/StereoCamera.h | 8 +++++ gtsam/geometry/tests/testStereoCamera.cpp | 42 +++++++++++++++++++++-- 3 files changed, 70 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/StereoCamera.cpp b/gtsam/geometry/StereoCamera.cpp index 9e5b88b31..3d334b2dd 100644 --- a/gtsam/geometry/StereoCamera.cpp +++ b/gtsam/geometry/StereoCamera.cpp @@ -95,4 +95,27 @@ namespace gtsam { return world_point; } + /* ************************************************************************* */ + Point3 StereoCamera::backproject2(const StereoPoint2& z, OptionalJacobian<3, 6> H1, + OptionalJacobian<3, 3> H2) { + const Cal3_S2Stereo& K = *K_; + const double fx = K.fx(), fy = K.fy(), cx = K.cx(), cy = K.cy(), b = K.baseline(); + + Vector measured = z.vector(); + double Z = b * fx / (measured[0] - measured[1]); + double X = Z * (measured[0] - cx) / fx; + double Y = Z * (measured[2] - cy) / fy; + + if(H1 || H2) { + if(H1) { + // do something here + } + if(H2) { + + } + } + + return leftCamPose_.transform_from(Point3(X, Y, Z)); + } + } diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index 35cf437e9..f09626c9d 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -137,6 +137,14 @@ public: /// back-project a measurement Point3 backproject(const StereoPoint2& z) const; + /** Back-project the 2D point and compute optional derivatives + * @param H1 derivative with respect to pose + * @param H2 derivative with respect to point + */ + Point3 backproject2(const StereoPoint2& z, + OptionalJacobian<3, 6> H1 = boost::none, + OptionalJacobian<3, 3> H2 = boost::none) const; + /// @} /// @name Deprecated /// @{ diff --git a/gtsam/geometry/tests/testStereoCamera.cpp b/gtsam/geometry/tests/testStereoCamera.cpp index 45f26c244..6d6972215 100644 --- a/gtsam/geometry/tests/testStereoCamera.cpp +++ b/gtsam/geometry/tests/testStereoCamera.cpp @@ -99,13 +99,13 @@ TEST( StereoCamera, Dproject) Matrix actual1; stereoCam.project2(landmark, actual1, boost::none); CHECK(assert_equal(expected1,actual1,1e-7)); - Matrix expected2 = numericalDerivative32(project3,camPose, landmark, *K); + Matrix expected2 = numericalDerivative32(project3, camPose, landmark, *K); Matrix actual2; stereoCam.project2(landmark, boost::none, actual2); CHECK(assert_equal(expected2,actual2,1e-7)); } /* ************************************************************************* */ -TEST( StereoCamera, backproject) +TEST( StereoCamera, backproject_case1) { Cal3_S2Stereo::shared_ptr K2(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5)); StereoCamera stereoCam2(Pose3(), K2); @@ -117,7 +117,7 @@ TEST( StereoCamera, backproject) } /* ************************************************************************* */ -TEST( StereoCamera, backproject2) +TEST( StereoCamera, backproject_case2) { Rot3 R(0.589511291, -0.804859792, 0.0683931805, -0.804435942, -0.592650676, -0.0405925523, @@ -132,6 +132,42 @@ TEST( StereoCamera, backproject2) CHECK(assert_equal(z, actual, 1e-3)); } +static Point3 backproject3(const Pose3& pose, const StereoPoint2& point, const Cal3_S2Stereo& K) { + return StereoCamera(pose, boost::make_shared(K)).backproject(point); +} + +/* ************************************************************************* */ +TEST( StereoCamera, backproject2_case1) +{ + Cal3_S2Stereo::shared_ptr K2(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5)); + StereoCamera stereoCam2(Pose3(), K2); + + Point3 expected_point(1.2, 2.3, 4.5); + StereoPoint2 stereo_point = stereoCam2.project(expected_point); + + Matrix actual_jacobian_1, actual_jacobian_2; + Point3 actual_point = stereoCam2.backproject2(stereo_point, actual_jacobian_1, actual_jacobian_2); + CHECK(assert_equal(expected_point, actual_point, 1e-8)); + + Matrix expected_jacobian = numericalDerivative32(backproject3, Pose3(), stereo_point, *K2); + CHECK(assert_equal(expected_jacobian, actual_jacobian_1, acutal_jacobian_2)); +} + +TEST( StereoCamera, backproject2_case2) +{ + Rot3 R(0.589511291, -0.804859792, 0.0683931805, + -0.804435942, -0.592650676, -0.0405925523, + 0.0732045588, -0.0310882277, -0.996832359); + Point3 t(53.5239823, 23.7866016, -4.42379876); + Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1733.75, 1733.75, 0, 689.645, 508.835, 0.0699612)); + StereoCamera camera(Pose3(R,t), K); + StereoPoint2 z(184.812, 129.068, 714.768); + + Point3 l = camera.backproject2(z); + StereoPoint2 actual = camera.project(l); + CHECK(assert_equal(z, actual, 1e-3)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */