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