landmark triangulation now throws two different exceptions instead of returning boost::none. TriangulationCheiralityException and TriangulationUnderconstrainedException

gtsam_unstable/geometry/tests/testTriangulation.cpp

@@ -89,11 +89,48 @@ TEST( triangulation, twoPoses) {
   poses += level_pose180;
   measurements += Point2(400,400);
 
-  boost::optional<Point3> 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<Pose3> poses;
+  vector<Point2> 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<Pose3> poses;
+  vector<Point2> measurements;
+
+  poses += Pose3();
+  measurements += Point2();
+
+  CHECK_EXCEPTION(triangulatePoint3(poses, measurements, K), TriangulationUnderconstrainedException);
+}
+
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
 /* ************************************************************************* */

gtsam_unstable/geometry/triangulation.cpp

@@ -29,7 +29,7 @@ namespace gtsam {
 /* ************************************************************************* */
 // See Hartley and Zisserman, 2nd Ed., page 312
 Point3 triangulateDLT(const vector<Matrix>& projection_matrices,
-    const vector<Point2>& measurements) {
+    const vector<Point2>& measurements, double rank_tol) {
 
   Matrix A = Matrix_(projection_matrices.size() *2, 4);
 
@@ -45,18 +45,22 @@ Point3 triangulateDLT(const vector<Matrix>& 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<Point3> triangulatePoint3(const vector<Pose3>& poses,
-    const vector<Point2>& measurements, const Cal3_S2& K) {
+Point3 triangulatePoint3(const vector<Pose3>& poses,
+    const vector<Point2>& measurements, const Cal3_S2& K, double rank_tol) {
 
   assert(poses.size() == measurements.size());
 
   if(poses.size() < 2)
-    return boost::none;
+    throw(TriangulationUnderconstrainedException());
 
   vector<Matrix> projection_matrices;
 
@@ -64,13 +68,13 @@ boost::optional<Point3> triangulatePoint3(const vector<Pose3>& 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;

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<Point3> triangulatePoint3(const std::vector<Pose3>& poses,
-    const std::vector<Point2>& measurements, const Cal3_S2& K);
+GTSAM_UNSTABLE_EXPORT Point3 triangulatePoint3(const std::vector<Pose3>& poses,
+    const std::vector<Point2>& measurements, const Cal3_S2& K, double rank_tol =
+        1e-9);
 
 
 } // \namespace gtsam