[Feature] Added function for converting gps coordinates to relative local

Author: artzha

math/gps_util.h

@@ -1,10 +1,12 @@
 #ifndef GPS_UTIL_H_
 #define GPS_UTIL_H_
 
+#include "eigen3/Eigen/Dense"
 #include <math.h>
 #include <tuple>
 
 namespace gps_util {
+
 inline std::tuple<double, double, int> gpsToUTM(double lat, double lon) {
     // Constants for UTM conversion
     constexpr double a = 6378137.0;           // WGS-84 major axis
@@ -56,6 +58,46 @@ inline std::tuple<double, double> gpsToGlobalCoord(double lat0, double lon0, dou
     const auto& [e1, n1, zone1] = gpsToUTM(lat1, lon1);
     return {e1 - e0, n1 - n0};
 }
+
+inline Eigen::Affine2f gpsToLocal(
+    double current_lat, double current_lon, double current_heading,
+    double goal_lat, double goal_lon, double goal_heading) {
+
+    // Step 1: Convert current and goal GPS coordinates to UTM
+    const auto& [curr_easting, curr_northing, curr_zone] = gpsToUTM(current_lat, current_lon);
+    const auto& [goal_easting, goal_northing, goal_zone] = gpsToUTM(goal_lat, goal_lon);
+    // Ensure that both coordinates are in the same UTM zone
+    if (goal_zone != curr_zone) {
+        throw std::runtime_error("GPS points are in different UTM zones.");
+    }
+
+    // Step 2: Calculate the translation vector from the current position to the goal in UTM
+    double dx = goal_easting - curr_easting;
+    double dy = goal_northing - curr_northing;
+
+    // Step 3: Convert the current heading to radians and adjust for x-axis reference
+    // Since 0 degrees points along the y-axis and rotates counter-clockwise,
+    // convert to radians with 0 degrees aligned along the x-axis
+    double current_heading_rad = (90.0 - current_heading) * M_PI / 180.0;
+
+    // Step 4: Rotate the translation vector to the robot's local frame
+    double local_x = dx * cos(-current_heading_rad) - dy * sin(-current_heading_rad);
+    double local_y = dx * sin(-current_heading_rad) + dy * cos(-current_heading_rad);
+
+    // Step 5: Convert the goal heading to the local frame
+    double goal_heading_rad = (90.0 - goal_heading) * M_PI / 180.0;
+    double local_heading = goal_heading_rad - current_heading_rad;
+
+    // Normalize local heading to the range [-pi, pi]
+    while (local_heading > M_PI) local_heading -= 2 * M_PI;
+    while (local_heading < -M_PI) local_heading += 2 * M_PI;
+
+    // Step 6: Create the affine transformation for the local pose of the goal
+    Eigen::Affine2f transform = Eigen::Translation2f(local_x, local_y) * Eigen::Rotation2Df(local_heading);
+
+    return transform;
+}
+
 }
 
 #endif