supertriangulation! with spherical camera

gtsam/geometry/tests/testTriangulation.cpp

@@ -484,13 +484,65 @@ TEST( triangulation, twoPoses_sphericalCamera) {
   optimize = false;
   boost::optional<Point3> actual3 = //
       triangulatePoint3<SphericalCamera>(cameras, measurements, rank_tol, optimize);
-  EXPECT(assert_equal(Point3(5.94321,0.654327,1.48029), *actual3, 1e-4));
+  EXPECT(assert_equal(Point3(5.9432, 0.654319, 1.48192), *actual3, 1e-3));
 
   // 6. Now with optimization on
   optimize = true;
   boost::optional<Point3> actual4 = //
       triangulatePoint3<SphericalCamera>(cameras, measurements, rank_tol, optimize);
-  EXPECT(assert_equal(Point3(5.94321,0.654327,1.48029), *actual4, 1e-4));
+  EXPECT(assert_equal(Point3(5.9432, 0.654334, 1.48192), *actual4, 1e-3));
+}
+
+//******************************************************************************
+TEST( triangulation, twoPoses_sphericalCamera_extremeFOV) {
+
+  vector<Pose3> poses;
+  std::vector<Unit3> measurements;
+
+  // Project landmark into two cameras and triangulate
+  Pose3 poseA = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0.0,0.0,0.0)); // with z pointing along x axis of global frame
+  Pose3 poseB = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(2.0,0.0,0.0)); // 2m in front of poseA
+  Point3 landmarkL(1.0,-1.0,0.0);// 1m to the right of both cameras, in front of poseA, behind poseB
+  SphericalCamera cam1(poseA);
+  SphericalCamera cam2(poseB);
+  Unit3 u1 = cam1.project(landmarkL);
+  Unit3 u2 = cam2.project(landmarkL);
+
+  EXPECT(assert_equal(Unit3(Point3(1.0,0.0,1.0)), u1, 1e-7)); // in front and to the right of PoseA
+  EXPECT(assert_equal(Unit3(Point3(1.0,0.0,-1.0)), u2, 1e-7));// behind and to the right of PoseB
+
+  poses += pose1, pose2;
+  measurements += u1, u2;
+
+  CameraSet<SphericalCamera> cameras;
+  cameras.push_back(cam1);
+  cameras.push_back(cam2);
+
+  double rank_tol = 1e-9;
+
+  // 1. Test simple DLT, when 1 point is behind spherical camera
+  bool optimize = false;
+#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
+  CHECK_EXCEPTION(
+      triangulatePoint3<SphericalCamera>(cameras, measurements, rank_tol,
+                                         optimize), TriangulationCheiralityException);
+#else // otherwise project should not throw the exception
+  boost::optional<Point3> actual1 =  //
+  triangulatePoint3<SphericalCamera>(cameras, measurements, rank_tol, optimize);
+  EXPECT(assert_equal(landmarkL, *actual1, 1e-7));
+#endif
+
+  // 2. test with optimization on, same answer
+  optimize = true;
+#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
+  CHECK_EXCEPTION(
+      triangulatePoint3<SphericalCamera>(cameras, measurements, rank_tol,
+                                         optimize), TriangulationCheiralityException);
+#else // otherwise project should not throw the exception
+  boost::optional<Point3> actual1 =  //
+  triangulatePoint3<SphericalCamera>(cameras, measurements, rank_tol, optimize);
+  EXPECT(assert_equal(landmarkL, *actual1, 1e-7));
+#endif
 }
 
 //******************************************************************************

gtsam/geometry/triangulation.cpp

@@ -53,31 +53,55 @@ Vector4 triangulateHomogeneousDLT(
   return v;
 }
 
+Vector4 triangulateHomogeneousDLT(
+    const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
+    const std::vector<Unit3>& measurements, double rank_tol) {
+
+  // number of cameras
+  size_t m = projection_matrices.size();
+
+  // Allocate DLT matrix
+  Matrix A = Matrix::Zero(m * 2, 4);
+
+  for (size_t i = 0; i < m; i++) {
+    size_t row = i * 2;
+    const Matrix34& projection = projection_matrices.at(i);
+    const Point3& p = measurements.at(i).point3(); // to get access to x,y,z of the bearing vector
+
+    // build system of equations
+    A.row(row) = p.x() * projection.row(2) - p.z() * projection.row(0);
+    A.row(row + 1) = p.y() * projection.row(2) - p.z() * projection.row(1);
+  }
+  int rank;
+  double error;
+  Vector v;
+  boost::tie(rank, error, v) = DLT(A, rank_tol);
+
+  if (rank < 3)
+    throw(TriangulationUnderconstrainedException());
+
+  return v;
+}
+
 Point3 triangulateDLT(
     const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
     const Point2Vector& measurements, double rank_tol) {
 
   Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements,
                                         rank_tol);
-
   // Create 3D point from homogeneous coordinates
   return Point3(v.head<3>() / v[3]);
 }
 
 Point3 triangulateDLT(
     const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
-    const std::vector<Unit3>& unit3measurements, double rank_tol) {
+    const std::vector<Unit3>& measurements, double rank_tol) {
 
-  Point2Vector measurements;
+  // contrary to previous triangulateDLT, this is now taking Unit3 inputs
-  for (const Unit3& pu : unit3measurements) {  // get canonical pixel projection from Unit3
+  Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements,
-    Point3 p = pu.point3();
+                                         rank_tol);
-#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
+   // Create 3D point from homogeneous coordinates
-    if (p.z() <= 0) // TODO: maybe we should handle this more delicately
+   return Point3(v.head<3>() / v[3]);
-      throw(TriangulationCheiralityException());
-#endif
-    measurements.push_back(Point2(p.x() / p.z(), p.y() / p.z()));
-  }
-  return triangulateDLT(projection_matrices, measurements, rank_tol);
 }
 
 ///

gtsam/geometry/triangulation.h

@@ -59,6 +59,18 @@ GTSAM_EXPORT Vector4 triangulateHomogeneousDLT(
     const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
     const Point2Vector& measurements, double rank_tol = 1e-9);
 
+/**
+ * Same math as Hartley and Zisserman, 2nd Ed., page 312, but with unit-norm bearing vectors
+ * (contrarily to pinhole projection, the z entry is not assumed to be 1 as in Hartley and Zisserman)
+ * @param projection_matrices Projection matrices (K*P^-1)
+ * @param measurements Unit3 bearing measurements
+ * @param rank_tol SVD rank tolerance
+ * @return Triangulated point, in homogeneous coordinates
+ */
+GTSAM_EXPORT Vector4 triangulateHomogeneousDLT(
+    const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
+    const std::vector<Unit3>& measurements, double rank_tol = 1e-9);
+
 /**
  * DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312
  * @param projection_matrices Projection matrices (K*P^-1)

gtsam/slam/tests/testSmartProjectionFactorP.cpp

@@ -1148,7 +1148,7 @@ TEST( SmartProjectionFactorP, optimization_3poses_sphericalCamera ) {
                    -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598),
                    Point3(0.1, -0.1, 1.9)), values.at<Pose3>(x3)));
 
-  DOUBLES_EQUAL(0.1584588987292, graph.error(values), 1e-9);
+  DOUBLES_EQUAL(0.15734109864597129, graph.error(values), 1e-9);
 
   Values result;
   LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);