initial LOST implementation tested

gtsam/geometry/tests/testTriangulation.cpp

@@ -96,6 +96,24 @@ TEST(triangulation, twoPoses) {
   EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-4));
 }
 
+TEST(triangulation, twoCamerasLOST) {
+  std::vector<PinholeCamera<Cal3_S2>> cameras = {camera1, camera2};
+  std::vector<Point2> measurements = {z1, z2};
+
+  // 1. Test simple triangulation, perfect in no noise situation
+  Point3 actual1 =  //
+      triangulateLOSTPoint3<Cal3_S2>(cameras, measurements);
+  EXPECT(assert_equal(landmark, actual1, 1e-12));
+
+  // 3. Add some noise and try again: result should be ~ (4.995,
+  // 0.499167, 1.19814)
+  measurements[0] += Point2(0.1, 0.5);
+  measurements[1] += Point2(-0.2, 0.3);
+  Point3 actual2 =  //
+      triangulateLOSTPoint3<Cal3_S2>(cameras, measurements);
+  EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), actual2, 1e-4));
+}
+
 //******************************************************************************
 // Simple test with a well-behaved two camera situation with Cal3DS2 calibration.
 TEST(triangulation, twoPosesCal3DS2) {

gtsam/geometry/triangulation.cpp

@@ -39,6 +39,11 @@ Vector4 triangulateHomogeneousDLT(
     const Point2& p = measurements.at(i);
 
     // build system of equations
+    // [A_1; A_2; A_3] x = [b_1; b_2; b_3]
+    // [b_3 * A_1 - b_1 * A_3] x = 0
+    // [b_3 * A_2 - b_2 * A_3] x = 0
+    // A' x = 0
+    // A' 2x4 = [b_3 * A_1 - b_1 * A_3; b_3 * A_2 - b_2 * A_3]
     A.row(row) = p.x() * projection.row(2) - projection.row(0);
     A.row(row + 1) = p.y() * projection.row(2) - projection.row(1);
   }
@@ -53,6 +58,47 @@ Vector4 triangulateHomogeneousDLT(
   return v;
 }
 
+Vector3 triangulateLOSTHomogeneous(
+    const std::vector<Pose3>& poses,
+    const std::vector<Point3>& calibrated_measurements) {
+
+  // TODO(akshay-krishnan): make this an argument.
+  const double sigma_x = 1e-3;
+
+  size_t m = calibrated_measurements.size();
+  assert(m == poses.size());
+
+  // Construct the system matrices.
+  Matrix A = Matrix::Zero(m * 2, 3);
+  Matrix b = Matrix::Zero(m * 2, 1);
+
+  for (size_t i = 0; i < m; i++) {
+    const Pose3& wTi = poses[i];
+    // TODO(akshay-krishnan): are there better ways to select j?
+    const int j = (i + 1) % m;
+    const Pose3& wTj = poses[j];
+
+    Point3 d_ij = wTj.translation() -  wTi.translation();
+
+    Point3 w_measurement_i = wTi.rotation().rotate(calibrated_measurements[i]);
+    Point3 w_measurement_j = wTj.rotation().rotate(calibrated_measurements[j]);
+    
+    double numerator =  w_measurement_i.cross(w_measurement_j).norm();
+    double denominator = d_ij.cross(w_measurement_j).norm();
+
+    double q_i = numerator / (sigma_x * denominator);
+    Matrix23 coefficient_mat = q_i * skewSymmetric(calibrated_measurements[i]).topLeftCorner(2, 3) * wTi.rotation().matrix().transpose();
+
+    A.row(2 * i) = coefficient_mat.row(0);
+    A.row(2 * i + 1) = coefficient_mat.row(1);
+    Point2 p = coefficient_mat * wTi.translation();
+    b(2 * i) = p.x();
+    b(2 * i + 1) = p.y();
+  }
+  // Solve Ax = b, using QR decomposition
+  return A.colPivHouseholderQr().solve(b);
+}
+
 Vector4 triangulateHomogeneousDLT(
     const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
     const std::vector<Unit3>& measurements, double rank_tol) {

gtsam/geometry/triangulation.h

@@ -62,6 +62,18 @@ GTSAM_EXPORT Vector4 triangulateHomogeneousDLT(
     const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
     const Point2Vector& measurements, double rank_tol = 1e-9);
 
+/**
+ * @brief 
+ * 
+ * @param projection_matrices 
+ * @param measurements 
+ * @param rank_tol 
+ * @return GTSAM_EXPORT 
+ */
+GTSAM_EXPORT Vector3
+triangulateLOSTHomogeneous(const std::vector<Pose3>& poses,
+                           const std::vector<Point3>& calibrated_measurements);
+
 /**
  * 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)
@@ -382,6 +394,39 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
   return point;
 }
 
+template <class CALIBRATION>
+Point3 triangulateLOSTPoint3(const std::vector<PinholeCamera<CALIBRATION>>& cameras,
+                             const std::vector<Point2>& measurements) {
+  const size_t num_cameras = cameras.size();
+  assert(measurements.size() == num_cameras);
+
+  if (num_cameras < 2) throw(TriangulationUnderconstrainedException());
+
+  // Convert measurements to image plane coordinates.
+  std::vector<Point3> calibrated_measurements;
+  calibrated_measurements.reserve(measurements.size());
+  for (int i = 0; i < measurements.size(); ++i) {
+    Point2 p = cameras[i].calibration().calibrate(measurements[i]);
+    calibrated_measurements.emplace_back(p.x(), p.y(), 1.0);
+  }
+
+  std::vector<Pose3> poses;
+  poses.reserve(cameras.size());
+  for (const auto& camera : cameras) poses.push_back(camera.pose());
+
+  Point3 point = triangulateLOSTHomogeneous(poses, calibrated_measurements);
+
+#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
+  // verify that the triangulated point lies in front of all cameras
+  for (const auto& camera : cameras) {
+    const Point3& p_local = camera.pose().transformTo(point);
+    if (p_local.z() <= 0) throw(TriangulationCheiralityException());
+  }
+#endif
+
+  return point;
+}
+
 /**
  * Function to triangulate 3D landmark point from an arbitrary number
  * of poses (at least 2) using the DLT. This function is similar to the one