From ab51b50a9a993dae5f09e665ef54cfd6ca992144 Mon Sep 17 00:00:00 2001 From: Chris Beall Date: Mon, 19 Aug 2013 20:10:36 +0000 Subject: [PATCH] landmark triangulation now throws two different exceptions instead of returning boost::none. TriangulationCheiralityException and TriangulationUnderconstrainedException --- .../geometry/tests/testTriangulation.cpp | 43 +++++++++++++++++-- gtsam_unstable/geometry/triangulation.cpp | 18 +++++--- gtsam_unstable/geometry/triangulation.h | 23 +++++++++- 3 files changed, 72 insertions(+), 12 deletions(-) diff --git a/gtsam_unstable/geometry/tests/testTriangulation.cpp b/gtsam_unstable/geometry/tests/testTriangulation.cpp index 3f910e2a4..fbbc62991 100644 --- a/gtsam_unstable/geometry/tests/testTriangulation.cpp +++ b/gtsam_unstable/geometry/tests/testTriangulation.cpp @@ -89,11 +89,48 @@ TEST( triangulation, twoPoses) { poses += level_pose180; measurements += Point2(400,400); - boost::optional triangulated_4cameras = triangulatePoint3(poses, measurements, K); - EXPECT(boost::none == triangulated_4cameras); - + CHECK_EXCEPTION(triangulatePoint3(poses, measurements, K), TriangulationCheiralityException); } +/* ************************************************************************* */ +TEST( triangulation, twoIdenticalPoses) { + Cal3_S2 K(1500, 1200, 0, 640, 480); + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + SimpleCamera level_camera(level_pose, K); + + // landmark ~5 meters infront of camera + Point3 landmark(5, 0.5, 1.2); + + // 1. Project two landmarks into two cameras and triangulate + Point2 level_uv = level_camera.project(landmark); + + vector poses; + vector measurements; + + poses += level_pose, level_pose; + measurements += level_uv, level_uv; + + CHECK_EXCEPTION(triangulatePoint3(poses, measurements, K), TriangulationUnderconstrainedException); +} + +/* ************************************************************************* */ +TEST( triangulation, onePose) { + // we expect this test to fail with a TriangulationUnderconstrainedException + // because there's only one camera observation + + Cal3_S2 K(1500, 1200, 0, 640, 480); + + vector poses; + vector measurements; + + poses += Pose3(); + measurements += Point2(); + + CHECK_EXCEPTION(triangulatePoint3(poses, measurements, K), TriangulationUnderconstrainedException); +} + + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ diff --git a/gtsam_unstable/geometry/triangulation.cpp b/gtsam_unstable/geometry/triangulation.cpp index 28270afdf..09654d3f1 100644 --- a/gtsam_unstable/geometry/triangulation.cpp +++ b/gtsam_unstable/geometry/triangulation.cpp @@ -29,7 +29,7 @@ namespace gtsam { /* ************************************************************************* */ // See Hartley and Zisserman, 2nd Ed., page 312 Point3 triangulateDLT(const vector& projection_matrices, - const vector& measurements) { + const vector& measurements, double rank_tol) { Matrix A = Matrix_(projection_matrices.size() *2, 4); @@ -45,18 +45,22 @@ Point3 triangulateDLT(const vector& projection_matrices, int rank; double error; Vector v; - boost::tie(rank, error, v) = DLT(A); + boost::tie(rank, error, v) = DLT(A, rank_tol); + + if(rank < 3) + throw(TriangulationUnderconstrainedException()); + return Point3(sub( (v / v(3)),0,3)); } /* ************************************************************************* */ -boost::optional triangulatePoint3(const vector& poses, - const vector& measurements, const Cal3_S2& K) { +Point3 triangulatePoint3(const vector& poses, + const vector& measurements, const Cal3_S2& K, double rank_tol) { assert(poses.size() == measurements.size()); if(poses.size() < 2) - return boost::none; + throw(TriangulationUnderconstrainedException()); vector projection_matrices; @@ -64,13 +68,13 @@ boost::optional triangulatePoint3(const vector& poses, BOOST_FOREACH(const Pose3& pose, poses) projection_matrices += K.matrix() * sub(pose.inverse().matrix(),0,3,0,4); - Point3 triangulated_point = triangulateDLT(projection_matrices, measurements); + Point3 triangulated_point = triangulateDLT(projection_matrices, measurements, rank_tol); // verify that the triangulated point lies infront of all cameras BOOST_FOREACH(const Pose3& pose, poses) { const Point3& p_local = pose.transform_to(triangulated_point); if(p_local.z() <= 0) - return boost::none; + throw(TriangulationCheiralityException()); } return triangulated_point; diff --git a/gtsam_unstable/geometry/triangulation.h b/gtsam_unstable/geometry/triangulation.h index c8b55e8e7..25f52918a 100644 --- a/gtsam_unstable/geometry/triangulation.h +++ b/gtsam_unstable/geometry/triangulation.h @@ -26,6 +26,23 @@ namespace gtsam { +/// Exception thrown by triangulateDLT when SVD returns rank < 3 +class GTSAM_UNSTABLE_EXPORT TriangulationUnderconstrainedException: public std::runtime_error { +public: + TriangulationUnderconstrainedException() : + std::runtime_error("Triangulation Underconstrained Exception.") { + } +}; + +/// Exception thrown by triangulateDLT when SVD returns rank < 3 +class GTSAM_UNSTABLE_EXPORT TriangulationCheiralityException: public std::runtime_error { +public: + TriangulationCheiralityException() : + std::runtime_error( + "Triangulation Cheirality Exception: The resulting landmark is behind one or more cameras.") { + } +}; + /** * Function to triangulate 3D landmark point from an arbitrary number * of poses (at least 2) using the DLT. The function checks that the @@ -34,10 +51,12 @@ namespace gtsam { * @param poses A vector of camera poses * @param measurements A vector of camera measurements * @param K The camera calibration + * @param rank tolerance, default 1e-9 * @return Returns a Point3 on success, boost::none otherwise. */ -GTSAM_UNSTABLE_EXPORT boost::optional triangulatePoint3(const std::vector& poses, - const std::vector& measurements, const Cal3_S2& K); +GTSAM_UNSTABLE_EXPORT Point3 triangulatePoint3(const std::vector& poses, + const std::vector& measurements, const Cal3_S2& K, double rank_tol = + 1e-9); } // \namespace gtsam