diff --git a/math/gps_util.h b/math/gps_util.h index 7ff65c7..12619ce 100644 --- a/math/gps_util.h +++ b/math/gps_util.h @@ -1,103 +1,202 @@ #ifndef GPS_UTIL_H_ #define GPS_UTIL_H_ -#include "eigen3/Eigen/Dense" #include + +#include +#include #include +#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 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((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 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((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 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 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 \ No newline at end of file