[Feature] Integrated support for gps to amrl map integration

Author: artzha

math/gps_util.h

@@ -1,103 +1,202 @@
 #ifndef GPS_UTIL_H_
 #define GPS_UTIL_H_
 
-#include "eigen3/Eigen/Dense"
 #include <math.h>
+
+#include <cmath>
+#include <string>
 #include <tuple>
 
+#include "eigen3/Eigen/Dense"
+#include "shared/math/math_util.h"
+#include "shared/util/helpers.h"
+
+using Eigen::Rotation2Dd;
+using Eigen::Vector2d;
+using math_util::DegToRad;
+using std::string;
+
 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
-    constexpr double f = 1 / 298.257223563;   // WGS-84 flattening
-    constexpr double k0 = 0.9996;             // UTM scale factor
-    constexpr double e = sqrt(f * (2 - f));   // Eccentricity
-    constexpr double e2 = e * e;
-
-    // UTM Zone
-    int zone = static_cast<int>((lon + 180) / 6) + 1;
-
-    // Convert latitude and longitude to radians
-    double lat_rad = lat * M_PI / 180.0;
-    double lon_rad = lon * M_PI / 180.0;
-
-    // Central meridian of the UTM zone
-    double lon_origin = (zone - 1) * 6 - 180 + 3;
-    double lon_origin_rad = lon_origin * M_PI / 180.0;
-
-    // Calculations for UTM coordinates
-    double N = a / sqrt(1 - e2 * sin(lat_rad) * sin(lat_rad));
-    double T = tan(lat_rad) * tan(lat_rad);
-    double C = e2 / (1 - e2) * cos(lat_rad) * cos(lat_rad);
-    double A = cos(lat_rad) * (lon_rad - lon_origin_rad);
-
-    double M = a * ((1 - e2 / 4 - 3 * e2 * e2 / 64 - 5 * e2 * e2 * e2 / 256) * lat_rad
-                    - (3 * e2 / 8 + 3 * e2 * e2 / 32 + 45 * e2 * e2 * e2 / 1024) * sin(2 * lat_rad)
-                    + (15 * e2 * e2 / 256 + 45 * e2 * e2 * e2 / 1024) * sin(4 * lat_rad)
-                    - (35 * e2 * e2 * e2 / 3072) * sin(6 * lat_rad));
-
-    double easting = k0 * N * (A + (1 - T + C) * A * A * A / 6
-                    + (5 - 18 * T + T * T + 72 * C - 58 * e2) * A * A * A * A * A / 120) + 500000.0;
-
-    double northing = k0 * (M + N * tan(lat_rad) * (A * A / 2
-                    + (5 - T + 9 * C + 4 * C * C) * A * A * A * A / 24
-                    + (61 - 58 * T + T * T + 600 * C - 330 * e2) * A * A * A * A * A * A / 720));
-
-    // Correct for southern hemisphere
-    if (lat < 0) {
-        northing += 10000000.0;
+struct GPSTranslator {
+  string GetMapFileFromName(const string& maps_dir, const string& map) {
+    return maps_dir + "/" + map + "/" + map + ".gpsmap.txt";
+  }
+
+  bool Load(const string& maps_dir, const string& map) {
+    const string file = GetMapFileFromName(maps_dir, map);
+    ScopedFile fid(file, "r", true);
+    if (fid() == nullptr) return false;
+    if (fscanf(fid(), "%lf, %lf, %lf", &gps_origin_latitude,
+               &gps_origin_longitude, &map_orientation) != 3) {
+      return false;
     }
+    printf("Map origin: %12.8lf, %12.8lf\n", gps_origin_latitude,
+           gps_origin_longitude);
+    return true;
+  }
+
+  Vector2d GPSToMetric(const double latitude, const double longitude) {
+    const double theta = DegToRad(latitude);
+    const double c = std::cos(theta);
+    const double s = std::sin(theta);
+    const double r =
+        sqrt(Sq(wgs_84_a * wgs_84_b) / (Sq(c * wgs_84_b) + Sq(s * wgs_84_a)));
+    const double dlat = DegToRad(latitude - gps_origin_latitude);
+    const double dlong = DegToRad(longitude - gps_origin_longitude);
+    const double r1 = r * c;
+    const double x = r1 * dlong;
+    const double y = r * dlat;
+    return Rotation2Dd(map_orientation) * Vector2d(x, y);
+  }
+
+  void MetricToGPS(const Vector2d& loc, double* longitude, double* latitude) {
+    const double theta = DegToRad(gps_origin_latitude);
+    const double c = std::cos(theta);
+    const double s = std::sin(theta);
+    const double r =
+        sqrt(Sq(wgs_84_a * wgs_84_b) / (Sq(c * wgs_84_b) + Sq(s * wgs_84_a)));
+    const double r1 = r * c;
+    const double dlat = loc.y() / r;
+    const double dlong = loc.x() / r1;
+    *longitude = gps_origin_longitude + dlong;
+    *latitude = gps_origin_latitude + dlat;
+  }
+
+  double gps_origin_longitude;
+  double gps_origin_latitude;
+  double map_orientation;
+
+  // Earth geoid parameters from WGS 84 system
+  // https://en.wikipedia.org/wiki/World_Geodetic_System#A_new_World_Geodetic_System:_WGS_84
+  // a = Semimajor (Equatorial) axis
+  static constexpr double wgs_84_a = 6378137.0;
+  // b = Semiminor (Polar) axis
+  static constexpr double wgs_84_b = 6356752.314245;
+};
 
-    return std::make_tuple(easting, northing, zone);
+inline std::tuple<double, double, int> gpsToUTM(double lat, double lon) {
+  // Constants for UTM conversion
+  constexpr double a = 6378137.0;          // WGS-84 major axis
+  constexpr double f = 1 / 298.257223563;  // WGS-84 flattening
+  constexpr double k0 = 0.9996;            // UTM scale factor
+  constexpr double e = sqrt(f * (2 - f));  // Eccentricity
+  constexpr double e2 = e * e;
+
+  // UTM Zone
+  int zone = static_cast<int>((lon + 180) / 6) + 1;
+
+  // Convert latitude and longitude to radians
+  double lat_rad = lat * M_PI / 180.0;
+  double lon_rad = lon * M_PI / 180.0;
+
+  // Central meridian of the UTM zone
+  double lon_origin = (zone - 1) * 6 - 180 + 3;
+  double lon_origin_rad = lon_origin * M_PI / 180.0;
+
+  // Calculations for UTM coordinates
+  double N = a / sqrt(1 - e2 * sin(lat_rad) * sin(lat_rad));
+  double T = tan(lat_rad) * tan(lat_rad);
+  double C = e2 / (1 - e2) * cos(lat_rad) * cos(lat_rad);
+  double A = cos(lat_rad) * (lon_rad - lon_origin_rad);
+
+  double M =
+      a * ((1 - e2 / 4 - 3 * e2 * e2 / 64 - 5 * e2 * e2 * e2 / 256) * lat_rad -
+           (3 * e2 / 8 + 3 * e2 * e2 / 32 + 45 * e2 * e2 * e2 / 1024) *
+               sin(2 * lat_rad) +
+           (15 * e2 * e2 / 256 + 45 * e2 * e2 * e2 / 1024) * sin(4 * lat_rad) -
+           (35 * e2 * e2 * e2 / 3072) * sin(6 * lat_rad));
+
+  double easting =
+      k0 * N *
+          (A + (1 - T + C) * A * A * A / 6 +
+           (5 - 18 * T + T * T + 72 * C - 58 * e2) * A * A * A * A * A / 120) +
+      500000.0;
+
+  double northing =
+      k0 *
+      (M + N * tan(lat_rad) *
+               (A * A / 2 + (5 - T + 9 * C + 4 * C * C) * A * A * A * A / 24 +
+                (61 - 58 * T + T * T + 600 * C - 330 * e2) * A * A * A * A * A *
+                    A / 720));
+
+  // Correct for southern hemisphere
+  if (lat < 0) {
+    northing += 10000000.0;
+  }
+
+  return std::make_tuple(easting, northing, zone);
 }
 
-inline std::tuple<double, double> gpsToGlobalCoord(double lat0, double lon0, double lat1, double lon1) {
-    // Convert latitude and longitude to utm coordinates
-    const auto& [e0, n0, zone0] = gpsToUTM(lat0, lon0);
-    const auto& [e1, n1, zone1] = gpsToUTM(lat1, lon1);
-    return {e1 - e0, n1 - n0};
+inline double gpsDistance(double lat0, double lon0, double lat1, double lon1) {
+  // Convert latitude and longitude to radians
+  double lat0_rad = lat0 * M_PI / 180.0;
+  double lon0_rad = lon0 * M_PI / 180.0;
+  double lat1_rad = lat1 * M_PI / 180.0;
+  double lon1_rad = lon1 * M_PI / 180.0;
+
+  // Haversine formula for distance between two GPS coordinates
+  double dlat = lat1_rad - lat0_rad;
+  double dlon = lon1_rad - lon0_rad;
+  double a = pow(sin(dlat / 2), 2) +
+             cos(lat0_rad) * cos(lat1_rad) * pow(sin(dlon / 2), 2);
+  double c = 2 * atan2(sqrt(a), sqrt(1 - a));
+  return 6371000 * c;  // Radius of Earth in meters
 }
 
-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;
+inline std::tuple<double, double> gpsToGlobalCoord(double lat0, double lon0,
+                                                   double lat1, double lon1) {
+  // Convert latitude and longitude to utm coordinates
+  const auto& [e0, n0, zone0] = gpsToUTM(lat0, lon0);
+  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;
 }
 
+}  // namespace gps_util
+
 #endif