From 8a78039e39cc05962bc39d15b245fe3a5d55124e Mon Sep 17 00:00:00 2001 From: IvarNilsson Date: Mon, 5 Aug 2024 11:44:31 +0200 Subject: [PATCH] made triangulatePoint more human readable --- src/target_handler/triangulate.cpp | 26 ++++++++++++++++---------- 1 file changed, 16 insertions(+), 10 deletions(-) diff --git a/src/target_handler/triangulate.cpp b/src/target_handler/triangulate.cpp index e990fac..2daebfb 100644 --- a/src/target_handler/triangulate.cpp +++ b/src/target_handler/triangulate.cpp @@ -11,17 +11,23 @@ Eigen::Vector3d triangulatePoint(Eigen::ParametrizedLine &vector1, Ei const double distance_threshold) { using Vec3 = Eigen::Vector3d; - const Vec3 r1 = vector1.origin(); - const Vec3 e1 = vector1.direction(); - const Vec3 r2 = vector2.origin(); - const Vec3 e2 = vector2.direction(); + // function for each vector (simple Y=kx+m): point = origin + direction * t (where t is the length along the vector) + const Vec3 origin1 = vector1.origin(); + const Vec3 direction1 = vector1.direction(); + const Vec3 origin2 = vector2.origin(); + const Vec3 direction2 = vector2.direction(); - const Vec3 n = e1.cross(e2); - const double t1 = e2.cross(n).dot(r2 - r1) / n.dot(n); - const double t2 = e1.cross(n).dot(r2 - r1) / n.dot(n); + // the formula for finding the closest points between two lines is derived using alot of linera algebra but can be summed to: + // n(tanget vector) = direction1 x direction2 + // t1 = (direction2 x n).(origin2 - origin1)/(n.n) (.=dot product, x=cross product) + // use y=kx+m along the vector (closestPoint1 = origin1 + direction1 * t1) + const Vec3 n = direction1.cross(direction2);// get tanget vector to calculate the closes point - const Vec3 closestPoint1 = r1 + e1 * t1; - const Vec3 closestPoint2 = r2 + e2 * t2; + const double t1 = direction2.cross(n).dot(origin2 - origin1) / n.dot(n);// calculate the distance on the vector closest to the other vector + const double t2 = direction1.cross(n).dot(origin2 - origin1) / n.dot(n); + + const Vec3 closestPoint1 = origin1 + direction1 * t1;// calculate the point from the function of the vector and the value t + const Vec3 closestPoint2 = origin2 + direction2 * t2; if (constexpr double min_dist = 0, max_dist = 1; (closestPoint1 - closestPoint2).norm() > distance_threshold || ((closestPoint1 + closestPoint2) / 2).norm() > 20// We can't expect to hear targets further than 20 m away @@ -31,7 +37,7 @@ Eigen::Vector3d triangulatePoint(Eigen::ParametrizedLine &vector1, Ei return Vec3::Zero(); } - return (closestPoint1 + closestPoint2) / 2.0; + return (closestPoint1 + closestPoint2) / 2.0;// we return the point in the middle of the two closest points on the two direction vectors } nlohmann::json PositionToGPS(const Eigen::Vector3d &position, const gps_data_t &gps_position) {