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