Skip to content

Commit 81489dc

Browse files
berndgassmannberndgassmann
committed
GeoProjection: Fix the handling of left handed Unreal location values
Use a helper class LocationRightHanded to allow the implemenation of the GeoProjections to perform their projections in a right handed coordinate frame. The explicit handling using LocationRightHanded revealed error in OffsetTransform: result Location is now transformed back to Unreal coordinates and Rotation is fixed. Allow OpenDRIVE offsets for any type of projection Fix UniversalTransverseMercatorParams::operator!=()
1 parent 2605780 commit 81489dc

6 files changed

Lines changed: 196 additions & 66 deletions

File tree

‎CHANGELOG.md‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,5 @@
11
## Latest Changes
2+
* Fixed North/South latitude inversion in geo-coordinate conversion for Transverse Mercator and UTM projections
23
* 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
34
* 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)
45
* Fix possible RPC Server deadlock on shutdown

‎LibCarla/source/carla/geom/GeoProjection.cpp‎

Lines changed: 48 additions & 37 deletions
Original file line numberDiff line numberDiff line change
@@ -11,11 +11,11 @@ namespace carla {
1111
namespace geom {
1212

1313
static double DegreesToRadians(double degrees) {
14-
return degrees * Math::Pi<double>() / 180.0;
14+
return degrees * Math::Pi<double>() / 180.0;
1515
}
1616

1717
static double RadiansToDegrees(double radians) {
18-
return radians * 180.0 / Math::Pi<double>();
18+
return radians * 180.0 / Math::Pi<double>();
1919
}
2020

2121
Location GeoProjection::GeoLocationToTransform(const GeoLocation& geolocation) const {
@@ -48,31 +48,31 @@ namespace geom {
4848
}
4949
}
5050

51-
GeoLocation GeoProjection::TransformToGeoLocation(const Location& location) const {
51+
GeoLocation GeoProjection::TransformToGeoLocation(const Location& location_lh) const {
5252
switch (static_cast<ProjectionType>(params.index())) {
5353
case ProjectionType::TransverseMercator: {
5454
auto& p = boost::variant2::get<TransverseMercatorParams>(params);
55-
return TransformToGeoLocationTransverseMercator(location, p);
55+
return TransformToGeoLocationTransverseMercator(location_lh, p);
5656
}
5757

5858
case ProjectionType::UniversalTransverseMercator: {
5959
auto& p = boost::variant2::get<UniversalTransverseMercatorParams>(params);
60-
return TransformToGeoLocationUniversalTransverseMercator(location, p);
60+
return TransformToGeoLocationUniversalTransverseMercator(location_lh, p);
6161
}
6262

6363
case ProjectionType::WebMercator: {
6464
auto& p = boost::variant2::get<WebMercatorParams>(params);
65-
return TransformToGeoLocationWebMercator(location, p);
65+
return TransformToGeoLocationWebMercator(location_lh, p);
6666
}
6767

6868
case ProjectionType::LambertConformalConic:{
6969
auto& p = boost::variant2::get<LambertConformalConicParams>(params);
70-
return TransformToGeoLocationLambertConformalConic(location, p);
70+
return TransformToGeoLocationLambertConformalConic(location_lh, p);
7171
}
7272

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

119-
return Location(static_cast<float>(x), static_cast<float>(y), static_cast<float>(geolocation.altitude));
119+
return LocationRightHanded(x, y, geolocation.altitude);
120120
}
121121

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

156-
return Location(static_cast<float>(x), static_cast<float>(y), static_cast<float>(geolocation.altitude));
156+
return LocationRightHanded(x, y, geolocation.altitude);
157157
}
158158

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

168-
return Location(static_cast<float>(x), static_cast<float>(y), static_cast<float>(geolocation.altitude));
168+
return LocationRightHanded(x, y, geolocation.altitude);
169169
}
170170

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

206-
return Location(static_cast<float>(x), static_cast<float>(y), static_cast<float>(geolocation.altitude));
206+
return LocationRightHanded(x, y, geolocation.altitude);
207207
}
208208

209209
GeoLocation GeoProjection::TransformToGeoLocationTransverseMercator(
210-
const Location& location, const TransverseMercatorParams p) const {
210+
const Location& location_lh, const TransverseMercatorParams p) const {
211+
212+
LocationRightHanded location_rh(location_lh);
213+
if (p.offset.has_value()) {
214+
location_rh = LocationRightHanded(p.offset->ApplyTransformation(location_lh));
215+
}
211216

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

222-
double x = (location.x - p.x_0) / p.k;
223-
double y = (location.y - p.y_0) / p.k;
227+
double x = (location_rh.x - p.x_0) / p.k;
228+
double y = (location_rh.y - p.y_0) / p.k;
224229

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

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

258-
return GeoLocation(RadiansToDegrees(lat), RadiansToDegrees(lon), location.z);
263+
return GeoLocation(RadiansToDegrees(lat), RadiansToDegrees(lon), location_rh.z);
259264
}
260265

261266
GeoLocation GeoProjection::TransformToGeoLocationUniversalTransverseMercator(
262-
const Location& location_in, const UniversalTransverseMercatorParams p) const {
267+
const Location& location_lh, const UniversalTransverseMercatorParams p) const {
268+
269+
LocationRightHanded location_rh(location_lh);
270+
if (p.offset.has_value()) {
271+
location_rh = LocationRightHanded(p.offset->ApplyTransformation(location_lh));
272+
}
263273

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

276-
Location location;
277-
if (p.offset.has_value()) {
278-
location = p.offset->ApplyTransformation(location_in);
279-
}
280-
else{
281-
location = location_in;
282-
}
283-
284-
//Negate the value of y because of the unreal left hand rule
285-
double x = (location.x - x_0) / k;
286-
double y = (location.y - y_0) / k;
286+
double x = (location_rh.x - x_0) / k;
287+
double y = (location_rh.y - y_0) / k;
287288

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

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

317-
return GeoLocation(RadiansToDegrees(lat), RadiansToDegrees(lon), location.z);;
318+
return GeoLocation(RadiansToDegrees(lat), RadiansToDegrees(lon), location_rh.z);;
318319
}
319320

320321
GeoLocation GeoProjection::TransformToGeoLocationWebMercator(
321-
const Location& location, const WebMercatorParams p) const {
322+
const Location& location_lh, const WebMercatorParams p) const {
322323

323-
double lon = location.x / p.ellps.a;
324-
double lat = 2*std::atan(std::exp(location.y / p.ellps.a)) - Math::Pi<double>()/2;
324+
LocationRightHanded location_rh(location_lh);
325+
if (p.offset.has_value()) {
326+
location_rh = LocationRightHanded(p.offset->ApplyTransformation(location_lh));
327+
}
328+
329+
double lon = location_rh.x / p.ellps.a;
330+
double lat = 2*std::atan(std::exp(location_rh.y / p.ellps.a)) - Math::Pi<double>()/2;
325331

326-
return GeoLocation(RadiansToDegrees(lat), RadiansToDegrees(lon), location.z);
332+
return GeoLocation(RadiansToDegrees(lat), RadiansToDegrees(lon), location_rh.z);
327333
}
328334

329335
GeoLocation GeoProjection::TransformToGeoLocationLambertConformalConic(
330-
const Location& location, const LambertConformalConicParams p) const {
336+
const Location& location_lh, const LambertConformalConicParams p) const {
337+
338+
LocationRightHanded location_rh(location_lh);
339+
if (p.offset.has_value()) {
340+
location_rh = LocationRightHanded(p.offset->ApplyTransformation(location_lh));
341+
}
331342

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

355-
double x = static_cast<double>(location.x) - p.x_0;
356-
double y = static_cast<double>(location.y) - p.y_0;
366+
double x = static_cast<double>(location_rh.x) - p.x_0;
367+
double y = static_cast<double>(location_rh.y) - p.y_0;
357368

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

378-
return GeoLocation(RadiansToDegrees(lat), RadiansToDegrees(lon), location.z);
389+
return GeoLocation(RadiansToDegrees(lat), RadiansToDegrees(lon), location_rh.z);
379390
}
380391
} // namespace geom
381392
} // namespace carla

‎LibCarla/source/carla/geom/GeoProjectionsParams.h‎

Lines changed: 49 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -64,29 +64,57 @@ namespace geom {
6464
}
6565
};
6666

67+
// Since geo projections are using right handed systems, we have to negate the
68+
// y-coordinate of the Unreal Location values before performing the geo conversion
69+
// This supporting class is performing this automatically for us.
70+
class LocationRightHanded {
71+
public:
72+
LocationRightHanded(double const _x, double const _y, double const _z)
73+
: x(_x)
74+
, y(_y)
75+
, z(_z)
76+
{}
77+
78+
explicit LocationRightHanded(Location const &location)
79+
: x(location.x)
80+
, y(-location.y)
81+
, z(location.z)
82+
{}
83+
84+
operator Location() {
85+
return Location(static_cast<float>(x),static_cast<float>(-y),static_cast<float>(z));
86+
}
87+
88+
double x;
89+
double y;
90+
double z;
91+
};
92+
6793
class OffsetTransform {
6894
public:
6995

7096
OffsetTransform() = default;
7197

7298
OffsetTransform(double offset_x, double offset_y, double offset_z, double offset_hdg):
73-
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)) {}
99+
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)) {}
74100

75101
double offset_x = 0.0;
76102
double offset_y = 0.0;
77103
double offset_z = 0.0;
78104
double offset_cos_h = 1.0;
79105
double offset_sin_h = 0.0;
80106

81-
Location ApplyTransformation(const Location& location) const{
107+
Location ApplyTransformation(const Location& location_lh) const{
108+
109+
LocationRightHanded location_rh(location_lh);
82110

83-
double tx = location.x + offset_x;
84-
double ty = -location.y + offset_y;
111+
double tx = location_rh.x + offset_x;
112+
double ty = location_rh.y + offset_y;
85113

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

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

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

100128
TransverseMercatorParams() = default;
101129

102-
TransverseMercatorParams(double lat_0, double lon_0, double k, double x_0, double y_0, Ellipsoid ellps):
103-
lat_0(lat_0), lon_0(lon_0), k(k), x_0(x_0), y_0(y_0), ellps(ellps) {}
130+
TransverseMercatorParams(double lat_0, double lon_0, double k, double x_0, double y_0, Ellipsoid ellps, boost::optional<OffsetTransform> offset = boost::none):
131+
lat_0(lat_0), lon_0(lon_0), k(k), x_0(x_0), y_0(y_0), ellps(ellps), offset(offset) {}
104132

105133
double lat_0 = 0.0f;
106134
double lon_0 = 0.0f;
107135
double k = 1.0f;
108136
double x_0 = 0.0f;
109137
double y_0 = 0.0f;
110138
Ellipsoid ellps = Ellipsoid();
139+
boost::optional<OffsetTransform> offset;
111140

112141
bool operator==(const TransverseMercatorParams &rhs) const {
113142
return (lat_0 == rhs.lat_0) && (lon_0 == rhs.lon_0) && (k == rhs.k)
114-
&& (x_0 == rhs.x_0) && (y_0 == rhs.y_0) && (ellps == rhs.ellps);
143+
&& (x_0 == rhs.x_0) && (y_0 == rhs.y_0) && (ellps == rhs.ellps) && (offset == rhs.offset);
115144
}
116145

117146
bool operator!=(const TransverseMercatorParams &rhs) const {
118147
return (lat_0 != rhs.lat_0) || (lon_0 != rhs.lon_0) || (k != rhs.k)
119-
|| (x_0 != rhs.x_0) || (y_0 != rhs.y_0) || (ellps != rhs.ellps);
148+
|| (x_0 != rhs.x_0) || (y_0 != rhs.y_0) || (ellps != rhs.ellps) || (offset != rhs.offset);
120149
}
121150
};
122151

@@ -138,7 +167,7 @@ namespace geom {
138167
}
139168

140169
bool operator!=(const UniversalTransverseMercatorParams &rhs) const {
141-
return (zone != rhs.zone) || (north != rhs.north) || (ellps != rhs.ellps) || (offset == rhs.offset);
170+
return (zone != rhs.zone) || (north != rhs.north) || (ellps != rhs.ellps) || (offset != rhs.offset);
142171
}
143172
};
144173

@@ -148,17 +177,18 @@ namespace geom {
148177

149178
WebMercatorParams() = default;
150179

151-
WebMercatorParams(Ellipsoid ellps):
152-
ellps(ellps) {}
180+
WebMercatorParams(Ellipsoid ellps, boost::optional<OffsetTransform> offset = boost::none):
181+
ellps(ellps), offset(offset) {}
153182

154183
Ellipsoid ellps = Ellipsoid();
184+
boost::optional<OffsetTransform> offset;
155185

156186
bool operator==(const WebMercatorParams &rhs) const {
157-
return (ellps == rhs.ellps);
187+
return (ellps == rhs.ellps) && (offset == rhs.offset);
158188
}
159189

160190
bool operator!=(const WebMercatorParams &rhs) const {
161-
return (ellps != rhs.ellps);
191+
return (ellps != rhs.ellps) || (offset != rhs.offset);
162192
}
163193
};
164194

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

170200
LambertConformalConicParams(
171-
double lat_0, double lat_1, double lat_2, double lon_0, double x_0, double y_0, Ellipsoid ellps):
172-
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) {}
201+
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):
202+
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) {}
173203

174204
double lat_0 = 0.0;
175205
double lat_1 = -5.0;
@@ -178,15 +208,16 @@ namespace geom {
178208
double x_0 = 0.0;
179209
double y_0 = 0.0;
180210
Ellipsoid ellps = Ellipsoid();
211+
boost::optional<OffsetTransform> offset;
181212

182213
bool operator==(const LambertConformalConicParams &rhs) const {
183214
return (lat_0 == rhs.lat_0) && (lat_1 == rhs.lat_1) && (lat_2 == rhs.lat_2) && (lon_0 == rhs.lon_0)
184-
&& (x_0 == rhs.x_0) && (y_0 == rhs.y_0) && (ellps == rhs.ellps);
215+
&& (x_0 == rhs.x_0) && (y_0 == rhs.y_0) && (ellps == rhs.ellps) && (offset == rhs.offset);
185216
}
186217

187218
bool operator!=(const LambertConformalConicParams &rhs) const {
188219
return (lat_0 != rhs.lat_0) || (lat_1 != rhs.lat_1) || (lat_2 != rhs.lat_2) || (lon_0 != rhs.lon_0)
189-
|| (x_0 != rhs.x_0) || (y_0 != rhs.y_0) || (ellps != rhs.ellps);
220+
|| (x_0 != rhs.x_0) || (y_0 != rhs.y_0) || (ellps != rhs.ellps) || (offset != rhs.offset);
190221
}
191222
};
192223

0 commit comments

Comments
 (0)