From c49ad326d1f34327e9d751b5e2fdbacd4f71ba38 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Fri, 17 Jun 2022 10:57:51 -0700 Subject: [PATCH 01/12] initial LOST implementation tested --- gtsam/geometry/tests/testTriangulation.cpp | 18 +++++++++ gtsam/geometry/triangulation.cpp | 46 ++++++++++++++++++++++ gtsam/geometry/triangulation.h | 45 +++++++++++++++++++++ 3 files changed, 109 insertions(+) diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index fb66fb6a2..838a5e07f 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/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> cameras = {camera1, camera2}; + std::vector measurements = {z1, z2}; + + // 1. Test simple triangulation, perfect in no noise situation + Point3 actual1 = // + triangulateLOSTPoint3(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(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) { diff --git a/gtsam/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp index 026afef24..a02da2eff 100644 --- a/gtsam/geometry/triangulation.cpp +++ b/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& poses, + const std::vector& 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>& projection_matrices, const std::vector& measurements, double rank_tol) { diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 401fd2d0b..3b834c615 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -62,6 +62,18 @@ GTSAM_EXPORT Vector4 triangulateHomogeneousDLT( const std::vector>& 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& poses, + const std::vector& 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& poses, return point; } +template +Point3 triangulateLOSTPoint3(const std::vector>& cameras, + const std::vector& 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 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 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 From 5ea8f2529fc91b82a414a770c027eac14fc3a205 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Mon, 20 Jun 2022 23:20:29 -0700 Subject: [PATCH 02/12] 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 From ca4b450e7ee98a2d6344df2120148c958e6ab410 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Tue, 28 Jun 2022 00:59:20 -0700 Subject: [PATCH 03/12] experimenting with LS and TLS --- gtsam/geometry/triangulation.cpp | 66 +++++++++++++++++++++++++------- gtsam/geometry/triangulation.h | 17 ++++++-- 2 files changed, 65 insertions(+), 18 deletions(-) diff --git a/gtsam/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp index bec239240..9d8325d68 100644 --- a/gtsam/geometry/triangulation.cpp +++ b/gtsam/geometry/triangulation.cpp @@ -65,6 +65,50 @@ Vector3 triangulateLOSTHomogeneous( size_t m = calibrated_measurements.size(); assert(m == poses.size()); + // Construct the system matrices. + Matrix A = Matrix::Zero(m * 2, 4); + + 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]; + + const Point3 d_ij = wTj.translation() - wTi.translation(); + + const Point3 w_measurement_i = + wTi.rotation().rotate(calibrated_measurements[i]); + const Point3 w_measurement_j = + wTj.rotation().rotate(calibrated_measurements[j]); + + const double q_i = w_measurement_i.cross(w_measurement_j).norm() / + (measurement_sigma * d_ij.cross(w_measurement_j).norm()); + const Matrix23 coefficient_mat = + q_i * skewSymmetric(calibrated_measurements[i]).topLeftCorner(2, 3) * + wTi.rotation().matrix().transpose(); + + A.block<2, 4>(2 * i, 0) << coefficient_mat, -coefficient_mat * wTi.translation(); + } + + const double rank_tol = 1e-6; + int rank; + double error; + Vector v; + boost::tie(rank, error, v) = DLT(A, rank_tol); + + if (rank < 3) + throw(TriangulationUnderconstrainedException()); + + return Point3(v.head<3>() / v[3]); +} + +Vector3 triangulateLOSTHomogeneousLS( + const std::vector& poses, + const std::vector& calibrated_measurements, + const double measurement_sigma) { + 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); @@ -75,26 +119,20 @@ Vector3 triangulateLOSTHomogeneous( const int j = (i + 1) % m; const Pose3& wTj = poses[j]; - Point3 d_ij = wTj.translation() - wTi.translation(); + const 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]); + const Point3 w_measurement_i = wTi.rotation().rotate(calibrated_measurements[i]); + const 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 / (measurement_sigma * denominator); - Matrix23 coefficient_mat = + const double q_i = w_measurement_i.cross(w_measurement_j).norm() / + (measurement_sigma * d_ij.cross(w_measurement_j).norm()); + const 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(); + A.block<2, 3>(2*i, 0) = coefficient_mat; + b.block<2, 1>(2*i, 0) = coefficient_mat * wTi.translation(); } - // Solve Ax = b, using QR decomposition return A.colPivHouseholderQr().solve(b); } diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index d31630e67..5bea4d93c 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -75,6 +75,11 @@ triangulateLOSTHomogeneous(const std::vector& poses, const std::vector& calibrated_measurements, const double measurement_sigma); +GTSAM_EXPORT Vector3 +triangulateLOSTHomogeneousLS(const std::vector& poses, + 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 * (contrarily to pinhole projection, the z entry is not assumed to be 1 as in Hartley and Zisserman) @@ -397,9 +402,9 @@ Point3 triangulatePoint3(const std::vector& poses, template Point3 triangulateLOSTPoint3( - const std::vector>& cameras, - const std::vector& measurements, - const double measurement_sigma = 1e-2) { + const CameraSet>& cameras, + const Point2Vector& measurements, + const double measurement_sigma = 1e-2, bool use_dlt = false) { const size_t num_cameras = cameras.size(); assert(measurements.size() == num_cameras); @@ -417,7 +422,11 @@ Point3 triangulateLOSTPoint3( poses.reserve(cameras.size()); for (const auto& camera : cameras) poses.push_back(camera.pose()); - Point3 point = triangulateLOSTHomogeneous(poses, calibrated_measurements, measurement_sigma); + Point3 point = use_dlt + ? triangulateLOSTHomogeneous( + poses, calibrated_measurements, measurement_sigma) + : triangulateLOSTHomogeneousLS( + poses, calibrated_measurements, measurement_sigma); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION // verify that the triangulated point lies in front of all cameras From 63e0446f0b04fe23e1224b19e9f819c0f9c9a249 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Sun, 10 Jul 2022 16:49:15 -0700 Subject: [PATCH 04/12] update triangulation LOST API --- gtsam/geometry/Point3.h | 1 + gtsam/geometry/tests/testTriangulation.cpp | 63 +++--- gtsam/geometry/triangulation.cpp | 159 +++++++-------- gtsam/geometry/triangulation.h | 218 +++++++++++++-------- 4 files changed, 247 insertions(+), 194 deletions(-) diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 835c53d7c..b1ac6195b 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -33,6 +33,7 @@ namespace gtsam { /// As of GTSAM 4, in order to make GTSAM more lean, /// it is now possible to just typedef Point3 to Vector3 typedef Vector3 Point3; +typedef std::vector > Point3Vector; // Convenience typedef using Point3Pair = std::pair; diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index 0b885d434..731558327 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -96,23 +96,33 @@ TEST(triangulation, twoPoses) { EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-4)); } -TEST(triangulation, twoCamerasLOST) { - std::vector> cameras = {camera1, camera2}; - std::vector measurements = {z1, z2}; +TEST(triangulation, twoCamerasUsingLOST) { + CameraSet> cameras; + cameras.push_back(camera1); + cameras.push_back(camera2); + + Point2Vector measurements = {z1, z2}; + SharedNoiseModel measurementNoise = noiseModel::Isotropic::Sigma(2, 1e-4); + double rank_tol = 1e-9; // 1. Test simple triangulation, perfect in no noise situation - Point3 actual1 = // - triangulateLOSTPoint3(cameras, measurements); - EXPECT(assert_equal(landmark, actual1, 1e-12)); + boost::optional actual1 = // + triangulatePoint3>( + cameras, measurements, rank_tol, + /*optimize=*/false, measurementNoise, + /*use_lost_triangulation=*/true); + 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); - const double measurement_sigma = 1e-3; - Point3 actual2 = // - triangulateLOSTPoint3(cameras, measurements, measurement_sigma); - EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), actual2, 1e-4)); + boost::optional actual2 = // + triangulatePoint3>( + cameras, measurements, rank_tol, + /*optimize=*/false, measurementNoise, + /*use_lost_triangulation=*/true); + EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual2, 1e-4)); } TEST(triangulation, twoCamerasLOSTvsDLT) { @@ -121,33 +131,32 @@ TEST(triangulation, twoCamerasLOSTvsDLT) { 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); + CameraSet> cameras; + cameras.emplace_back(pose_1, identity_K); + cameras.emplace_back(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 = cameras[0].project(gt_point) + Point2(0.00817, 0.00977); + Point2 x2_noisy = cameras[1].project(gt_point) + Point2(-0.00610, 0.01969); + Point2Vector measurements = {x1_noisy, x2_noisy}; - Point2 x1_noisy = x1 + Point2(0.00817, 0.00977); - Point2 x2_noisy = x2 + Point2(-0.00610, 0.01969); + double rank_tol = 1e-9; + SharedNoiseModel measurementNoise = noiseModel::Isotropic::Sigma(2, 1e-2); - const double measurement_sigma = 1e-2; - Point3 estimate_lost = triangulateLOSTPoint3( - {camera_1, camera_2}, {x1_noisy, x2_noisy}, measurement_sigma); + boost::optional estimate_lost = // + triangulatePoint3>( + cameras, measurements, rank_tol, + /*optimize=*/false, measurementNoise, + /*use_lost_triangulation=*/true); // 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; + EXPECT(assert_equal(Point3(0.007, 0.011, 0.945), *estimate_lost, 1e-3)); - 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); + triangulatePoint3(cameras, measurements, rank_tol, false); // The LOST estimate should have a smaller error. - EXPECT((gt_point - estimate_lost).norm() <= + EXPECT((gt_point - *estimate_lost).norm() <= (gt_point - *estimate_dlt).norm()); } diff --git a/gtsam/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp index 9d8325d68..c2c798d8b 100644 --- a/gtsam/geometry/triangulation.cpp +++ b/gtsam/geometry/triangulation.cpp @@ -24,9 +24,9 @@ namespace gtsam { Vector4 triangulateHomogeneousDLT( - const std::vector>& projection_matrices, + const std::vector>& + projection_matrices, const Point2Vector& measurements, double rank_tol) { - // number of cameras size_t m = projection_matrices.size(); @@ -52,61 +52,75 @@ Vector4 triangulateHomogeneousDLT( Vector v; boost::tie(rank, error, v) = DLT(A, rank_tol); - if (rank < 3) - throw(TriangulationUnderconstrainedException()); + if (rank < 3) throw(TriangulationUnderconstrainedException()); return v; } -Vector3 triangulateLOSTHomogeneous( - const std::vector& poses, - const std::vector& calibrated_measurements, - const double measurement_sigma) { - size_t m = calibrated_measurements.size(); - assert(m == poses.size()); +Vector4 triangulateHomogeneousDLT( + const std::vector>& + projection_matrices, + const std::vector& measurements, double rank_tol) { + // number of cameras + size_t m = projection_matrices.size(); - // Construct the system matrices. + // Allocate DLT matrix Matrix A = Matrix::Zero(m * 2, 4); 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]; + 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 - const Point3 d_ij = wTj.translation() - wTi.translation(); - - const Point3 w_measurement_i = - wTi.rotation().rotate(calibrated_measurements[i]); - const Point3 w_measurement_j = - wTj.rotation().rotate(calibrated_measurements[j]); - - const double q_i = w_measurement_i.cross(w_measurement_j).norm() / - (measurement_sigma * d_ij.cross(w_measurement_j).norm()); - const Matrix23 coefficient_mat = - q_i * skewSymmetric(calibrated_measurements[i]).topLeftCorner(2, 3) * - wTi.rotation().matrix().transpose(); - - A.block<2, 4>(2 * i, 0) << coefficient_mat, -coefficient_mat * wTi.translation(); + // 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); } - - const double rank_tol = 1e-6; int rank; double error; Vector v; boost::tie(rank, error, v) = DLT(A, rank_tol); - if (rank < 3) - throw(TriangulationUnderconstrainedException()); + if (rank < 3) throw(TriangulationUnderconstrainedException()); + + return v; +} + +Point3 triangulateDLT(const std::vector& poses, + const Point3Vector& homogenousMeasurements, + double rank_tol) { + // number of cameras + size_t m = poses.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 = poses[i].matrix().block(0, 0, 3, 4); + const Point3& p = homogenousMeasurements[i]; // 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 Point3(v.head<3>() / v[3]); } -Vector3 triangulateLOSTHomogeneousLS( - const std::vector& poses, - const std::vector& calibrated_measurements, - const double measurement_sigma) { - size_t m = calibrated_measurements.size(); +Point3 triangulateLOST(const std::vector& poses, + const Point3Vector& calibratedMeasurements, + const SharedIsotropic& measurementNoise) { + size_t m = calibratedMeasurements.size(); assert(m == poses.size()); // Construct the system matrices. @@ -121,70 +135,43 @@ Vector3 triangulateLOSTHomogeneousLS( const Point3 d_ij = wTj.translation() - wTi.translation(); - const Point3 w_measurement_i = wTi.rotation().rotate(calibrated_measurements[i]); - const Point3 w_measurement_j = wTj.rotation().rotate(calibrated_measurements[j]); + const Point3 w_measurement_i = + wTi.rotation().rotate(calibratedMeasurements[i]); + const Point3 w_measurement_j = + wTj.rotation().rotate(calibratedMeasurements[j]); - const double q_i = w_measurement_i.cross(w_measurement_j).norm() / - (measurement_sigma * d_ij.cross(w_measurement_j).norm()); + double q_i = + w_measurement_i.cross(w_measurement_j).norm() / + (measurementNoise->sigma() * d_ij.cross(w_measurement_j).norm()); const Matrix23 coefficient_mat = - q_i * skewSymmetric(calibrated_measurements[i]).topLeftCorner(2, 3) * + q_i * skewSymmetric(calibratedMeasurements[i]).topLeftCorner(2, 3) * wTi.rotation().matrix().transpose(); - A.block<2, 3>(2*i, 0) = coefficient_mat; - b.block<2, 1>(2*i, 0) = coefficient_mat * wTi.translation(); + A.block<2, 3>(2 * i, 0) << coefficient_mat; + b.block<2, 1>(2 * i, 0) << coefficient_mat * wTi.translation(); } return A.colPivHouseholderQr().solve(b); } -Vector4 triangulateHomogeneousDLT( - const std::vector>& projection_matrices, - const std::vector& 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>& projection_matrices, + const std::vector>& + projection_matrices, const Point2Vector& measurements, double rank_tol) { - - Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements, - 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>& projection_matrices, + const std::vector>& + projection_matrices, const std::vector& measurements, double rank_tol) { - // contrary to previous triangulateDLT, this is now taking Unit3 inputs - Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements, - rank_tol); - // Create 3D point from homogeneous coordinates - return Point3(v.head<3>() / v[3]); + Vector4 v = + triangulateHomogeneousDLT(projection_matrices, measurements, rank_tol); + // Create 3D point from homogeneous coordinates + return Point3(v.head<3>() / v[3]); } /// @@ -215,4 +202,4 @@ Point3 optimize(const NonlinearFactorGraph& graph, const Values& values, return result.at(landmarkKey); } -} // \namespace gtsam +} // namespace gtsam diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 5bea4d93c..9e59a92cb 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -62,24 +62,6 @@ GTSAM_EXPORT Vector4 triangulateHomogeneousDLT( const std::vector>& 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& poses, - const std::vector& calibrated_measurements, - const double measurement_sigma); - -GTSAM_EXPORT Vector3 -triangulateLOSTHomogeneousLS(const std::vector& poses, - 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 * (contrarily to pinhole projection, the z entry is not assumed to be 1 as in Hartley and Zisserman) @@ -112,6 +94,20 @@ GTSAM_EXPORT Point3 triangulateDLT( const std::vector& measurements, double rank_tol = 1e-9); +/** + * @brief Triangulation using the LOST (Linear Optimal Sine Triangulation) + * algorithm proposed in https://arxiv.org/pdf/2205.12197.pdf by Sebastien Henry + * and John Christian. + * @param poses camera poses in world frame + * @param calibratedMeasurements measurements in homogeneous coordinates in each + * camera pose + * @param measurementNoise isotropic noise model for the measurements + * @return triangulated point in world coordinates + */ +GTSAM_EXPORT Point3 triangulateLOST(const std::vector& poses, + const Point3Vector& calibratedMeasurements, + const SharedIsotropic& measurementNoise); + /** * Create a factor graph with projection factors from poses and one calibration * @param poses Camera poses @@ -320,8 +316,8 @@ template typename CAMERA::MeasurementVector undistortMeasurements( const CameraSet& cameras, const typename CAMERA::MeasurementVector& measurements) { - const size_t num_meas = cameras.size(); - assert(num_meas == measurements.size()); + const size_t num_meas = measurements.size(); + assert(num_meas == cameras.size()); typename CAMERA::MeasurementVector undistortedMeasurements(num_meas); for (size_t ii = 0; ii < num_meas; ++ii) { // Calibrate with cal and uncalibrate with pinhole version of cal so that @@ -349,6 +345,65 @@ inline SphericalCamera::MeasurementVector undistortMeasurements( return measurements; } +/** Convert pixel measurements in image to homogeneous measurements in the image + * plane using shared camera intrinsics. + * + * @tparam CALIBRATION Calibration type to use. + * @param cal Calibration with which measurements were taken. + * @param measurements Vector of measurements to undistort. + * @return homogeneous measurements in image plane + */ +template +inline Point3Vector calibrateMeasurementsShared( + const CALIBRATION& cal, const Point2Vector& measurements) { + Point3Vector calibratedMeasurements; + // Calibrate with cal and uncalibrate with pinhole version of cal so that + // measurements are undistorted. + std::transform(measurements.begin(), measurements.end(), + std::back_inserter(calibratedMeasurements), + [&cal](const Point2& measurement) { + Point3 p; + p << cal.calibrate(measurement), 1.0; + return p; + }); + return calibratedMeasurements; +} + +/** Convert pixel measurements in image to homogeneous measurements in the image + * plane using camera intrinsics of each measurement. + * + * @tparam CAMERA Camera type to use. + * @param cameras Cameras corresponding to each measurement. + * @param measurements Vector of measurements to undistort. + * @return homogeneous measurements in image plane + */ +template +inline Point3Vector calibrateMeasurements( + const CameraSet& cameras, + const typename CAMERA::MeasurementVector& measurements) { + const size_t num_meas = measurements.size(); + assert(num_meas == cameras.size()); + Point3Vector calibratedMeasurements(num_meas); + for (size_t ii = 0; ii < num_meas; ++ii) { + calibratedMeasurements[ii] + << cameras[ii].calibration().calibrate(measurements[ii]), + 1.0; + } + return calibratedMeasurements; +} + +/** Specialize for SphericalCamera to do nothing. */ +template +inline Point3Vector calibrateMeasurements( + const CameraSet& cameras, + const SphericalCamera::MeasurementVector& measurements) { + Point3Vector calibratedMeasurements(measurements.size()); + for (size_t ii = 0; ii < measurements.size(); ++ii) { + calibratedMeasurements[ii] << measurements[ii].point3(); + } + return calibratedMeasurements; +} + /** * Function to triangulate 3D landmark point from an arbitrary number * of poses (at least 2) using the DLT. The function checks that the @@ -359,6 +414,7 @@ inline SphericalCamera::MeasurementVector undistortMeasurements( * @param measurements A vector of camera measurements * @param rank_tol rank tolerance, default 1e-9 * @param optimize Flag to turn on nonlinear refinement of triangulation + * @param use_lost_triangulation whether to use the LOST algorithm instead of DLT * @return Returns a Point3 */ template @@ -366,22 +422,36 @@ Point3 triangulatePoint3(const std::vector& poses, boost::shared_ptr sharedCal, const Point2Vector& measurements, double rank_tol = 1e-9, bool optimize = false, - const SharedNoiseModel& model = nullptr) { + const SharedNoiseModel& model = nullptr, + const bool use_lost_triangulation = true) { assert(poses.size() == measurements.size()); if (poses.size() < 2) throw(TriangulationUnderconstrainedException()); - // construct projection matrices from poses & calibration - auto projection_matrices = projectionMatricesFromPoses(poses, sharedCal); - - // Undistort the measurements, leaving only the pinhole elements in effect. - auto undistortedMeasurements = - undistortMeasurements(*sharedCal, measurements); - // Triangulate linearly - Point3 point = - triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol); + Point3 point; + if(use_lost_triangulation) { + // Reduce input noise model to an isotropic noise model using the mean of + // the diagonal. + const double measurementSigma = model ? model->sigmas().mean() : 1e-4; + SharedIsotropic measurementNoise = + noiseModel::Isotropic::Sigma(2, measurementSigma); + // calibrate the measurements to obtain homogenous coordinates in image plane. + auto calibratedMeasurements = + calibrateMeasurementsShared(*sharedCal, measurements); + + point = triangulateLOST(poses, calibratedMeasurements, measurementNoise); + } else { + // construct projection matrices from poses & calibration + auto projection_matrices = projectionMatricesFromPoses(poses, sharedCal); + + // Undistort the measurements, leaving only the pinhole elements in effect. + auto undistortedMeasurements = + undistortMeasurements(*sharedCal, measurements); + + point = triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol); + } // Then refine using non-linear optimization if (optimize) @@ -400,45 +470,6 @@ Point3 triangulatePoint3(const std::vector& poses, return point; } -template -Point3 triangulateLOSTPoint3( - const CameraSet>& cameras, - const Point2Vector& measurements, - const double measurement_sigma = 1e-2, bool use_dlt = false) { - 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 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 poses; - poses.reserve(cameras.size()); - for (const auto& camera : cameras) poses.push_back(camera.pose()); - - Point3 point = use_dlt - ? triangulateLOSTHomogeneous( - poses, calibrated_measurements, measurement_sigma) - : triangulateLOSTHomogeneousLS( - poses, calibrated_measurements, measurement_sigma); - -#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 @@ -449,6 +480,7 @@ Point3 triangulateLOSTPoint3( * @param measurements A vector of camera measurements * @param rank_tol rank tolerance, default 1e-9 * @param optimize Flag to turn on nonlinear refinement of triangulation + * @param use_lost_triangulation whether to use the LOST algorithm instead of DLT * @return Returns a Point3 */ template @@ -456,7 +488,8 @@ Point3 triangulatePoint3( const CameraSet& cameras, const typename CAMERA::MeasurementVector& measurements, double rank_tol = 1e-9, bool optimize = false, - const SharedNoiseModel& model = nullptr) { + const SharedNoiseModel& model = nullptr, + const bool use_lost_triangulation=false) { size_t m = cameras.size(); assert(measurements.size() == m); @@ -464,19 +497,41 @@ Point3 triangulatePoint3( if (m < 2) throw(TriangulationUnderconstrainedException()); - // construct projection matrices from poses & calibration - auto projection_matrices = projectionMatricesFromCameras(cameras); + // Triangulate linearly + Point3 point; + if(use_lost_triangulation) { + // Reduce input noise model to an isotropic noise model using the mean of + // the diagonal. + const double measurementSigma = model ? model->sigmas().mean() : 1e-4; + SharedIsotropic measurementNoise = + noiseModel::Isotropic::Sigma(2, measurementSigma); - // Undistort the measurements, leaving only the pinhole elements in effect. - auto undistortedMeasurements = - undistortMeasurements(cameras, measurements); + // construct poses from cameras. + std::vector poses; + poses.reserve(cameras.size()); + for (const auto& camera : cameras) poses.push_back(camera.pose()); - Point3 point = - triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol); + // calibrate the measurements to obtain homogenous coordinates in image + // plane. + auto calibratedMeasurements = + calibrateMeasurements(cameras, measurements); - // The n refine using non-linear optimization - if (optimize) + point = triangulateLOST(poses, calibratedMeasurements, measurementNoise); + } else { + // construct projection matrices from poses & calibration + auto projection_matrices = projectionMatricesFromCameras(cameras); + + // Undistort the measurements, leaving only the pinhole elements in effect. + auto undistortedMeasurements = + undistortMeasurements(cameras, measurements); + + point = triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol); + } + + // Then refine using non-linear optimization + if (optimize) { point = triangulateNonlinear(cameras, measurements, point, model); + } #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION // verify that the triangulated point lies in front of all cameras @@ -496,9 +551,10 @@ Point3 triangulatePoint3( const CameraSet >& cameras, const Point2Vector& measurements, double rank_tol = 1e-9, bool optimize = false, - const SharedNoiseModel& model = nullptr) { + const SharedNoiseModel& model = nullptr, + const bool use_lost_triangulation = false) { return triangulatePoint3 > // - (cameras, measurements, rank_tol, optimize, model); + (cameras, measurements, rank_tol, optimize, model, use_lost_triangulation); } struct GTSAM_EXPORT TriangulationParameters { From ed5fa923cfe04ddfed1b9e74da9d268ca8009b41 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Sun, 10 Jul 2022 16:49:30 -0700 Subject: [PATCH 05/12] Example to run LOST and compare to DLT --- examples/TriangulationLOSTExample.cpp | 161 ++++++++++++++++++++++++++ 1 file changed, 161 insertions(+) create mode 100644 examples/TriangulationLOSTExample.cpp diff --git a/examples/TriangulationLOSTExample.cpp b/examples/TriangulationLOSTExample.cpp new file mode 100644 index 000000000..a903c625a --- /dev/null +++ b/examples/TriangulationLOSTExample.cpp @@ -0,0 +1,161 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file TriangulationLOSTExample.cpp + * @author Akshay Krishnan + * @brief This example runs triangulation several times using 3 different + * approaches: LOST, DLT, and DLT with optimization. It reports the covariance + * and the runtime for each approach. + * + * @date 2022-07-10 + */ +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +using namespace std; +using namespace gtsam; + +static std::mt19937 rng(42); + +void PrintCovarianceStats(const Matrix& mat, const std::string& method) { + Matrix centered = mat.rowwise() - mat.colwise().mean(); + Matrix cov = (centered.adjoint() * centered) / double(mat.rows() - 1); + std::cout << method << " covariance: " << std::endl; + std::cout << cov << std::endl; + std::cout << "Trace sqrt: " << sqrt(cov.trace()) << std::endl << std::endl; +} + +void PrintDuration(const std::chrono::nanoseconds dur, double num_samples, + const std::string& method) { + double nanoseconds = dur.count() / num_samples; + std::cout << "Time taken by " << method << ": " << nanoseconds * 1e-3 + << std::endl; +} + +void GetLargeCamerasDataset(CameraSet>* cameras, + std::vector* poses, Point3* point, + Point2Vector* measurements) { + const double min_xy = -10, max_xy = 10; + const double min_z = -20, max_z = 0; + const int num_cameras = 500; + cameras->reserve(num_cameras); + poses->reserve(num_cameras); + measurements->reserve(num_cameras); + *point = Point3(0.0, 0.0, 10.0); + + std::uniform_real_distribution rand_xy(min_xy, max_xy); + std::uniform_real_distribution rand_z(min_z, max_z); + Cal3_S2 identity_K; + + for (int i = 0; i < num_cameras; ++i) { + Point3 wti(rand_xy(rng), rand_xy(rng), rand_z(rng)); + Pose3 wTi(Rot3(), wti); + + poses->push_back(wTi); + cameras->emplace_back(wTi, identity_K); + measurements->push_back(cameras->back().project(*point)); + } +} + +void GetSmallCamerasDataset(CameraSet>* cameras, + std::vector* poses, Point3* point, + Point2Vector* measurements) { + Pose3 pose_1; + Pose3 pose_2(Rot3(), Point3(5., 0., -5.)); + Cal3_S2 identity_K; + PinholeCamera camera_1(pose_1, identity_K); + PinholeCamera camera_2(pose_2, identity_K); + + *point = Point3(0, 0, 1); + cameras->push_back(camera_1); + cameras->push_back(camera_2); + *poses = {pose_1, pose_2}; + *measurements = {camera_1.project(*point), camera_2.project(*point)}; +} + +Point2Vector AddNoiseToMeasurements(const Point2Vector& measurements, + const double measurement_sigma) { + std::normal_distribution normal(0.0, measurement_sigma); + + Point2Vector noisy_measurements; + noisy_measurements.reserve(measurements.size()); + for (const auto& p : measurements) { + noisy_measurements.emplace_back(p.x() + normal(rng), p.y() + normal(rng)); + } + return noisy_measurements; +} + +/* ************************************************************************* */ +int main(int argc, char* argv[]) { + CameraSet> cameras; + std::vector poses; + Point3 gt_point; + Point2Vector measurements; + GetLargeCamerasDataset(&cameras, &poses, >_point, &measurements); + // GetSmallCamerasDataset(&cameras, &poses, >_point, &measurements); + const double measurement_sigma = 1e-2; + SharedNoiseModel measurement_noise = + noiseModel::Isotropic::Sigma(2, measurement_sigma); + + const long int num_trials = 1000; + Matrix dlt_errors = Matrix::Zero(num_trials, 3); + Matrix lost_errors = Matrix::Zero(num_trials, 3); + Matrix dlt_opt_errors = Matrix::Zero(num_trials, 3); + + double rank_tol = 1e-9; + boost::shared_ptr calib = boost::make_shared(); + std::chrono::nanoseconds dlt_duration; + std::chrono::nanoseconds dlt_opt_duration; + std::chrono::nanoseconds lost_duration; + std::chrono::nanoseconds lost_tls_duration; + + for (int i = 0; i < num_trials; i++) { + Point2Vector noisy_measurements = + AddNoiseToMeasurements(measurements, measurement_sigma); + + auto lost_start = std::chrono::high_resolution_clock::now(); + boost::optional estimate_lost = triangulatePoint3( + cameras, noisy_measurements, rank_tol, false, measurement_noise, true); + lost_duration += std::chrono::high_resolution_clock::now() - lost_start; + + auto dlt_start = std::chrono::high_resolution_clock::now(); + boost::optional estimate_dlt = triangulatePoint3( + cameras, noisy_measurements, rank_tol, false, measurement_noise, false); + dlt_duration += std::chrono::high_resolution_clock::now() - dlt_start; + + auto dlt_opt_start = std::chrono::high_resolution_clock::now(); + boost::optional estimate_dlt_opt = triangulatePoint3( + cameras, noisy_measurements, rank_tol, true, measurement_noise, false); + dlt_opt_duration += + std::chrono::high_resolution_clock::now() - dlt_opt_start; + + lost_errors.row(i) = *estimate_lost - gt_point; + dlt_errors.row(i) = *estimate_dlt - gt_point; + dlt_opt_errors.row(i) = *estimate_dlt_opt - gt_point; + } + PrintCovarianceStats(lost_errors, "LOST"); + PrintCovarianceStats(dlt_errors, "DLT"); + PrintCovarianceStats(dlt_opt_errors, "DLT_OPT"); + + PrintDuration(lost_duration, num_trials, "LOST"); + PrintDuration(dlt_duration, num_trials, "DLT"); + PrintDuration(dlt_opt_duration, num_trials, "DLT_OPT"); +} \ No newline at end of file From 7d9cf475794e1176d6a31f9b695bd1ade1e27301 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Sun, 10 Jul 2022 22:00:18 -0700 Subject: [PATCH 06/12] unit tests pass --- gtsam/geometry/triangulation.cpp | 32 +--------------- gtsam/geometry/triangulation.h | 64 ++++++++++++++++---------------- 2 files changed, 32 insertions(+), 64 deletions(-) diff --git a/gtsam/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp index c2c798d8b..c68fffb82 100644 --- a/gtsam/geometry/triangulation.cpp +++ b/gtsam/geometry/triangulation.cpp @@ -88,35 +88,6 @@ Vector4 triangulateHomogeneousDLT( return v; } -Point3 triangulateDLT(const std::vector& poses, - const Point3Vector& homogenousMeasurements, - double rank_tol) { - // number of cameras - size_t m = poses.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 = poses[i].matrix().block(0, 0, 3, 4); - const Point3& p = homogenousMeasurements[i]; // 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 Point3(v.head<3>() / v[3]); -} - Point3 triangulateLOST(const std::vector& poses, const Point3Vector& calibratedMeasurements, const SharedIsotropic& measurementNoise) { @@ -140,7 +111,7 @@ Point3 triangulateLOST(const std::vector& poses, const Point3 w_measurement_j = wTj.rotation().rotate(calibratedMeasurements[j]); - double q_i = + const double q_i = w_measurement_i.cross(w_measurement_j).norm() / (measurementNoise->sigma() * d_ij.cross(w_measurement_j).norm()); const Matrix23 coefficient_mat = @@ -174,7 +145,6 @@ Point3 triangulateDLT( return Point3(v.head<3>() / v[3]); } -/// /** * Optimize for triangulation * @param graph nonlinear factors for projection diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 9e59a92cb..8a709ccd0 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -414,30 +414,30 @@ inline Point3Vector calibrateMeasurements( * @param measurements A vector of camera measurements * @param rank_tol rank tolerance, default 1e-9 * @param optimize Flag to turn on nonlinear refinement of triangulation - * @param use_lost_triangulation whether to use the LOST algorithm instead of DLT + * @param use_lost_triangulation whether to use the LOST algorithm instead of + * DLT * @return Returns a Point3 */ -template +template Point3 triangulatePoint3(const std::vector& poses, - boost::shared_ptr sharedCal, - const Point2Vector& measurements, double rank_tol = 1e-9, - bool optimize = false, - const SharedNoiseModel& model = nullptr, - const bool use_lost_triangulation = true) { - + boost::shared_ptr sharedCal, + const Point2Vector& measurements, + double rank_tol = 1e-9, bool optimize = false, + const SharedNoiseModel& model = nullptr, + const bool use_lost_triangulation = false) { assert(poses.size() == measurements.size()); - if (poses.size() < 2) - throw(TriangulationUnderconstrainedException()); + if (poses.size() < 2) throw(TriangulationUnderconstrainedException()); // Triangulate linearly Point3 point; - if(use_lost_triangulation) { + if (use_lost_triangulation) { // Reduce input noise model to an isotropic noise model using the mean of // the diagonal. const double measurementSigma = model ? model->sigmas().mean() : 1e-4; SharedIsotropic measurementNoise = noiseModel::Isotropic::Sigma(2, measurementSigma); - // calibrate the measurements to obtain homogenous coordinates in image plane. + // calibrate the measurements to obtain homogenous coordinates in image + // plane. auto calibratedMeasurements = calibrateMeasurementsShared(*sharedCal, measurements); @@ -450,20 +450,20 @@ Point3 triangulatePoint3(const std::vector& poses, auto undistortedMeasurements = undistortMeasurements(*sharedCal, measurements); - point = triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol); + point = + triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol); } // Then refine using non-linear optimization if (optimize) - point = triangulateNonlinear // + point = triangulateNonlinear // (poses, sharedCal, measurements, point, model); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION // verify that the triangulated point lies in front of all cameras - for(const Pose3& pose: poses) { + for (const Pose3& pose : poses) { const Point3& p_local = pose.transformTo(point); - if (p_local.z() <= 0) - throw(TriangulationCheiralityException()); + if (p_local.z() <= 0) throw(TriangulationCheiralityException()); } #endif @@ -480,26 +480,24 @@ Point3 triangulatePoint3(const std::vector& poses, * @param measurements A vector of camera measurements * @param rank_tol rank tolerance, default 1e-9 * @param optimize Flag to turn on nonlinear refinement of triangulation - * @param use_lost_triangulation whether to use the LOST algorithm instead of DLT + * @param use_lost_triangulation whether to use the LOST algorithm instead of + * DLT * @return Returns a Point3 */ -template -Point3 triangulatePoint3( - const CameraSet& cameras, - const typename CAMERA::MeasurementVector& measurements, double rank_tol = 1e-9, - bool optimize = false, - const SharedNoiseModel& model = nullptr, - const bool use_lost_triangulation=false) { - +template +Point3 triangulatePoint3(const CameraSet& cameras, + const typename CAMERA::MeasurementVector& measurements, + double rank_tol = 1e-9, bool optimize = false, + const SharedNoiseModel& model = nullptr, + const bool use_lost_triangulation = false) { size_t m = cameras.size(); assert(measurements.size() == m); - if (m < 2) - throw(TriangulationUnderconstrainedException()); + if (m < 2) throw(TriangulationUnderconstrainedException()); // Triangulate linearly Point3 point; - if(use_lost_triangulation) { + if (use_lost_triangulation) { // Reduce input noise model to an isotropic noise model using the mean of // the diagonal. const double measurementSigma = model ? model->sigmas().mean() : 1e-4; @@ -525,7 +523,8 @@ Point3 triangulatePoint3( auto undistortedMeasurements = undistortMeasurements(cameras, measurements); - point = triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol); + point = + triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol); } // Then refine using non-linear optimization @@ -535,10 +534,9 @@ Point3 triangulatePoint3( #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION // verify that the triangulated point lies in front of all cameras - for(const CAMERA& camera: cameras) { + for (const CAMERA& camera : cameras) { const Point3& p_local = camera.pose().transformTo(point); - if (p_local.z() <= 0) - throw(TriangulationCheiralityException()); + if (p_local.z() <= 0) throw(TriangulationCheiralityException()); } #endif From 223ea468d6109273ccadbd2ec84ee0c0ed75e294 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Tue, 12 Jul 2022 18:45:46 -0700 Subject: [PATCH 07/12] change global variable names in test --- gtsam/geometry/tests/testTriangulation.cpp | 240 ++++++++++----------- 1 file changed, 120 insertions(+), 120 deletions(-) diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index 731558327..f6408fab9 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -38,24 +38,24 @@ using namespace boost::assign; // Some common constants -static const boost::shared_ptr sharedCal = // +static const boost::shared_ptr kSharedCal = // boost::make_shared(1500, 1200, 0.1, 640, 480); // Looking along X-axis, 1 meter above ground plane (x-y) static const Rot3 upright = Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2); -static const Pose3 pose1 = Pose3(upright, gtsam::Point3(0, 0, 1)); -PinholeCamera camera1(pose1, *sharedCal); +static const Pose3 kPose1 = Pose3(upright, gtsam::Point3(0, 0, 1)); +static const PinholeCamera kCamera1(kPose1, *kSharedCal); // create second camera 1 meter to the right of first camera -static const Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); -PinholeCamera camera2(pose2, *sharedCal); +static const Pose3 kPose2 = kPose1 * Pose3(Rot3(), Point3(1, 0, 0)); +static const PinholeCamera kCamera2(kPose2, *kSharedCal); // landmark ~5 meters infront of camera -static const Point3 landmark(5, 0.5, 1.2); +static const Point3 kLandmark(5, 0.5, 1.2); // 1. Project two landmarks into two cameras and triangulate -Point2 z1 = camera1.project(landmark); -Point2 z2 = camera2.project(landmark); +static const Point2 kZ1 = kCamera1.project(kLandmark); +static const Point2 kZ2 = kCamera2.project(kLandmark); //****************************************************************************** // Simple test with a well-behaved two camera situation @@ -63,22 +63,22 @@ TEST(triangulation, twoPoses) { vector poses; Point2Vector measurements; - poses += pose1, pose2; - measurements += z1, z2; + poses += kPose1, kPose2; + measurements += kZ1, kZ2; double rank_tol = 1e-9; // 1. Test simple DLT, perfect in no noise situation bool optimize = false; boost::optional actual1 = // - triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize); - EXPECT(assert_equal(landmark, *actual1, 1e-7)); + triangulatePoint3(poses, kSharedCal, measurements, rank_tol, optimize); + EXPECT(assert_equal(kLandmark, *actual1, 1e-7)); // 2. test with optimization on, same answer optimize = true; boost::optional actual2 = // - triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize); - EXPECT(assert_equal(landmark, *actual2, 1e-7)); + triangulatePoint3(poses, kSharedCal, measurements, rank_tol, optimize); + EXPECT(assert_equal(kLandmark, *actual2, 1e-7)); // 3. Add some noise and try again: result should be ~ (4.995, // 0.499167, 1.19814) @@ -86,22 +86,22 @@ TEST(triangulation, twoPoses) { measurements.at(1) += Point2(-0.2, 0.3); optimize = false; boost::optional actual3 = // - triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize); + triangulatePoint3(poses, kSharedCal, measurements, rank_tol, optimize); EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual3, 1e-4)); // 4. Now with optimization on optimize = true; boost::optional actual4 = // - triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize); + triangulatePoint3(poses, kSharedCal, measurements, rank_tol, optimize); EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-4)); } TEST(triangulation, twoCamerasUsingLOST) { CameraSet> cameras; - cameras.push_back(camera1); - cameras.push_back(camera2); + cameras.push_back(kCamera1); + cameras.push_back(kCamera2); - Point2Vector measurements = {z1, z2}; + Point2Vector measurements = {kZ1, kZ2}; SharedNoiseModel measurementNoise = noiseModel::Isotropic::Sigma(2, 1e-4); double rank_tol = 1e-9; @@ -111,7 +111,7 @@ TEST(triangulation, twoCamerasUsingLOST) { cameras, measurements, rank_tol, /*optimize=*/false, measurementNoise, /*use_lost_triangulation=*/true); - EXPECT(assert_equal(landmark, *actual1, 1e-12)); + EXPECT(assert_equal(kLandmark, *actual1, 1e-12)); // 3. Add some noise and try again: result should be ~ (4.995, // 0.499167, 1.19814) @@ -167,18 +167,18 @@ TEST(triangulation, twoPosesCal3DS2) { boost::make_shared(1500, 1200, 0, 640, 480, -.3, 0.1, 0.0001, -0.0003); - PinholeCamera camera1Distorted(pose1, *sharedDistortedCal); + PinholeCamera camera1Distorted(kPose1, *sharedDistortedCal); - PinholeCamera camera2Distorted(pose2, *sharedDistortedCal); + PinholeCamera camera2Distorted(kPose2, *sharedDistortedCal); // 0. Project two landmarks into two cameras and triangulate - Point2 z1Distorted = camera1Distorted.project(landmark); - Point2 z2Distorted = camera2Distorted.project(landmark); + Point2 z1Distorted = camera1Distorted.project(kLandmark); + Point2 z2Distorted = camera2Distorted.project(kLandmark); vector poses; Point2Vector measurements; - poses += pose1, pose2; + poses += kPose1, kPose2; measurements += z1Distorted, z2Distorted; double rank_tol = 1e-9; @@ -188,14 +188,14 @@ TEST(triangulation, twoPosesCal3DS2) { boost::optional actual1 = // triangulatePoint3(poses, sharedDistortedCal, measurements, rank_tol, optimize); - EXPECT(assert_equal(landmark, *actual1, 1e-7)); + EXPECT(assert_equal(kLandmark, *actual1, 1e-7)); // 2. test with optimization on, same answer optimize = true; boost::optional actual2 = // triangulatePoint3(poses, sharedDistortedCal, measurements, rank_tol, optimize); - EXPECT(assert_equal(landmark, *actual2, 1e-7)); + EXPECT(assert_equal(kLandmark, *actual2, 1e-7)); // 3. Add some noise and try again: result should be ~ (4.995, // 0.499167, 1.19814) @@ -224,18 +224,18 @@ TEST(triangulation, twoPosesFisheye) { boost::make_shared(1500, 1200, .1, 640, 480, -.3, 0.1, 0.0001, -0.0003); - PinholeCamera camera1Distorted(pose1, *sharedDistortedCal); + PinholeCamera camera1Distorted(kPose1, *sharedDistortedCal); - PinholeCamera camera2Distorted(pose2, *sharedDistortedCal); + PinholeCamera camera2Distorted(kPose2, *sharedDistortedCal); // 0. Project two landmarks into two cameras and triangulate - Point2 z1Distorted = camera1Distorted.project(landmark); - Point2 z2Distorted = camera2Distorted.project(landmark); + Point2 z1Distorted = camera1Distorted.project(kLandmark); + Point2 z2Distorted = camera2Distorted.project(kLandmark); vector poses; Point2Vector measurements; - poses += pose1, pose2; + poses += kPose1, kPose2; measurements += z1Distorted, z2Distorted; double rank_tol = 1e-9; @@ -245,14 +245,14 @@ TEST(triangulation, twoPosesFisheye) { boost::optional actual1 = // triangulatePoint3(poses, sharedDistortedCal, measurements, rank_tol, optimize); - EXPECT(assert_equal(landmark, *actual1, 1e-7)); + EXPECT(assert_equal(kLandmark, *actual1, 1e-7)); // 2. test with optimization on, same answer optimize = true; boost::optional actual2 = // triangulatePoint3(poses, sharedDistortedCal, measurements, rank_tol, optimize); - EXPECT(assert_equal(landmark, *actual2, 1e-7)); + EXPECT(assert_equal(kLandmark, *actual2, 1e-7)); // 3. Add some noise and try again: result should be ~ (4.995, // 0.499167, 1.19814) @@ -277,17 +277,17 @@ TEST(triangulation, twoPosesFisheye) { TEST(triangulation, twoPosesBundler) { boost::shared_ptr bundlerCal = // boost::make_shared(1500, 0.1, 0.2, 640, 480); - PinholeCamera camera1(pose1, *bundlerCal); - PinholeCamera camera2(pose2, *bundlerCal); + PinholeCamera camera1(kPose1, *bundlerCal); + PinholeCamera camera2(kPose2, *bundlerCal); // 1. Project two landmarks into two cameras and triangulate - Point2 z1 = camera1.project(landmark); - Point2 z2 = camera2.project(landmark); + Point2 z1 = camera1.project(kLandmark); + Point2 z2 = camera2.project(kLandmark); vector poses; Point2Vector measurements; - poses += pose1, pose2; + poses += kPose1, kPose2; measurements += z1, z2; bool optimize = true; @@ -296,7 +296,7 @@ TEST(triangulation, twoPosesBundler) { boost::optional actual = // triangulatePoint3(poses, bundlerCal, measurements, rank_tol, optimize); - EXPECT(assert_equal(landmark, *actual, 1e-7)); + EXPECT(assert_equal(kLandmark, *actual, 1e-7)); // Add some noise and try again measurements.at(0) += Point2(0.1, 0.5); @@ -313,12 +313,12 @@ TEST(triangulation, fourPoses) { vector poses; Point2Vector measurements; - poses += pose1, pose2; - measurements += z1, z2; + poses += kPose1, kPose2; + measurements += kZ1, kZ2; boost::optional actual = - triangulatePoint3(poses, sharedCal, measurements); - EXPECT(assert_equal(landmark, *actual, 1e-2)); + triangulatePoint3(poses, kSharedCal, measurements); + EXPECT(assert_equal(kLandmark, *actual, 1e-2)); // 2. Add some noise and try again: result should be ~ (4.995, // 0.499167, 1.19814) @@ -326,37 +326,37 @@ TEST(triangulation, fourPoses) { measurements.at(1) += Point2(-0.2, 0.3); boost::optional actual2 = // - triangulatePoint3(poses, sharedCal, measurements); - EXPECT(assert_equal(landmark, *actual2, 1e-2)); + triangulatePoint3(poses, kSharedCal, measurements); + EXPECT(assert_equal(kLandmark, *actual2, 1e-2)); // 3. Add a slightly rotated third camera above, again with measurement noise - Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); - PinholeCamera camera3(pose3, *sharedCal); - Point2 z3 = camera3.project(landmark); + Pose3 pose3 = kPose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); + PinholeCamera camera3(pose3, *kSharedCal); + Point2 z3 = camera3.project(kLandmark); poses += pose3; measurements += z3 + Point2(0.1, -0.1); boost::optional triangulated_3cameras = // - triangulatePoint3(poses, sharedCal, measurements); - EXPECT(assert_equal(landmark, *triangulated_3cameras, 1e-2)); + triangulatePoint3(poses, kSharedCal, measurements); + EXPECT(assert_equal(kLandmark, *triangulated_3cameras, 1e-2)); // Again with nonlinear optimization boost::optional triangulated_3cameras_opt = - triangulatePoint3(poses, sharedCal, measurements, 1e-9, true); - EXPECT(assert_equal(landmark, *triangulated_3cameras_opt, 1e-2)); + triangulatePoint3(poses, kSharedCal, measurements, 1e-9, true); + EXPECT(assert_equal(kLandmark, *triangulated_3cameras_opt, 1e-2)); // 4. Test failure: Add a 4th camera facing the wrong way Pose3 pose4 = Pose3(Rot3::Ypr(M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); - PinholeCamera camera4(pose4, *sharedCal); + PinholeCamera camera4(pose4, *kSharedCal); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION - CHECK_EXCEPTION(camera4.project(landmark), CheiralityException); + CHECK_EXCEPTION(camera4.project(kLandmark), CheiralityException); poses += pose4; measurements += Point2(400, 400); - CHECK_EXCEPTION(triangulatePoint3(poses, sharedCal, measurements), + CHECK_EXCEPTION(triangulatePoint3(poses, kSharedCal, measurements), TriangulationCheiralityException); #endif } @@ -364,33 +364,33 @@ TEST(triangulation, fourPoses) { //****************************************************************************** TEST(triangulation, threePoses_robustNoiseModel) { - Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); - PinholeCamera camera3(pose3, *sharedCal); - Point2 z3 = camera3.project(landmark); + Pose3 pose3 = kPose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); + PinholeCamera camera3(pose3, *kSharedCal); + Point2 z3 = camera3.project(kLandmark); vector poses; Point2Vector measurements; - poses += pose1, pose2, pose3; - measurements += z1, z2, z3; + poses += kPose1, kPose2, pose3; + measurements += kZ1, kZ2, z3; // noise free, so should give exactly the landmark boost::optional actual = - triangulatePoint3(poses, sharedCal, measurements); - EXPECT(assert_equal(landmark, *actual, 1e-2)); + triangulatePoint3(poses, kSharedCal, measurements); + EXPECT(assert_equal(kLandmark, *actual, 1e-2)); // Add outlier measurements.at(0) += Point2(100, 120); // very large pixel noise! // now estimate does not match landmark boost::optional actual2 = // - triangulatePoint3(poses, sharedCal, measurements); + triangulatePoint3(poses, kSharedCal, measurements); // DLT is surprisingly robust, but still off (actual error is around 0.26m): - EXPECT( (landmark - *actual2).norm() >= 0.2); - EXPECT( (landmark - *actual2).norm() <= 0.5); + EXPECT( (kLandmark - *actual2).norm() >= 0.2); + EXPECT( (kLandmark - *actual2).norm() <= 0.5); // Again with nonlinear optimization boost::optional actual3 = - triangulatePoint3(poses, sharedCal, measurements, 1e-9, true); + triangulatePoint3(poses, kSharedCal, measurements, 1e-9, true); // result from nonlinear (but non-robust optimization) is close to DLT and still off EXPECT(assert_equal(*actual2, *actual3, 0.1)); @@ -398,27 +398,27 @@ TEST(triangulation, threePoses_robustNoiseModel) { auto model = noiseModel::Robust::Create( noiseModel::mEstimator::Huber::Create(1.345), noiseModel::Unit::Create(2)); boost::optional actual4 = triangulatePoint3( - poses, sharedCal, measurements, 1e-9, true, model); + poses, kSharedCal, measurements, 1e-9, true, model); // using the Huber loss we now have a quite small error!! nice! - EXPECT(assert_equal(landmark, *actual4, 0.05)); + EXPECT(assert_equal(kLandmark, *actual4, 0.05)); } //****************************************************************************** TEST(triangulation, fourPoses_robustNoiseModel) { - Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); - PinholeCamera camera3(pose3, *sharedCal); - Point2 z3 = camera3.project(landmark); + Pose3 pose3 = kPose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); + PinholeCamera camera3(pose3, *kSharedCal); + Point2 z3 = camera3.project(kLandmark); vector poses; Point2Vector measurements; - poses += pose1, pose1, pose2, pose3; // 2 measurements from pose 1 - measurements += z1, z1, z2, z3; + poses += kPose1, kPose1, kPose2, pose3; // 2 measurements from pose 1 + measurements += kZ1, kZ1, kZ2, z3; // noise free, so should give exactly the landmark boost::optional actual = - triangulatePoint3(poses, sharedCal, measurements); - EXPECT(assert_equal(landmark, *actual, 1e-2)); + triangulatePoint3(poses, kSharedCal, measurements); + EXPECT(assert_equal(kLandmark, *actual, 1e-2)); // Add outlier measurements.at(0) += Point2(100, 120); // very large pixel noise! @@ -429,14 +429,14 @@ TEST(triangulation, fourPoses_robustNoiseModel) { // now estimate does not match landmark boost::optional actual2 = // - triangulatePoint3(poses, sharedCal, measurements); + triangulatePoint3(poses, kSharedCal, measurements); // DLT is surprisingly robust, but still off (actual error is around 0.17m): - EXPECT( (landmark - *actual2).norm() >= 0.1); - EXPECT( (landmark - *actual2).norm() <= 0.5); + EXPECT( (kLandmark - *actual2).norm() >= 0.1); + EXPECT( (kLandmark - *actual2).norm() <= 0.5); // Again with nonlinear optimization boost::optional actual3 = - triangulatePoint3(poses, sharedCal, measurements, 1e-9, true); + triangulatePoint3(poses, kSharedCal, measurements, 1e-9, true); // result from nonlinear (but non-robust optimization) is close to DLT and still off EXPECT(assert_equal(*actual2, *actual3, 0.1)); @@ -444,24 +444,24 @@ TEST(triangulation, fourPoses_robustNoiseModel) { auto model = noiseModel::Robust::Create( noiseModel::mEstimator::Huber::Create(1.345), noiseModel::Unit::Create(2)); boost::optional actual4 = triangulatePoint3( - poses, sharedCal, measurements, 1e-9, true, model); + poses, kSharedCal, measurements, 1e-9, true, model); // using the Huber loss we now have a quite small error!! nice! - EXPECT(assert_equal(landmark, *actual4, 0.05)); + EXPECT(assert_equal(kLandmark, *actual4, 0.05)); } //****************************************************************************** TEST(triangulation, fourPoses_distinct_Ks) { Cal3_S2 K1(1500, 1200, 0, 640, 480); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - PinholeCamera camera1(pose1, K1); + PinholeCamera camera1(kPose1, K1); // create second camera 1 meter to the right of first camera Cal3_S2 K2(1600, 1300, 0, 650, 440); - PinholeCamera camera2(pose2, K2); + PinholeCamera camera2(kPose2, K2); // 1. Project two landmarks into two cameras and triangulate - Point2 z1 = camera1.project(landmark); - Point2 z2 = camera2.project(landmark); + Point2 z1 = camera1.project(kLandmark); + Point2 z2 = camera2.project(kLandmark); CameraSet> cameras; Point2Vector measurements; @@ -471,7 +471,7 @@ TEST(triangulation, fourPoses_distinct_Ks) { boost::optional actual = // triangulatePoint3(cameras, measurements); - EXPECT(assert_equal(landmark, *actual, 1e-2)); + EXPECT(assert_equal(kLandmark, *actual, 1e-2)); // 2. Add some noise and try again: result should be ~ (4.995, // 0.499167, 1.19814) @@ -480,25 +480,25 @@ TEST(triangulation, fourPoses_distinct_Ks) { boost::optional actual2 = // triangulatePoint3(cameras, measurements); - EXPECT(assert_equal(landmark, *actual2, 1e-2)); + EXPECT(assert_equal(kLandmark, *actual2, 1e-2)); // 3. Add a slightly rotated third camera above, again with measurement noise - Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); + Pose3 pose3 = kPose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); Cal3_S2 K3(700, 500, 0, 640, 480); PinholeCamera camera3(pose3, K3); - Point2 z3 = camera3.project(landmark); + Point2 z3 = camera3.project(kLandmark); cameras += camera3; measurements += z3 + Point2(0.1, -0.1); boost::optional triangulated_3cameras = // triangulatePoint3(cameras, measurements); - EXPECT(assert_equal(landmark, *triangulated_3cameras, 1e-2)); + EXPECT(assert_equal(kLandmark, *triangulated_3cameras, 1e-2)); // Again with nonlinear optimization boost::optional triangulated_3cameras_opt = triangulatePoint3(cameras, measurements, 1e-9, true); - EXPECT(assert_equal(landmark, *triangulated_3cameras_opt, 1e-2)); + EXPECT(assert_equal(kLandmark, *triangulated_3cameras_opt, 1e-2)); // 4. Test failure: Add a 4th camera facing the wrong way Pose3 pose4 = Pose3(Rot3::Ypr(M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); @@ -506,7 +506,7 @@ TEST(triangulation, fourPoses_distinct_Ks) { PinholeCamera camera4(pose4, K4); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION - CHECK_EXCEPTION(camera4.project(landmark), CheiralityException); + CHECK_EXCEPTION(camera4.project(kLandmark), CheiralityException); cameras += camera4; measurements += Point2(400, 400); @@ -519,15 +519,15 @@ TEST(triangulation, fourPoses_distinct_Ks) { TEST(triangulation, fourPoses_distinct_Ks_distortion) { Cal3DS2 K1(1500, 1200, 0, 640, 480, -.3, 0.1, 0.0001, -0.0003); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - PinholeCamera camera1(pose1, K1); + PinholeCamera camera1(kPose1, K1); // create second camera 1 meter to the right of first camera Cal3DS2 K2(1600, 1300, 0, 650, 440, -.2, 0.05, 0.0002, -0.0001); - PinholeCamera camera2(pose2, K2); + PinholeCamera camera2(kPose2, K2); // 1. Project two landmarks into two cameras and triangulate - Point2 z1 = camera1.project(landmark); - Point2 z2 = camera2.project(landmark); + Point2 z1 = camera1.project(kLandmark); + Point2 z2 = camera2.project(kLandmark); CameraSet> cameras; Point2Vector measurements; @@ -537,22 +537,22 @@ TEST(triangulation, fourPoses_distinct_Ks_distortion) { boost::optional actual = // triangulatePoint3(cameras, measurements); - EXPECT(assert_equal(landmark, *actual, 1e-2)); + EXPECT(assert_equal(kLandmark, *actual, 1e-2)); } //****************************************************************************** TEST(triangulation, outliersAndFarLandmarks) { Cal3_S2 K1(1500, 1200, 0, 640, 480); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - PinholeCamera camera1(pose1, K1); + PinholeCamera camera1(kPose1, K1); // create second camera 1 meter to the right of first camera Cal3_S2 K2(1600, 1300, 0, 650, 440); - PinholeCamera camera2(pose2, K2); + PinholeCamera camera2(kPose2, K2); // 1. Project two landmarks into two cameras and triangulate - Point2 z1 = camera1.project(landmark); - Point2 z2 = camera2.project(landmark); + Point2 z1 = camera1.project(kLandmark); + Point2 z2 = camera2.project(kLandmark); CameraSet> cameras; Point2Vector measurements; @@ -565,7 +565,7 @@ TEST(triangulation, outliersAndFarLandmarks) { 1.0, false, landmarkDistanceThreshold); // all default except // landmarkDistanceThreshold TriangulationResult actual = triangulateSafe(cameras, measurements, params); - EXPECT(assert_equal(landmark, *actual, 1e-2)); + EXPECT(assert_equal(kLandmark, *actual, 1e-2)); EXPECT(actual.valid()); landmarkDistanceThreshold = 4; // landmark is farther than that @@ -577,10 +577,10 @@ TEST(triangulation, outliersAndFarLandmarks) { // 3. Add a slightly rotated third camera above with a wrong measurement // (OUTLIER) - Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); + Pose3 pose3 = kPose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); Cal3_S2 K3(700, 500, 0, 640, 480); PinholeCamera camera3(pose3, K3); - Point2 z3 = camera3.project(landmark); + Point2 z3 = camera3.project(kLandmark); cameras += camera3; measurements += z3 + Point2(10, -10); @@ -603,18 +603,18 @@ TEST(triangulation, outliersAndFarLandmarks) { //****************************************************************************** TEST(triangulation, twoIdenticalPoses) { // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - PinholeCamera camera1(pose1, *sharedCal); + PinholeCamera camera1(kPose1, *kSharedCal); // 1. Project two landmarks into two cameras and triangulate - Point2 z1 = camera1.project(landmark); + Point2 z1 = camera1.project(kLandmark); vector poses; Point2Vector measurements; - poses += pose1, pose1; + poses += kPose1, kPose1; measurements += z1, z1; - CHECK_EXCEPTION(triangulatePoint3(poses, sharedCal, measurements), + CHECK_EXCEPTION(triangulatePoint3(poses, kSharedCal, measurements), TriangulationUnderconstrainedException); } @@ -629,7 +629,7 @@ TEST(triangulation, onePose) { poses += Pose3(); measurements += Point2(0, 0); - CHECK_EXCEPTION(triangulatePoint3(poses, sharedCal, measurements), + CHECK_EXCEPTION(triangulatePoint3(poses, kSharedCal, measurements), TriangulationUnderconstrainedException); } @@ -745,12 +745,12 @@ TEST(triangulation, twoPoses_sphericalCamera) { std::vector measurements; // Project landmark into two cameras and triangulate - SphericalCamera cam1(pose1); - SphericalCamera cam2(pose2); - Unit3 u1 = cam1.project(landmark); - Unit3 u2 = cam2.project(landmark); + SphericalCamera cam1(kPose1); + SphericalCamera cam2(kPose2); + Unit3 u1 = cam1.project(kLandmark); + Unit3 u2 = cam2.project(kLandmark); - poses += pose1, pose2; + poses += kPose1, kPose2; measurements += u1, u2; CameraSet cameras; @@ -762,25 +762,25 @@ TEST(triangulation, twoPoses_sphericalCamera) { // 1. Test linear triangulation via DLT auto projection_matrices = projectionMatricesFromCameras(cameras); Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol); - EXPECT(assert_equal(landmark, point, 1e-7)); + EXPECT(assert_equal(kLandmark, point, 1e-7)); // 2. Test nonlinear triangulation point = triangulateNonlinear(cameras, measurements, point); - EXPECT(assert_equal(landmark, point, 1e-7)); + EXPECT(assert_equal(kLandmark, point, 1e-7)); // 3. Test simple DLT, now within triangulatePoint3 bool optimize = false; boost::optional actual1 = // triangulatePoint3(cameras, measurements, rank_tol, optimize); - EXPECT(assert_equal(landmark, *actual1, 1e-7)); + EXPECT(assert_equal(kLandmark, *actual1, 1e-7)); // 4. test with optimization on, same answer optimize = true; boost::optional actual2 = // triangulatePoint3(cameras, measurements, rank_tol, optimize); - EXPECT(assert_equal(landmark, *actual2, 1e-7)); + EXPECT(assert_equal(kLandmark, *actual2, 1e-7)); // 5. Add some noise and try again: result should be ~ (4.995, // 0.499167, 1.19814) @@ -825,7 +825,7 @@ TEST(triangulation, twoPoses_sphericalCamera_extremeFOV) { EXPECT(assert_equal(Unit3(Point3(1.0, 0.0, -1.0)), u2, 1e-7)); // behind and to the right of PoseB - poses += pose1, pose2; + poses += kPose1, kPose2; measurements += u1, u2; CameraSet cameras; From da78cc202a33f1eb9ba77aed8edbcb2bb7c37311 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Tue, 12 Jul 2022 18:47:37 -0700 Subject: [PATCH 08/12] add author --- gtsam/geometry/tests/testTriangulation.cpp | 1 + gtsam/geometry/triangulation.cpp | 1 + gtsam/geometry/triangulation.h | 1 + 3 files changed, 3 insertions(+) diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index f6408fab9..0ce67083e 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -15,6 +15,7 @@ * @date July 30th, 2013 * @author Chris Beall (cbeall3) * @author Luca Carlone + * @author Akshay Krishnan */ #include diff --git a/gtsam/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp index c68fffb82..4a8bb279d 100644 --- a/gtsam/geometry/triangulation.cpp +++ b/gtsam/geometry/triangulation.cpp @@ -14,6 +14,7 @@ * @brief Functions for triangulation * @date July 31, 2013 * @author Chris Beall + * @author Akshay Krishnan */ #include diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 8a709ccd0..1b0970fe2 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -15,6 +15,7 @@ * @date July 31, 2013 * @author Chris Beall * @author Luca Carlone + * @author Akshay Krishnan */ #pragma once From b52aaeab22400f9d496611f0a431466d117ad767 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Tue, 12 Jul 2022 19:14:26 -0700 Subject: [PATCH 09/12] update to camelcase names in test --- gtsam/geometry/tests/testTriangulation.cpp | 27 +++++++++++----------- 1 file changed, 13 insertions(+), 14 deletions(-) diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index 0ce67083e..d9e605d55 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -129,36 +129,35 @@ TEST(triangulation, twoCamerasUsingLOST) { 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.)); + const Cal3_S2 identityK; + const Pose3 pose1; + const Pose3 pose2(Rot3(), Point3(5., 0., -5.)); CameraSet> cameras; - cameras.emplace_back(pose_1, identity_K); - cameras.emplace_back(pose_2, identity_K); + cameras.emplace_back(pose1, identityK); + cameras.emplace_back(pose2, identityK); - Point3 gt_point(0, 0, 1); - Point2 x1_noisy = cameras[0].project(gt_point) + Point2(0.00817, 0.00977); - Point2 x2_noisy = cameras[1].project(gt_point) + Point2(-0.00610, 0.01969); + Point3 landmark(0, 0, 1); + Point2 x1_noisy = cameras[0].project(landmark) + Point2(0.00817, 0.00977); + Point2 x2_noisy = cameras[1].project(landmark) + Point2(-0.00610, 0.01969); Point2Vector measurements = {x1_noisy, x2_noisy}; - double rank_tol = 1e-9; + const double rank_tol = 1e-9; SharedNoiseModel measurementNoise = noiseModel::Isotropic::Sigma(2, 1e-2); - boost::optional estimate_lost = // + boost::optional estimateLOST = // triangulatePoint3>( cameras, measurements, rank_tol, /*optimize=*/false, measurementNoise, /*use_lost_triangulation=*/true); // These values are from a MATLAB implementation. - EXPECT(assert_equal(Point3(0.007, 0.011, 0.945), *estimate_lost, 1e-3)); + EXPECT(assert_equal(Point3(0.007, 0.011, 0.945), *estimateLOST, 1e-3)); - boost::optional estimate_dlt = + boost::optional estimateDLT = triangulatePoint3(cameras, measurements, rank_tol, false); // The LOST estimate should have a smaller error. - EXPECT((gt_point - *estimate_lost).norm() <= - (gt_point - *estimate_dlt).norm()); + EXPECT((landmark - *estimateLOST).norm() <= (landmark - *estimateDLT).norm()); } //****************************************************************************** From f347209d4ce4d36db373519d62ebc47b76c67e2b Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Tue, 12 Jul 2022 19:30:41 -0700 Subject: [PATCH 10/12] more camelcase changes --- gtsam/geometry/tests/testTriangulation.cpp | 16 ++++++------ gtsam/geometry/triangulation.cpp | 19 +++++++------- gtsam/geometry/triangulation.h | 30 ++++++++++------------ 3 files changed, 31 insertions(+), 34 deletions(-) diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index d9e605d55..bc843ad75 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -108,10 +108,10 @@ TEST(triangulation, twoCamerasUsingLOST) { // 1. Test simple triangulation, perfect in no noise situation boost::optional actual1 = // - triangulatePoint3>( - cameras, measurements, rank_tol, - /*optimize=*/false, measurementNoise, - /*use_lost_triangulation=*/true); + triangulatePoint3>(cameras, measurements, rank_tol, + /*optimize=*/false, + measurementNoise, + /*useLOST=*/true); EXPECT(assert_equal(kLandmark, *actual1, 1e-12)); // 3. Add some noise and try again: result should be ~ (4.995, @@ -145,10 +145,10 @@ TEST(triangulation, twoCamerasLOSTvsDLT) { SharedNoiseModel measurementNoise = noiseModel::Isotropic::Sigma(2, 1e-2); boost::optional estimateLOST = // - triangulatePoint3>( - cameras, measurements, rank_tol, - /*optimize=*/false, measurementNoise, - /*use_lost_triangulation=*/true); + triangulatePoint3>(cameras, measurements, rank_tol, + /*optimize=*/false, + measurementNoise, + /*useLOST=*/true); // These values are from a MATLAB implementation. EXPECT(assert_equal(Point3(0.007, 0.011, 0.945), *estimateLOST, 1e-3)); diff --git a/gtsam/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp index 4a8bb279d..acb0ddf0e 100644 --- a/gtsam/geometry/triangulation.cpp +++ b/gtsam/geometry/triangulation.cpp @@ -107,20 +107,19 @@ Point3 triangulateLOST(const std::vector& poses, const Point3 d_ij = wTj.translation() - wTi.translation(); - const Point3 w_measurement_i = - wTi.rotation().rotate(calibratedMeasurements[i]); - const Point3 w_measurement_j = - wTj.rotation().rotate(calibratedMeasurements[j]); + const Point3 wZi = wTi.rotation().rotate(calibratedMeasurements[i]); + const Point3 wZj = wTj.rotation().rotate(calibratedMeasurements[j]); - const double q_i = - w_measurement_i.cross(w_measurement_j).norm() / - (measurementNoise->sigma() * d_ij.cross(w_measurement_j).norm()); - const Matrix23 coefficient_mat = + // Note: Setting q_i = 1.0 gives same results as DLT. + const double q_i = wZi.cross(wZj).norm() / + (measurementNoise->sigma() * d_ij.cross(wZj).norm()); + + const Matrix23 coefficientMat = q_i * skewSymmetric(calibratedMeasurements[i]).topLeftCorner(2, 3) * wTi.rotation().matrix().transpose(); - A.block<2, 3>(2 * i, 0) << coefficient_mat; - b.block<2, 1>(2 * i, 0) << coefficient_mat * wTi.translation(); + A.block<2, 3>(2 * i, 0) << coefficientMat; + b.block<2, 1>(2 * i, 0) << coefficientMat * wTi.translation(); } return A.colPivHouseholderQr().solve(b); } diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 1b0970fe2..aa9433484 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -415,8 +415,7 @@ inline Point3Vector calibrateMeasurements( * @param measurements A vector of camera measurements * @param rank_tol rank tolerance, default 1e-9 * @param optimize Flag to turn on nonlinear refinement of triangulation - * @param use_lost_triangulation whether to use the LOST algorithm instead of - * DLT + * @param useLOST whether to use the LOST algorithm instead of DLT * @return Returns a Point3 */ template @@ -425,13 +424,13 @@ Point3 triangulatePoint3(const std::vector& poses, const Point2Vector& measurements, double rank_tol = 1e-9, bool optimize = false, const SharedNoiseModel& model = nullptr, - const bool use_lost_triangulation = false) { + const bool useLOST = false) { assert(poses.size() == measurements.size()); if (poses.size() < 2) throw(TriangulationUnderconstrainedException()); // Triangulate linearly Point3 point; - if (use_lost_triangulation) { + if (useLOST) { // Reduce input noise model to an isotropic noise model using the mean of // the diagonal. const double measurementSigma = model ? model->sigmas().mean() : 1e-4; @@ -481,7 +480,7 @@ Point3 triangulatePoint3(const std::vector& poses, * @param measurements A vector of camera measurements * @param rank_tol rank tolerance, default 1e-9 * @param optimize Flag to turn on nonlinear refinement of triangulation - * @param use_lost_triangulation whether to use the LOST algorithm instead of + * @param useLOST whether to use the LOST algorithm instead of * DLT * @return Returns a Point3 */ @@ -490,7 +489,7 @@ Point3 triangulatePoint3(const CameraSet& cameras, const typename CAMERA::MeasurementVector& measurements, double rank_tol = 1e-9, bool optimize = false, const SharedNoiseModel& model = nullptr, - const bool use_lost_triangulation = false) { + const bool useLOST = false) { size_t m = cameras.size(); assert(measurements.size() == m); @@ -498,7 +497,7 @@ Point3 triangulatePoint3(const CameraSet& cameras, // Triangulate linearly Point3 point; - if (use_lost_triangulation) { + if (useLOST) { // Reduce input noise model to an isotropic noise model using the mean of // the diagonal. const double measurementSigma = model ? model->sigmas().mean() : 1e-4; @@ -545,15 +544,14 @@ Point3 triangulatePoint3(const CameraSet& cameras, } /// Pinhole-specific version -template -Point3 triangulatePoint3( - const CameraSet >& cameras, - const Point2Vector& measurements, double rank_tol = 1e-9, - bool optimize = false, - const SharedNoiseModel& model = nullptr, - const bool use_lost_triangulation = false) { - return triangulatePoint3 > // - (cameras, measurements, rank_tol, optimize, model, use_lost_triangulation); +template +Point3 triangulatePoint3(const CameraSet>& cameras, + const Point2Vector& measurements, + double rank_tol = 1e-9, bool optimize = false, + const SharedNoiseModel& model = nullptr, + const bool useLOST = false) { + return triangulatePoint3> // + (cameras, measurements, rank_tol, optimize, model, useLOST); } struct GTSAM_EXPORT TriangulationParameters { From 897dae4436e9d8ce5b9518c23cca7abe9e5b7f4b Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Tue, 12 Jul 2022 20:03:02 -0700 Subject: [PATCH 11/12] more renames to camelcase --- examples/TriangulationLOSTExample.cpp | 130 +++++++++++++------------- gtsam/geometry/triangulation.h | 16 ++-- 2 files changed, 72 insertions(+), 74 deletions(-) diff --git a/examples/TriangulationLOSTExample.cpp b/examples/TriangulationLOSTExample.cpp index a903c625a..a862026e0 100644 --- a/examples/TriangulationLOSTExample.cpp +++ b/examples/TriangulationLOSTExample.cpp @@ -53,24 +53,24 @@ void PrintDuration(const std::chrono::nanoseconds dur, double num_samples, void GetLargeCamerasDataset(CameraSet>* cameras, std::vector* poses, Point3* point, Point2Vector* measurements) { - const double min_xy = -10, max_xy = 10; - const double min_z = -20, max_z = 0; - const int num_cameras = 500; - cameras->reserve(num_cameras); - poses->reserve(num_cameras); - measurements->reserve(num_cameras); + const double minXY = -10, maxXY = 10; + const double minZ = -20, maxZ = 0; + const int nrCameras = 500; + cameras->reserve(nrCameras); + poses->reserve(nrCameras); + measurements->reserve(nrCameras); *point = Point3(0.0, 0.0, 10.0); - std::uniform_real_distribution rand_xy(min_xy, max_xy); - std::uniform_real_distribution rand_z(min_z, max_z); - Cal3_S2 identity_K; + std::uniform_real_distribution rand_xy(minXY, maxXY); + std::uniform_real_distribution rand_z(minZ, maxZ); + Cal3_S2 identityK; - for (int i = 0; i < num_cameras; ++i) { + for (int i = 0; i < nrCameras; ++i) { Point3 wti(rand_xy(rng), rand_xy(rng), rand_z(rng)); Pose3 wTi(Rot3(), wti); poses->push_back(wTi); - cameras->emplace_back(wTi, identity_K); + cameras->emplace_back(wTi, identityK); measurements->push_back(cameras->back().project(*point)); } } @@ -78,84 +78,82 @@ void GetLargeCamerasDataset(CameraSet>* cameras, void GetSmallCamerasDataset(CameraSet>* cameras, std::vector* poses, Point3* point, Point2Vector* measurements) { - Pose3 pose_1; - Pose3 pose_2(Rot3(), Point3(5., 0., -5.)); - Cal3_S2 identity_K; - PinholeCamera camera_1(pose_1, identity_K); - PinholeCamera camera_2(pose_2, identity_K); + Pose3 pose1; + Pose3 pose2(Rot3(), Point3(5., 0., -5.)); + Cal3_S2 identityK; + PinholeCamera camera1(pose1, identityK); + PinholeCamera camera2(pose2, identityK); *point = Point3(0, 0, 1); - cameras->push_back(camera_1); - cameras->push_back(camera_2); - *poses = {pose_1, pose_2}; - *measurements = {camera_1.project(*point), camera_2.project(*point)}; + cameras->push_back(camera1); + cameras->push_back(camera2); + *poses = {pose1, pose2}; + *measurements = {camera1.project(*point), camera2.project(*point)}; } Point2Vector AddNoiseToMeasurements(const Point2Vector& measurements, - const double measurement_sigma) { - std::normal_distribution normal(0.0, measurement_sigma); + const double measurementSigma) { + std::normal_distribution normal(0.0, measurementSigma); - Point2Vector noisy_measurements; - noisy_measurements.reserve(measurements.size()); + Point2Vector noisyMeasurements; + noisyMeasurements.reserve(measurements.size()); for (const auto& p : measurements) { - noisy_measurements.emplace_back(p.x() + normal(rng), p.y() + normal(rng)); + noisyMeasurements.emplace_back(p.x() + normal(rng), p.y() + normal(rng)); } - return noisy_measurements; + return noisyMeasurements; } /* ************************************************************************* */ int main(int argc, char* argv[]) { CameraSet> cameras; std::vector poses; - Point3 gt_point; + Point3 landmark; Point2Vector measurements; - GetLargeCamerasDataset(&cameras, &poses, >_point, &measurements); - // GetSmallCamerasDataset(&cameras, &poses, >_point, &measurements); - const double measurement_sigma = 1e-2; - SharedNoiseModel measurement_noise = - noiseModel::Isotropic::Sigma(2, measurement_sigma); + GetLargeCamerasDataset(&cameras, &poses, &landmark, &measurements); + // GetSmallCamerasDataset(&cameras, &poses, &landmark, &measurements); + const double measurementSigma = 1e-2; + SharedNoiseModel measurementNoise = + noiseModel::Isotropic::Sigma(2, measurementSigma); - const long int num_trials = 1000; - Matrix dlt_errors = Matrix::Zero(num_trials, 3); - Matrix lost_errors = Matrix::Zero(num_trials, 3); - Matrix dlt_opt_errors = Matrix::Zero(num_trials, 3); + const long int nrTrials = 1000; + Matrix errorsDLT = Matrix::Zero(nrTrials, 3); + Matrix errorsLOST = Matrix::Zero(nrTrials, 3); + Matrix errorsDLTOpt = Matrix::Zero(nrTrials, 3); double rank_tol = 1e-9; boost::shared_ptr calib = boost::make_shared(); - std::chrono::nanoseconds dlt_duration; - std::chrono::nanoseconds dlt_opt_duration; - std::chrono::nanoseconds lost_duration; - std::chrono::nanoseconds lost_tls_duration; + std::chrono::nanoseconds durationDLT; + std::chrono::nanoseconds durationDLTOpt; + std::chrono::nanoseconds durationLOST; - for (int i = 0; i < num_trials; i++) { - Point2Vector noisy_measurements = - AddNoiseToMeasurements(measurements, measurement_sigma); + for (int i = 0; i < nrTrials; i++) { + Point2Vector noisyMeasurements = + AddNoiseToMeasurements(measurements, measurementSigma); - auto lost_start = std::chrono::high_resolution_clock::now(); - boost::optional estimate_lost = triangulatePoint3( - cameras, noisy_measurements, rank_tol, false, measurement_noise, true); - lost_duration += std::chrono::high_resolution_clock::now() - lost_start; + auto lostStart = std::chrono::high_resolution_clock::now(); + boost::optional estimateLOST = triangulatePoint3( + cameras, noisyMeasurements, rank_tol, false, measurementNoise, true); + durationLOST += std::chrono::high_resolution_clock::now() - lostStart; - auto dlt_start = std::chrono::high_resolution_clock::now(); - boost::optional estimate_dlt = triangulatePoint3( - cameras, noisy_measurements, rank_tol, false, measurement_noise, false); - dlt_duration += std::chrono::high_resolution_clock::now() - dlt_start; + auto dltStart = std::chrono::high_resolution_clock::now(); + boost::optional estimateDLT = triangulatePoint3( + cameras, noisyMeasurements, rank_tol, false, measurementNoise, false); + durationDLT += std::chrono::high_resolution_clock::now() - dltStart; - auto dlt_opt_start = std::chrono::high_resolution_clock::now(); - boost::optional estimate_dlt_opt = triangulatePoint3( - cameras, noisy_measurements, rank_tol, true, measurement_noise, false); - dlt_opt_duration += - std::chrono::high_resolution_clock::now() - dlt_opt_start; + auto dltOptStart = std::chrono::high_resolution_clock::now(); + boost::optional estimateDLTOpt = triangulatePoint3( + cameras, noisyMeasurements, rank_tol, true, measurementNoise, false); + durationDLTOpt += std::chrono::high_resolution_clock::now() - dltOptStart; - lost_errors.row(i) = *estimate_lost - gt_point; - dlt_errors.row(i) = *estimate_dlt - gt_point; - dlt_opt_errors.row(i) = *estimate_dlt_opt - gt_point; + errorsLOST.row(i) = *estimateLOST - landmark; + errorsDLT.row(i) = *estimateDLT - landmark; + errorsDLTOpt.row(i) = *estimateDLTOpt - landmark; } - PrintCovarianceStats(lost_errors, "LOST"); - PrintCovarianceStats(dlt_errors, "DLT"); - PrintCovarianceStats(dlt_opt_errors, "DLT_OPT"); + PrintCovarianceStats(errorsLOST, "LOST"); + PrintCovarianceStats(errorsDLT, "DLT"); + PrintCovarianceStats(errorsDLTOpt, "DLT_OPT"); - PrintDuration(lost_duration, num_trials, "LOST"); - PrintDuration(dlt_duration, num_trials, "DLT"); - PrintDuration(dlt_opt_duration, num_trials, "DLT_OPT"); -} \ No newline at end of file + PrintDuration(durationLOST, nrTrials, "LOST"); + PrintDuration(durationDLT, nrTrials, "DLT"); + PrintDuration(durationDLTOpt, nrTrials, "DLT_OPT"); +} diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index aa9433484..f6c919b24 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -317,10 +317,10 @@ template typename CAMERA::MeasurementVector undistortMeasurements( const CameraSet& cameras, const typename CAMERA::MeasurementVector& measurements) { - const size_t num_meas = measurements.size(); - assert(num_meas == cameras.size()); - typename CAMERA::MeasurementVector undistortedMeasurements(num_meas); - for (size_t ii = 0; ii < num_meas; ++ii) { + const size_t nrMeasurements = measurements.size(); + assert(nrMeasurements == cameras.size()); + typename CAMERA::MeasurementVector undistortedMeasurements(nrMeasurements); + for (size_t ii = 0; ii < nrMeasurements; ++ii) { // Calibrate with cal and uncalibrate with pinhole version of cal so that // measurements are undistorted. undistortedMeasurements[ii] = @@ -382,10 +382,10 @@ template inline Point3Vector calibrateMeasurements( const CameraSet& cameras, const typename CAMERA::MeasurementVector& measurements) { - const size_t num_meas = measurements.size(); - assert(num_meas == cameras.size()); - Point3Vector calibratedMeasurements(num_meas); - for (size_t ii = 0; ii < num_meas; ++ii) { + const size_t nrMeasurements = measurements.size(); + assert(nrMeasurements == cameras.size()); + Point3Vector calibratedMeasurements(nrMeasurements); + for (size_t ii = 0; ii < nrMeasurements; ++ii) { calibratedMeasurements[ii] << cameras[ii].calibration().calibrate(measurements[ii]), 1.0; From a3ee2660e5809ef40dcd5f9a2d56f32cf7b2efa3 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Tue, 12 Jul 2022 22:50:58 -0700 Subject: [PATCH 12/12] LOST in python wrapper --- gtsam/geometry/geometry.i | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 517558265..597f6a169 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -1129,7 +1129,8 @@ gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3_S2& cameras, const gtsam::Point2Vector& measurements, double rank_tol, bool optimize, - const gtsam::SharedNoiseModel& model = nullptr); + const gtsam::SharedNoiseModel& model = nullptr, + const bool useLOST = false); gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, gtsam::Cal3_S2* sharedCal, const gtsam::Point2Vector& measurements, @@ -1151,7 +1152,8 @@ gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3DS2& cameras, const gtsam::Point2Vector& measurements, double rank_tol, bool optimize, - const gtsam::SharedNoiseModel& model = nullptr); + const gtsam::SharedNoiseModel& model = nullptr, + const bool useLOST = false); gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, gtsam::Cal3DS2* sharedCal, const gtsam::Point2Vector& measurements, @@ -1173,7 +1175,8 @@ gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Bundler& cameras, const gtsam::Point2Vector& measurements, double rank_tol, bool optimize, - const gtsam::SharedNoiseModel& model = nullptr); + const gtsam::SharedNoiseModel& model = nullptr, + const bool useLOST = false); gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, gtsam::Cal3Bundler* sharedCal, const gtsam::Point2Vector& measurements, @@ -1195,7 +1198,8 @@ gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Fisheye& cameras, const gtsam::Point2Vector& measurements, double rank_tol, bool optimize, - const gtsam::SharedNoiseModel& model = nullptr); + const gtsam::SharedNoiseModel& model = nullptr, + const bool useLOST = false); gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, gtsam::Cal3Fisheye* sharedCal, const gtsam::Point2Vector& measurements, @@ -1217,7 +1221,8 @@ gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Unified& cameras, const gtsam::Point2Vector& measurements, double rank_tol, bool optimize, - const gtsam::SharedNoiseModel& model = nullptr); + const gtsam::SharedNoiseModel& model = nullptr, + const bool useLOST = false); gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, gtsam::Cal3Unified* sharedCal, const gtsam::Point2Vector& measurements,