make noise input, add LOST vs DLT unit test

gtsam/geometry/tests/testTriangulation.cpp

@@ -109,11 +109,48 @@ TEST(triangulation, twoCamerasLOST) {
   // 0.499167, 1.19814)
   measurements[0] += Point2(0.1, 0.5);
   measurements[1] += Point2(-0.2, 0.3);
+  const double measurement_sigma = 1e-3;
   Point3 actual2 =  //
-      triangulateLOSTPoint3<Cal3_S2>(cameras, measurements);
+      triangulateLOSTPoint3<Cal3_S2>(cameras, measurements, measurement_sigma);
   EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), actual2, 1e-4));
 }
 
+TEST(triangulation, twoCamerasLOSTvsDLT) {
+  // LOST has been shown to preform better when the point is much closer to one
+  // camera compared to another. This unit test checks this configuration.
+  Cal3_S2 identity_K;
+  Pose3 pose_1;
+  Pose3 pose_2(Rot3(), Point3(5., 0., -5.));
+  PinholeCamera<Cal3_S2> camera_1(pose_1, identity_K);
+  PinholeCamera<Cal3_S2> camera_2(pose_2, identity_K);
+
+  Point3 gt_point(0, 0, 1);
+  Point2 x1 = camera_1.project(gt_point);
+  Point2 x2 = camera_2.project(gt_point);
+
+  Point2 x1_noisy = x1 + Point2(0.00817, 0.00977);
+  Point2 x2_noisy = x2 + Point2(-0.00610, 0.01969);
+
+  const double measurement_sigma = 1e-2;
+  Point3 estimate_lost = triangulateLOSTPoint3<Cal3_S2>(
+      {camera_1, camera_2}, {x1_noisy, x2_noisy}, measurement_sigma);
+
+  // These values are from a MATLAB implementation.
+  EXPECT(assert_equal(Point3(0.007, 0.011, 0.945), estimate_lost, 1e-3));
+  
+  double rank_tol = 1e-9;
+
+  Pose3Vector poses = {pose_1, pose_2};
+  Point2Vector points = {x1_noisy, x2_noisy};
+  boost::shared_ptr<Cal3_S2> cal = boost::make_shared<Cal3_S2>(identity_K);
+  boost::optional<Point3> estimate_dlt =
+      triangulatePoint3<Cal3_S2>(poses, cal, points, rank_tol, false);
+
+  // The LOST estimate should have a smaller error.
+  EXPECT((gt_point - estimate_lost).norm() <=
+         (gt_point - *estimate_dlt).norm());
+}
+
 //******************************************************************************
 // Simple test with a well-behaved two camera situation with Cal3DS2 calibration.
 TEST(triangulation, twoPosesCal3DS2) {

gtsam/geometry/triangulation.cpp

@@ -60,11 +60,8 @@ Vector4 triangulateHomogeneousDLT(
 
 Vector3 triangulateLOSTHomogeneous(
     const std::vector<Pose3>& poses,
-    const std::vector<Point3>& calibrated_measurements) {
+    const std::vector<Point3>& calibrated_measurements, 
-
+    const double measurement_sigma) {
-  // TODO(akshay-krishnan): make this an argument.
-  const double sigma_x = 1e-3;
-
   size_t m = calibrated_measurements.size();
   assert(m == poses.size());
 
@@ -86,8 +83,10 @@ Vector3 triangulateLOSTHomogeneous(
     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);
+    double q_i = numerator / (measurement_sigma * denominator);
-    Matrix23 coefficient_mat = q_i * skewSymmetric(calibrated_measurements[i]).topLeftCorner(2, 3) * wTi.rotation().matrix().transpose();
+    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);

gtsam/geometry/triangulation.h

@@ -72,7 +72,8 @@ GTSAM_EXPORT Vector4 triangulateHomogeneousDLT(
  */
 GTSAM_EXPORT Vector3
 triangulateLOSTHomogeneous(const std::vector<Pose3>& poses,
-                           const std::vector<Point3>& calibrated_measurements);
+                           const std::vector<Point3>& calibrated_measurements,
+                           const double measurement_sigma);
 
 /**
  * Same math as Hartley and Zisserman, 2nd Ed., page 312, but with unit-norm bearing vectors
@@ -395,8 +396,10 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
 }
 
 template <class CALIBRATION>
-Point3 triangulateLOSTPoint3(const std::vector<PinholeCamera<CALIBRATION>>& cameras,
+Point3 triangulateLOSTPoint3(
-                             const std::vector<Point2>& measurements) {
+    const std::vector<PinholeCamera<CALIBRATION>>& cameras,
+    const std::vector<Point2>& measurements,
+    const double measurement_sigma = 1e-2) {
   const size_t num_cameras = cameras.size();
   assert(measurements.size() == num_cameras);
 
@@ -414,7 +417,7 @@ Point3 triangulateLOSTPoint3(const std::vector<PinholeCamera<CALIBRATION>>& came
   poses.reserve(cameras.size());
   for (const auto& camera : cameras) poses.push_back(camera.pose());
 
-  Point3 point = triangulateLOSTHomogeneous(poses, calibrated_measurements);
+  Point3 point = triangulateLOSTHomogeneous(poses, calibrated_measurements, measurement_sigma);
 
 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
   // verify that the triangulated point lies in front of all cameras