Skip to content

GeoProjection: Fix the handling of left handed Unreal location values #9590

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
berndgassmann wants to merge 1 commit into
base: ue4-dev
Choose a base branch
from
Open

GeoProjection: Fix the handling of left handed Unreal location values #9590

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions CHANGELOG.md
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,5 @@
## Latest Changes
* Fixed North/South latitude inversion in geo-coordinate conversion for Transverse Mercator and UTM projections
* Fixed all 282 compiler warnings in LibCarla across 27 files (unused variables, missing braces, narrowing conversions, implicit casts, initializer order, etc.) establishing a zero-warning baseline for server and client release builds
* Fixed ROS2 native sensors not streaming data when no Python client is listening for UE4, caused by a missing ROS2 enablement check in AreClientsListening() after the multistream refactor (PR #9431)
* Fix possible RPC Server deadlock on shutdown
Expand Down
85 changes: 48 additions & 37 deletions LibCarla/source/carla/geom/GeoProjection.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,11 +11,11 @@ namespace carla {
namespace geom {

static double DegreesToRadians(double degrees) {
return degrees * Math::Pi<double>() / 180.0;
return degrees * Math::Pi<double>() / 180.0;
}

static double RadiansToDegrees(double radians) {
return radians * 180.0 / Math::Pi<double>();
return radians * 180.0 / Math::Pi<double>();
}

Location GeoProjection::GeoLocationToTransform(const GeoLocation& geolocation) const {
Expand Down Expand Up @@ -48,31 +48,31 @@ namespace geom {
}
}

GeoLocation GeoProjection::TransformToGeoLocation(const Location& location) const {
GeoLocation GeoProjection::TransformToGeoLocation(const Location& location_lh) const {
switch (static_cast<ProjectionType>(params.index())) {
case ProjectionType::TransverseMercator: {
auto& p = boost::variant2::get<TransverseMercatorParams>(params);
return TransformToGeoLocationTransverseMercator(location, p);
return TransformToGeoLocationTransverseMercator(location_lh, p);
}

case ProjectionType::UniversalTransverseMercator: {
auto& p = boost::variant2::get<UniversalTransverseMercatorParams>(params);
return TransformToGeoLocationUniversalTransverseMercator(location, p);
return TransformToGeoLocationUniversalTransverseMercator(location_lh, p);
}

case ProjectionType::WebMercator: {
auto& p = boost::variant2::get<WebMercatorParams>(params);
return TransformToGeoLocationWebMercator(location, p);
return TransformToGeoLocationWebMercator(location_lh, p);
}

case ProjectionType::LambertConformalConic:{
auto& p = boost::variant2::get<LambertConformalConicParams>(params);
return TransformToGeoLocationLambertConformalConic(location, p);
return TransformToGeoLocationLambertConformalConic(location_lh, p);
}

default: {
auto& p = boost::variant2::get<TransverseMercatorParams>(params);
return TransformToGeoLocationTransverseMercator(location, p);
return TransformToGeoLocationTransverseMercator(location_lh, p);
}
}
}
Expand Down Expand Up @@ -116,7 +116,7 @@ namespace geom {
+ (5.0 - T + 9.0 * C + 4.0 * C * C) * std::pow(A, 4) / 24.0
+ (61.0 - 58.0 * T + T * T + 600.0 * C - 330.0 * ep2) * std::pow(A, 6) / 720.0));

return Location(static_cast<float>(x), static_cast<float>(y), static_cast<float>(geolocation.altitude));
return LocationRightHanded(x, y, geolocation.altitude);
}

Location GeoProjection::GeoLocationToTransformUniversalTransverseMercator(
Expand Down Expand Up @@ -153,7 +153,7 @@ namespace geom {
+ (5.0 - T + 9.0 * C + 4.0 * C * C) * std::pow(A, 4) / 24.0
+ (61.0 - 58.0 * T + T * T + 600.0 * C - 330.0 * ep2) * std::pow(A, 6) / 720.0));

return Location(static_cast<float>(x), static_cast<float>(y), static_cast<float>(geolocation.altitude));
return LocationRightHanded(x, y, geolocation.altitude);
}

Location GeoProjection::GeoLocationToTransformWebMercator(
Expand All @@ -165,7 +165,7 @@ namespace geom {
double x = p.ellps.a * lon;
double y = p.ellps.a * std::log(std::tan(Math::Pi<double>() / 4.0 + lat / 2.0));

return Location(static_cast<float>(x), static_cast<float>(y), static_cast<float>(geolocation.altitude));
return LocationRightHanded(x, y, geolocation.altitude);
}

Location GeoProjection::GeoLocationToTransformLambertConformalConic(
Expand Down Expand Up @@ -203,11 +203,16 @@ namespace geom {
double x = p.x_0 + rho * std::sin(theta);
double y = p.y_0 + rho0 - rho * std::cos(theta);

return Location(static_cast<float>(x), static_cast<float>(y), static_cast<float>(geolocation.altitude));
return LocationRightHanded(x, y, geolocation.altitude);
}

GeoLocation GeoProjection::TransformToGeoLocationTransverseMercator(
const Location& location, const TransverseMercatorParams p) const {
const Location& location_lh, const TransverseMercatorParams p) const {

LocationRightHanded location_rh(location_lh);
if (p.offset.has_value()) {
location_rh = LocationRightHanded(p.offset->ApplyTransformation(location_lh));
}

// Using Snyder TM inverse (ellipsoidal) to 6th order
double lat_0 = DegreesToRadians(p.lat_0);
Expand All @@ -219,8 +224,8 @@ namespace geom {
double e4 = e2 * e2;
double e6 = e4 * e2;

double x = (location.x - p.x_0) / p.k;
double y = (location.y - p.y_0) / p.k;
double x = (location_rh.x - p.x_0) / p.k;
double y = (location_rh.y - p.y_0) / p.k;

double M = a * ((1.0 - e2 / 4.0 - 3.0 * e4 / 64.0 - 5.0 * e6 / 256.0) * lat_0
- (3.0 * e2 / 8.0 + 3.0 * e4 / 32.0 + 45.0 * e6 / 1024.0) * std::sin(2.0 * lat_0)
Expand Down Expand Up @@ -255,11 +260,16 @@ namespace geom {

lon = std::atan2(std::sin(lon), std::cos(lon));

return GeoLocation(RadiansToDegrees(lat), RadiansToDegrees(lon), location.z);
return GeoLocation(RadiansToDegrees(lat), RadiansToDegrees(lon), location_rh.z);
}

GeoLocation GeoProjection::TransformToGeoLocationUniversalTransverseMercator(
const Location& location_in, const UniversalTransverseMercatorParams p) const {
const Location& location_lh, const UniversalTransverseMercatorParams p) const {

LocationRightHanded location_rh(location_lh);
if (p.offset.has_value()) {
location_rh = LocationRightHanded(p.offset->ApplyTransformation(location_lh));
}

// Using Snyder TM inverse (ellipsoidal) to 6th order. Same formula as Transverse Mercator.
double lon_0 = DegreesToRadians(6 * p.zone - 183); // central meridian
Expand All @@ -273,17 +283,8 @@ namespace geom {
double e4 = e2 * e2;
double e6 = e4 * e2;

Location location;
if (p.offset.has_value()) {
location = p.offset->ApplyTransformation(location_in);
}
else{
location = location_in;
}

//Negate the value of y because of the unreal left hand rule
double x = (location.x - x_0) / k;
double y = (location.y - y_0) / k;
double x = (location_rh.x - x_0) / k;
double y = (location_rh.y - y_0) / k;

double mu = y / (a * (1.0 - e2 / 4.0 - 3.0 * e4 / 64.0 - 5.0 * e6 / 256.0));
double e1 = (1.0 - std::sqrt(1.0 - e2)) / (1.0 + std::sqrt(1.0 - e2));
Expand Down Expand Up @@ -314,20 +315,30 @@ namespace geom {

lon = std::atan2(std::sin(lon), std::cos(lon));

return GeoLocation(RadiansToDegrees(lat), RadiansToDegrees(lon), location.z);;
return GeoLocation(RadiansToDegrees(lat), RadiansToDegrees(lon), location_rh.z);;
}

GeoLocation GeoProjection::TransformToGeoLocationWebMercator(
const Location& location, const WebMercatorParams p) const {
const Location& location_lh, const WebMercatorParams p) const {

double lon = location.x / p.ellps.a;
double lat = 2*std::atan(std::exp(location.y / p.ellps.a)) - Math::Pi<double>()/2;
LocationRightHanded location_rh(location_lh);
if (p.offset.has_value()) {
location_rh = LocationRightHanded(p.offset->ApplyTransformation(location_lh));
}

double lon = location_rh.x / p.ellps.a;
double lat = 2*std::atan(std::exp(location_rh.y / p.ellps.a)) - Math::Pi<double>()/2;

return GeoLocation(RadiansToDegrees(lat), RadiansToDegrees(lon), location.z);
return GeoLocation(RadiansToDegrees(lat), RadiansToDegrees(lon), location_rh.z);
}

GeoLocation GeoProjection::TransformToGeoLocationLambertConformalConic(
const Location& location, const LambertConformalConicParams p) const {
const Location& location_lh, const LambertConformalConicParams p) const {

LocationRightHanded location_rh(location_lh);
if (p.offset.has_value()) {
location_rh = LocationRightHanded(p.offset->ApplyTransformation(location_lh));
}

double lon_0 = DegreesToRadians(p.lon_0);
double lat_1 = DegreesToRadians(p.lat_1);
Expand All @@ -352,8 +363,8 @@ namespace geom {
double F = m1 / (n * std::pow(t1, n));
double rho0 = a * F * std::pow(t0, n);

double x = static_cast<double>(location.x) - p.x_0;
double y = static_cast<double>(location.y) - p.y_0;
double x = static_cast<double>(location_rh.x) - p.x_0;
double y = static_cast<double>(location_rh.y) - p.y_0;

double sgn = (n >= 0.0) ? 1.0 : -1.0;
double Y = rho0 - y;
Expand All @@ -375,7 +386,7 @@ namespace geom {
double lon = lon_0 + theta / n;
lon = std::atan2(std::sin(lon), std::cos(lon));

return GeoLocation(RadiansToDegrees(lat), RadiansToDegrees(lon), location.z);
return GeoLocation(RadiansToDegrees(lat), RadiansToDegrees(lon), location_rh.z);
}
} // namespace geom
} // namespace carla
67 changes: 49 additions & 18 deletions LibCarla/source/carla/geom/GeoProjectionsParams.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -64,29 +64,57 @@ namespace geom {
}
};

// Since geo projections are using right handed systems, we have to negate the
// y-coordinate of the Unreal Location values before performing the geo conversion
// This supporting class is performing this automatically for us.
class LocationRightHanded {
public:
LocationRightHanded(double const _x, double const _y, double const _z)
: x(_x)
, y(_y)
, z(_z)
{}

explicit LocationRightHanded(Location const &location)
: x(location.x)
, y(-location.y)
, z(location.z)
{}

operator Location() {
return Location(static_cast<float>(x),static_cast<float>(-y),static_cast<float>(z));
}

double x;
double y;
double z;
};

class OffsetTransform {
public:

OffsetTransform() = default;

OffsetTransform(double offset_x, double offset_y, double offset_z, double offset_hdg):
offset_x(offset_x), offset_y(offset_y), offset_z(offset_z), offset_cos_h(std::cos(-offset_hdg)), offset_sin_h(std::sin(-offset_hdg)) {}
offset_x(offset_x), offset_y(offset_y), offset_z(offset_z), offset_cos_h(std::cos(offset_hdg)), offset_sin_h(std::sin(offset_hdg)) {}

double offset_x = 0.0;
double offset_y = 0.0;
double offset_z = 0.0;
double offset_cos_h = 1.0;
double offset_sin_h = 0.0;

Location ApplyTransformation(const Location& location) const{
Location ApplyTransformation(const Location& location_lh) const{

LocationRightHanded location_rh(location_lh);

double tx = location.x + offset_x;
double ty = -location.y + offset_y;
double tx = location_rh.x + offset_x;
double ty = location_rh.y + offset_y;

double x_rot = (tx * offset_cos_h) - (ty * offset_sin_h);
double y_rot = (tx * offset_sin_h) + (ty * offset_cos_h);

return Location{static_cast<float>(x_rot), static_cast<float>(y_rot), static_cast<float>(location.z + offset_z)};
return LocationRightHanded{x_rot, y_rot, location_rh.z + offset_z};
}

bool operator==(const OffsetTransform& rhs) const {
Expand All @@ -99,24 +127,25 @@ namespace geom {

TransverseMercatorParams() = default;

TransverseMercatorParams(double lat_0, double lon_0, double k, double x_0, double y_0, Ellipsoid ellps):
lat_0(lat_0), lon_0(lon_0), k(k), x_0(x_0), y_0(y_0), ellps(ellps) {}
TransverseMercatorParams(double lat_0, double lon_0, double k, double x_0, double y_0, Ellipsoid ellps, boost::optional<OffsetTransform> offset = boost::none):
lat_0(lat_0), lon_0(lon_0), k(k), x_0(x_0), y_0(y_0), ellps(ellps), offset(offset) {}

double lat_0 = 0.0f;
double lon_0 = 0.0f;
double k = 1.0f;
double x_0 = 0.0f;
double y_0 = 0.0f;
Ellipsoid ellps = Ellipsoid();
boost::optional<OffsetTransform> offset;

bool operator==(const TransverseMercatorParams &rhs) const {
return (lat_0 == rhs.lat_0) && (lon_0 == rhs.lon_0) && (k == rhs.k)
&& (x_0 == rhs.x_0) && (y_0 == rhs.y_0) && (ellps == rhs.ellps);
&& (x_0 == rhs.x_0) && (y_0 == rhs.y_0) && (ellps == rhs.ellps) && (offset == rhs.offset);
}

bool operator!=(const TransverseMercatorParams &rhs) const {
return (lat_0 != rhs.lat_0) || (lon_0 != rhs.lon_0) || (k != rhs.k)
|| (x_0 != rhs.x_0) || (y_0 != rhs.y_0) || (ellps != rhs.ellps);
|| (x_0 != rhs.x_0) || (y_0 != rhs.y_0) || (ellps != rhs.ellps) || (offset != rhs.offset);
}
};

Expand All @@ -138,7 +167,7 @@ namespace geom {
}

bool operator!=(const UniversalTransverseMercatorParams &rhs) const {
return (zone != rhs.zone) || (north != rhs.north) || (ellps != rhs.ellps) || (offset == rhs.offset);
return (zone != rhs.zone) || (north != rhs.north) || (ellps != rhs.ellps) || (offset != rhs.offset);
}
};

Expand All @@ -148,17 +177,18 @@ namespace geom {

WebMercatorParams() = default;

WebMercatorParams(Ellipsoid ellps):
ellps(ellps) {}
WebMercatorParams(Ellipsoid ellps, boost::optional<OffsetTransform> offset = boost::none):
ellps(ellps), offset(offset) {}

Ellipsoid ellps = Ellipsoid();
boost::optional<OffsetTransform> offset;

bool operator==(const WebMercatorParams &rhs) const {
return (ellps == rhs.ellps);
return (ellps == rhs.ellps) && (offset == rhs.offset);
}

bool operator!=(const WebMercatorParams &rhs) const {
return (ellps != rhs.ellps);
return (ellps != rhs.ellps) || (offset != rhs.offset);
}
};

Expand All @@ -168,8 +198,8 @@ namespace geom {
LambertConformalConicParams() = default;

LambertConformalConicParams(
double lat_0, double lat_1, double lat_2, double lon_0, double x_0, double y_0, Ellipsoid ellps):
lat_0(lat_0), lat_1(lat_1), lat_2(lat_2), lon_0(lon_0), x_0(x_0), y_0(y_0), ellps(ellps) {}
double lat_0, double lat_1, double lat_2, double lon_0, double x_0, double y_0, Ellipsoid ellps, boost::optional<OffsetTransform> offset = boost::none):
lat_0(lat_0), lat_1(lat_1), lat_2(lat_2), lon_0(lon_0), x_0(x_0), y_0(y_0), ellps(ellps), offset(offset) {}

double lat_0 = 0.0;
double lat_1 = -5.0;
Expand All @@ -178,15 +208,16 @@ namespace geom {
double x_0 = 0.0;
double y_0 = 0.0;
Ellipsoid ellps = Ellipsoid();
boost::optional<OffsetTransform> offset;

bool operator==(const LambertConformalConicParams &rhs) const {
return (lat_0 == rhs.lat_0) && (lat_1 == rhs.lat_1) && (lat_2 == rhs.lat_2) && (lon_0 == rhs.lon_0)
&& (x_0 == rhs.x_0) && (y_0 == rhs.y_0) && (ellps == rhs.ellps);
&& (x_0 == rhs.x_0) && (y_0 == rhs.y_0) && (ellps == rhs.ellps) && (offset == rhs.offset);
}

bool operator!=(const LambertConformalConicParams &rhs) const {
return (lat_0 != rhs.lat_0) || (lat_1 != rhs.lat_1) || (lat_2 != rhs.lat_2) || (lon_0 != rhs.lon_0)
|| (x_0 != rhs.x_0) || (y_0 != rhs.y_0) || (ellps != rhs.ellps);
|| (x_0 != rhs.x_0) || (y_0 != rhs.y_0) || (ellps != rhs.ellps) || (offset != rhs.offset);
}
};

Expand Down
Loading