made triangulatePoint more human readable

src/target_handler/triangulate.cpp

@@ -11,17 +11,23 @@ Eigen::Vector3d triangulatePoint(Eigen::ParametrizedLine<double, 3> &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<double, 3> &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) {