From 5ea8f2529fc91b82a414a770c027eac14fc3a205 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Mon, 20 Jun 2022 23:20:29 -0700 Subject: [PATCH] make noise input, add LOST vs DLT unit test --- gtsam/geometry/tests/testTriangulation.cpp | 39 +++++++++++++++++++++- gtsam/geometry/triangulation.cpp | 19 +++++------ gtsam/geometry/triangulation.h | 11 +++--- 3 files changed, 54 insertions(+), 15 deletions(-) diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index 838a5e07f..0b885d434 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/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(cameras, measurements); + triangulateLOSTPoint3(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 camera_1(pose_1, identity_K); + PinholeCamera 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( + {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 cal = boost::make_shared(identity_K); + boost::optional estimate_dlt = + triangulatePoint3(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) { diff --git a/gtsam/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp index a02da2eff..bec239240 100644 --- a/gtsam/geometry/triangulation.cpp +++ b/gtsam/geometry/triangulation.cpp @@ -60,11 +60,8 @@ Vector4 triangulateHomogeneousDLT( Vector3 triangulateLOSTHomogeneous( const std::vector& poses, - const std::vector& calibrated_measurements) { - - // TODO(akshay-krishnan): make this an argument. - const double sigma_x = 1e-3; - + const std::vector& calibrated_measurements, + const double measurement_sigma) { size_t m = calibrated_measurements.size(); assert(m == poses.size()); @@ -78,16 +75,18 @@ Vector3 triangulateLOSTHomogeneous( const int j = (i + 1) % m; const Pose3& wTj = poses[j]; - Point3 d_ij = wTj.translation() - wTi.translation(); + 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 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(); + double q_i = numerator / (measurement_sigma * 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); diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 3b834c615..d31630e67 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -72,7 +72,8 @@ GTSAM_EXPORT Vector4 triangulateHomogeneousDLT( */ GTSAM_EXPORT Vector3 triangulateLOSTHomogeneous(const std::vector& poses, - const std::vector& calibrated_measurements); + const std::vector& 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& poses, } template -Point3 triangulateLOSTPoint3(const std::vector>& cameras, - const std::vector& measurements) { +Point3 triangulateLOSTPoint3( + const std::vector>& cameras, + const std::vector& 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>& 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