From 1966da780fc74f993088168ba0eb08fd1c670b18 Mon Sep 17 00:00:00 2001 From: artzha Date: Tue, 10 Dec 2024 23:51:42 -0600 Subject: [PATCH] Refactored gps translator for gps to map conversion option --- math/gps_util.h | 157 +++++++++++++++++++----------------------------- 1 file changed, 62 insertions(+), 95 deletions(-) diff --git a/math/gps_util.h b/math/gps_util.h index f7724cd..b5e87b2 100644 --- a/math/gps_util.h +++ b/math/gps_util.h @@ -11,6 +11,7 @@ #include "shared/math/math_util.h" #include "shared/util/helpers.h" +using Eigen::Affine2d; using Eigen::Rotation2Dd; using Eigen::Vector2d; using math_util::AngleMod; @@ -40,76 +41,6 @@ struct GPSPoint { double heading; // True North (degrees) }; -struct GPSTranslator { - GPSTranslator() : is_origin_set(false) {} - - 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); - is_origin_set = true; - return true; - } - - void SetOrigin(double latitude, double longitude, double orientation) { - gps_origin_latitude = latitude; - gps_origin_longitude = longitude; - map_orientation = orientation; - is_origin_set = true; - } - - Vector2d GPSToMetric(const double latitude, const double longitude) { - CHECK(is_origin_set); - 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) { - CHECK(is_origin_set); - 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; - bool is_origin_set; - - // 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; -}; - inline std::tuple gpsToUTM(double lat, double lon) { // Constants for UTM conversion constexpr double a = 6378137.0; // WGS-84 major axis @@ -203,37 +134,73 @@ inline double gpsToGlobalHeading(const GPSPoint& p0) { return AngleMod(heading); } -inline Eigen::Affine2f gpsToLocal(const GPSPoint& current, - const GPSPoint& goal) { - // Step 1: Convert the GPS coordinates to global coordinates - const auto& dvec = gpsToGlobalCoord(current, goal); +struct GPSTranslator { + // Convenience wrapper for GPS to map coordinate transformations + GPSTranslator() + : is_gps_origin_set(false), + is_map_origin_set(false), + use_map_as_origin(false) {} + + bool Initialized() const { return is_gps_origin_set && is_map_origin_set; } + + void SetReferenceFrame(const string& frame) { + if (frame == "map") { + use_map_as_origin = true; + } else if (frame == "utm") { + use_map_as_origin = false; + } else { + throw std::runtime_error("Invalid reference frame."); + } + } - // Step 2: Convert the current heading to radians and adjust for x-axis - // reference Since 0 degrees pogpsToGlobalHeadingints 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; + void SetGPSOrigin(const GPSPoint& p) { + gps_origin = p; + is_gps_origin_set = true; + T_initial_gps = Eigen::Affine2d::Identity(); + } - // Step 3: Rotate the translation vector to the robot's local frame - double local_x = dvec.x() * cos(-current_heading_rad) - - dvec.y() * sin(-current_heading_rad); - double local_y = dvec.x() * sin(-current_heading_rad) + - dvec.y() * cos(-current_heading_rad); + void SetMapOrigin(const Affine2d& p) { + T_initial_map = p; + is_map_origin_set = true; + } + + Eigen::Vector2d GpsToGlobalCoord(const GPSPoint& p1) { + if (!is_gps_origin_set || !is_map_origin_set) { + throw std::runtime_error("GPS or map origin not set."); + } - // Step 4: 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; + Eigen::Vector2d gps_loc = gpsToGlobalCoord(gps_origin, p1); + Eigen::Affine2d T_base_gps = Eigen::Rotation2Dd(gpsToGlobalHeading(p1)) * + Eigen::Translation2d(gps_loc); + if (use_map_as_origin) { + Eigen::Affine2d T_gps_map = T_initial_gps * T_initial_map.inverse(); + return (T_gps_map * T_base_gps).translation(); + } - // 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; + return gps_loc; + } - // Step 5: Create the affine transformation for the local pose of the goal - Eigen::Affine2f transform = Eigen::Translation2f(local_x, local_y) * - Eigen::Rotation2Df(local_heading); + double GpsToGlobalHeading(const GPSPoint& p0) { + if (!is_gps_origin_set || !is_map_origin_set) { + throw std::runtime_error("GPS or map origin not set."); + } + double heading_global = gpsToGlobalHeading(p0); + if (use_map_as_origin) { + Eigen::Affine2d T_gps_map = T_initial_gps * T_initial_map.inverse(); + Eigen::Rotation2Dd R_base_gps = Eigen::Rotation2Dd(heading_global); + Eigen::Rotation2Dd R_gps_map(T_gps_map.linear()); + return (R_gps_map * R_base_gps).angle(); + } + return heading_global; + } - return transform; -} + bool is_gps_origin_set; + bool is_map_origin_set; + bool use_map_as_origin; + GPSPoint gps_origin; + Affine2d T_initial_gps; + Affine2d T_initial_map; +}; } // namespace gps_util