Skip to content

Commit

Permalink
Refactored gps translator for gps to map conversion option
Browse files Browse the repository at this point in the history
  • Loading branch information
@artzha
artzha committed Dec 11, 2024
1 parent 0d47c47 commit 1966da7
Showing 1 changed file with 62 additions and 95 deletions.
157 changes: 62 additions & 95 deletions math/gps_util.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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<double, double, int> gpsToUTM(double lat, double lon) {
// Constants for UTM conversion
constexpr double a = 6378137.0; // WGS-84 major axis
Expand Down Expand Up @@ -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

Expand Down

0 comments on commit 1966da7

Please sign in to comment.