CARLA
Loading...
Searching...
No Matches
GeoProjection.cpp
Go to the documentation of this file.
1// Copyright (c) 2026 Computer Vision Center (CVC) at the Universitat Autonoma
2// de Barcelona (UAB).
3//
4// This work is licensed under the terms of the MIT license.
5// For a copy, see <https://opensource.org/licenses/MIT>.
6
7#include <cmath>
8#include <cassert>
9
10#include "GeoProjection.h"
11
13#include "carla/geom/Location.h"
14#include "carla/geom/Math.h"
15
16namespace carla {
17namespace geom {
18
19 static double DegreesToRadians(double degrees) {
20 return degrees * Math::Pi<double>() / 180.0;
21 }
22
23 static double RadiansToDegrees(double radians) {
24 return radians * 180.0 / Math::Pi<double>();
25 }
26
27 // CARLA locations are left-handed (y towards the south, the OpenDRIVE y negated); the projections work in the
28 // right-handed OpenDRIVE inertial frame (y towards the north). The change is its own inverse.
29 static Location SwapHandedness(Location location) {
30 location.y = -location.y;
31 return location;
32 }
33
35 switch (static_cast<ProjectionType>(params.index())) {
36
38 auto& p = boost::variant2::get<TransverseMercatorParams>(params);
40 }
41
43 auto& p = boost::variant2::get<UniversalTransverseMercatorParams>(params);
45 }
46
48 auto& p = boost::variant2::get<WebMercatorParams>(params);
50 }
51
53 auto& p = boost::variant2::get<LambertConformalConicParams>(params);
55 }
56
57 default: {
58 auto& p = boost::variant2::get<TransverseMercatorParams>(params);
60 }
61 }
62 }
63
65 switch (static_cast<ProjectionType>(params.index())) {
67 auto& p = boost::variant2::get<TransverseMercatorParams>(params);
69 }
70
72 auto& p = boost::variant2::get<UniversalTransverseMercatorParams>(params);
74 }
75
77 auto& p = boost::variant2::get<WebMercatorParams>(params);
79 }
80
82 auto& p = boost::variant2::get<LambertConformalConicParams>(params);
84 }
85
86 default: {
87 auto& p = boost::variant2::get<TransverseMercatorParams>(params);
89 }
90 }
91 }
92
94 const GeoLocation& geolocation, const TransverseMercatorParams p) const {
95
96 // Using Snyder TM forward (ellipsoidal) to 6th order
97 double lat = DegreesToRadians(geolocation.latitude);
98 double lon = DegreesToRadians(geolocation.longitude);
99 double lat_0 = DegreesToRadians(p.lat_0);
100 double lon_0 = DegreesToRadians(p.lon_0);
101
102 double dlon = std::atan2(std::sin(lon - lon_0), std::cos(lon - lon_0));
103
104 double a = p.ellps.a;
105 double e2 = p.ellps.e2();
106 double ep2 = p.ellps.ep2();
107 double e4 = e2 * e2;
108 double e6 = e4 * e2;
109
110 double N = a / std::sqrt(1.0 - e2 * std::sin(lat) * std::sin(lat));
111 double T = std::tan(lat) * std::tan(lat);
112 double C = ep2 * std::cos(lat) * std::cos(lat);
113 double A = std::cos(lat) * dlon;
114
115 auto meridional_arc = [&](double phi) {
116 return p.ellps.a * ((1.0 - e2 / 4.0 - 3.0 * e4 / 64.0 - 5.0 * e6 / 256.0) * phi
117 - (3.0 * e2 / 8.0 + 3.0 * e4 / 32.0 + 45.0 * e6 / 1024.0) * std::sin(2.0 * phi)
118 + (15.0 * e4 / 256.0 + 45.0 * e6 / 1024.0) * std::sin(4.0 * phi)
119 - (35.0 * e6 / 3072.0) * std::sin(6.0 * phi));
120 };
121
122 double M = meridional_arc(lat);
123 double M_0 = meridional_arc(lat_0);
124
125 double x = p.x_0 + p.k * N * (A + (1.0 - T + C) * std::pow(A, 3) / 6.0
126 + (5.0 - 18.0 * T + T * T + 72.0 * C - 58.0 * ep2) * std::pow(A, 5) / 120.0);
127
128 double y = p.y_0 + p.k * ((M - M_0) + N * std::tan(lat) * ((A*A) / 2.0
129 + (5.0 - T + 9.0 * C + 4.0 * C * C) * std::pow(A, 4) / 24.0
130 + (61.0 - 58.0 * T + T * T + 600.0 * C - 330.0 * ep2) * std::pow(A, 6) / 720.0));
131
132 return Location(static_cast<float>(x), static_cast<float>(y), static_cast<float>(geolocation.altitude));
133 }
134
136 const GeoLocation& geolocation, const UniversalTransverseMercatorParams p) const {
137
138 // Using Snyder TM forward (ellipsoidal) to 6th order. Same formula as Transverse Mercator.
139 double lat = DegreesToRadians(geolocation.latitude);
140 double lon = DegreesToRadians(geolocation.longitude);
141 double lon_0 = DegreesToRadians(6 * p.zone - 183);
142 double k = 0.9996;
143 double x_0 = 500000.0;
144 double y_0 = (p.north) ? 0.0 : 10000000.0;
145
146 double a = p.ellps.a;
147 double e2 = p.ellps.e2();
148 double ep2 = p.ellps.ep2();
149 double e4 = e2 * e2;
150 double e6 = e4 * e2;
151
152 double N = a / std::sqrt(1.0 - e2 * std::sin(lat) * std::sin(lat));
153 double T = std::tan(lat) * std::tan(lat);
154 double C = ep2 * std::cos(lat) * std::cos(lat);
155 double A = std::cos(lat) * std::atan2(std::sin(lon - lon_0), std::cos(lon - lon_0));
156
157 double M = p.ellps.a * ((1.0 - e2 / 4.0 - 3.0 * e4 / 64.0 - 5.0 * e6 / 256.0) * lat
158 - (3.0 * e2 / 8.0 + 3.0 * e4 / 32.0 + 45.0 * e6 / 1024.0) * std::sin(2.0 * lat)
159 + (15.0 * e4 / 256.0 + 45.0 * e6 / 1024.0) * std::sin(4.0 * lat)
160 - (35.0 * e6 / 3072.0) * std::sin(6.0 * lat));
161
162 double x = x_0 + k * N * (A + (1.0 - T + C) * std::pow(A, 3) / 6.0
163 + (5.0 - 18.0 * T + T * T + 72.0 * C - 58.0 * ep2) * std::pow(A, 5) / 120.0);
164
165 double y = y_0 + k * (M + N * std::tan(lat) * ((A * A) / 2.0
166 + (5.0 - T + 9.0 * C + 4.0 * C * C) * std::pow(A, 4) / 24.0
167 + (61.0 - 58.0 * T + T * T + 600.0 * C - 330.0 * ep2) * std::pow(A, 6) / 720.0));
168
169 Location result(static_cast<float>(x), static_cast<float>(y), static_cast<float>(geolocation.altitude));
170
171 // The inverse transform applies p.offset to the input location before
172 // un-projecting; mirror it here with the inverse offset so a
173 // geo -> location -> geo round-trip stays the identity.
174 if (p.offset.has_value()) {
175 result = p.offset->ApplyInverseTransformation(result);
176 }
177
178 return result;
179 }
180
182 const GeoLocation& geolocation, const WebMercatorParams p) const {
183
184 double lat = DegreesToRadians(geolocation.latitude);
185 double lon = DegreesToRadians(geolocation.longitude);
186
187 double x = p.ellps.a * lon;
188 double y = p.ellps.a * std::log(std::tan(Math::Pi<double>() / 4.0 + lat / 2.0));
189
190 return Location(static_cast<float>(x), static_cast<float>(y), static_cast<float>(geolocation.altitude));
191 }
192
194 const GeoLocation& geolocation, const LambertConformalConicParams p) const {
195
196 double lat = DegreesToRadians(geolocation.latitude);
197 double lon = DegreesToRadians(geolocation.longitude);
198 double lon_0 = DegreesToRadians(p.lon_0);
199 double lat_0 = DegreesToRadians(p.lat_0);
200 double lat_1 = DegreesToRadians(p.lat_1);
201 double lat_2 = DegreesToRadians(p.lat_2);
202
203 double a = p.ellps.a;
204 double e2 = p.ellps.e2();
205 double e = std::sqrt(e2);
206
207 double m1 = std::cos(lat_1) / std::sqrt(1.0 - e2 * std::sin(lat_1) * std::sin(lat_1));
208 double m2 = std::cos(lat_2) / std::sqrt(1.0 - e2 * std::sin(lat_2) * std::sin(lat_2));
209
210 double t0 = std::tan(Math::Pi<double>() / 4 - lat_0 / 2)
211 / std::pow((1.0 - e * std::sin(lat_0)) / (1.0 + e * std::sin(lat_0)), e / 2);
212 double t1 = std::tan(Math::Pi<double>() / 4- lat_1 / 2)
213 / std::pow((1.0 - e * std::sin(lat_1)) / (1.0 + e * std::sin(lat_1)), e / 2);
214 double t2 = std::tan(Math::Pi<double>() / 4 - lat_2 / 2)
215 / std::pow((1.0 - e * std::sin(lat_2)) / (1.0 + e * std::sin(lat_2)), e / 2);
216 double t = std::tan(Math::Pi<double>() / 4 - lat / 2)
217 / std::pow((1.0 - e * std::sin(lat)) / (1.0 + e * std::sin(lat)), e / 2);
218
219 double n = (std::log(m1) - std::log(m2)) / (std::log(t1) - std::log(t2));
220 double F = m1 / (n * std::pow(t1, n));
221 double rho = a * F * std::pow(t, n);
222 double rho0 = a * F * std::pow(t0, n);
223 double theta = n * std::atan2(std::sin(lon - lon_0), std::cos(lon - lon_0));
224
225 double x = p.x_0 + rho * std::sin(theta);
226 double y = p.y_0 + rho0 - rho * std::cos(theta);
227
228 return Location(static_cast<float>(x), static_cast<float>(y), static_cast<float>(geolocation.altitude));
229 }
230
232 const Location& location, const TransverseMercatorParams p) const {
233
234 // Using Snyder TM inverse (ellipsoidal) to 6th order
235 double lat_0 = DegreesToRadians(p.lat_0);
236 double lon_0 = DegreesToRadians(p.lon_0);
237
238 double a = p.ellps.a;
239 double e2 = p.ellps.e2();
240 double ep2 = p.ellps.ep2();
241 double e4 = e2 * e2;
242 double e6 = e4 * e2;
243
244 double x = (location.x - p.x_0) / p.k;
245 double y = (location.y - p.y_0) / p.k;
246
247 double M = a * ((1.0 - e2 / 4.0 - 3.0 * e4 / 64.0 - 5.0 * e6 / 256.0) * lat_0
248 - (3.0 * e2 / 8.0 + 3.0 * e4 / 32.0 + 45.0 * e6 / 1024.0) * std::sin(2.0 * lat_0)
249 + (15.0 * e4 / 256.0 + 45.0 * e6 / 1024.0) * std::sin(4.0 * lat_0)
250 - (35.0 * e6 / 3072.0) * std::sin(6.0 * lat_0)) + y;
251
252 double mu = M / (a * (1.0 - e2 / 4.0 - 3.0 * e4 / 64.0 - 5.0 * e6 / 256.0));
253 double e1 = (1.0 - std::sqrt(1.0 - e2)) / (1.0 + std::sqrt(1.0 - e2));
254 double e1_2 = e1 * e1;
255 double e1_3 = e1_2 * e1;
256 double e1_4 = e1_3 * e1;
257
258 double phi1 = mu + (3.0 * e1 / 2.0 - 27.0 * e1_3 / 32.0) * std::sin(2.0 * mu)
259 + (21.0 * e1_2 / 16.0 - 55.0 * e1_4 / 32.0) * std::sin(4.0 * mu)
260 + (151.0 * e1_3 / 96.0) * std::sin(6.0 * mu) + (1097.0 * e1_4 / 512.0) * std::sin(8.0 * mu);
261
262 double sin1 = std::sin(phi1), cos1 = std::cos(phi1), tan1 = std::tan(phi1);
263
264 double N = a / std::sqrt(1.0 - e2 * sin1 * sin1);
265 double R = a * (1.0 - e2) / std::pow(1.0 - e2 * sin1 * sin1, 1.5);
266 double T = tan1 * tan1;
267 double C = ep2 * cos1 * cos1;
268 double D = x / N;
269
270 // Snyder TM inverse (to 6th order)
271 double lat = phi1 - (N * tan1 / R) * ((D * D) / 2.0
272 - (5.0 + 3.0 * T + 10.0 * C - 4.0 * C * C - 9.0 * ep2) * std::pow(D, 4) / 24.0
273 + (61.0 + 90.0 * T + 298.0 * C + 45.0 * T * T - 252.0 * ep2 - 3.0 * C * C) * std::pow(D, 6) / 720.0);
274
275 double lon = lon_0 + (D - (1.0 + 2.0 * T + C) * std::pow(D, 3) / 6.0
276 + (5.0 - 2.0 * C + 28.0 * T + 3.0 * C * C + 8.0 * ep2 + 24.0 * T * T) * std::pow(D, 5) / 120.0) / cos1;
277
278 lon = std::atan2(std::sin(lon), std::cos(lon));
279
280 return GeoLocation(RadiansToDegrees(lat), RadiansToDegrees(lon), location.z);
281 }
282
284 const Location& location_in, const UniversalTransverseMercatorParams p) const {
285
286 // Using Snyder TM inverse (ellipsoidal) to 6th order. Same formula as Transverse Mercator.
287 double lon_0 = DegreesToRadians(6 * p.zone - 183); // central meridian
288 double k = 0.9996;
289 double x_0 = 500000.0;
290 double y_0 = (p.north) ? 0.0 : 10000000.0;
291
292 double a = p.ellps.a;
293 double e2 = p.ellps.e2();
294 double ep2 = p.ellps.ep2();
295 double e4 = e2 * e2;
296 double e6 = e4 * e2;
297
298 Location location;
299 if (p.offset.has_value()) {
300 location = p.offset->ApplyTransformation(location_in);
301 }
302 else{
303 location = location_in;
304 }
305
306 //Negate the value of y because of the unreal left hand rule
307 double x = (location.x - x_0) / k;
308 double y = (location.y - y_0) / k;
309
310 double mu = y / (a * (1.0 - e2 / 4.0 - 3.0 * e4 / 64.0 - 5.0 * e6 / 256.0));
311 double e1 = (1.0 - std::sqrt(1.0 - e2)) / (1.0 + std::sqrt(1.0 - e2));
312 double e1_2 = e1 * e1;
313 double e1_3 = e1_2 * e1;
314 double e1_4 = e1_3 * e1;
315
316 double phi1 = mu + (3.0 * e1 / 2.0 - 27.0 * e1_3 / 32.0) * std::sin(2.0 * mu)
317 + (21.0 * e1_2 / 16.0 - 55.0 * e1_4 / 32.0) * std::sin(4.0 * mu)
318 + (151.0 * e1_3 / 96.0) * std::sin(6.0 * mu) + (1097.0 * e1_4 / 512.0) * std::sin(8.0 * mu);
319
320 double sin1 = std::sin(phi1);
321 double cos1 = std::cos(phi1);
322 double tan1 = std::tan(phi1);
323
324 double N = a / std::sqrt(1.0 - e2 * sin1 * sin1);
325 double R = a * (1.0 - e2) / std::pow(1.0 - e2 * sin1 * sin1, 1.5);
326 double T = tan1 * tan1;
327 double C = ep2 * cos1 * cos1;
328 double D = x / N;
329
330 double lat = phi1 - (N * tan1 / R) * ((D * D) / 2.0
331 - (5.0 + 3.0 * T + 10.0 * C - 4.0 * C * C - 9.0 * ep2) * std::pow(D, 4) / 24.0
332 + (61.0 + 90.0 * T + 298.0 * C + 45.0 * T * T - 252.0 * ep2 - 3.0 * C * C) * std::pow(D, 6) / 720.0);
333
334 double lon = lon_0 + (D - (1.0 + 2.0 * T + C) * std::pow(D, 3) / 6.0
335 + (5.0 - 2.0 * C + 28.0 * T + 3.0 * C * C + 8.0 * ep2 + 24.0 * T * T) * std::pow(D, 5) / 120.0) / cos1;
336
337 lon = std::atan2(std::sin(lon), std::cos(lon));
338
339 return GeoLocation(RadiansToDegrees(lat), RadiansToDegrees(lon), location.z);
340 }
341
343 const Location& location, const WebMercatorParams p) const {
344
345 double lon = location.x / p.ellps.a;
346 double lat = 2*std::atan(std::exp(location.y / p.ellps.a)) - Math::Pi<double>()/2;
347
348 return GeoLocation(RadiansToDegrees(lat), RadiansToDegrees(lon), location.z);
349 }
350
352 const Location& location, const LambertConformalConicParams p) const {
353
354 double lon_0 = DegreesToRadians(p.lon_0);
355 double lat_1 = DegreesToRadians(p.lat_1);
356 double lat_2 = DegreesToRadians(p.lat_2);
357 double lat_0 = DegreesToRadians(p.lat_0);
358
359 double a = p.ellps.a;
360 double e2 = p.ellps.e2();
361 double e = std::sqrt(e2);
362
363 double m1 = std::cos(lat_1) / std::sqrt(1.0 - e2 * std::sin(lat_1) * std::sin(lat_1));
364 double m2 = std::cos(lat_2) / std::sqrt(1.0 - e2 * std::sin(lat_2) * std::sin(lat_2));
365
366 double t0 = std::tan(Math::Pi<double>() / 4 - lat_0 / 2)
367 / std::pow((1.0 - e * std::sin(lat_0)) / (1.0 + e * std::sin(lat_0)), e / 2);
368 double t1 = std::tan(Math::Pi<double>() / 4- lat_1 / 2)
369 / std::pow((1.0 - e * std::sin(lat_1)) / (1.0 + e * std::sin(lat_1)), e / 2);
370 double t2 = std::tan(Math::Pi<double>() / 4 - lat_2 / 2)
371 / std::pow((1.0 - e * std::sin(lat_2)) / (1.0 + e * std::sin(lat_2)), e / 2);
372
373 double n = (std::log(m1) - std::log(m2)) / (std::log(t1) - std::log(t2)); // cone constant
374 double F = m1 / (n * std::pow(t1, n));
375 double rho0 = a * F * std::pow(t0, n);
376
377 double x = static_cast<double>(location.x) - p.x_0;
378 double y = static_cast<double>(location.y) - p.y_0;
379
380 double sgn = (n >= 0.0) ? 1.0 : -1.0;
381 double Y = rho0 - y;
382 double rho = sgn * std::hypot(x, Y);
383 double theta = std::atan2(sgn * x, sgn * Y);
384
385 double t = std::pow(rho / (a * F), 1.0 / n);
386
387 // initial spherical guess
388 double lat = Math::Pi<double>() * 0.5 - 2.0 * std::atan(t);
389 for (int i = 0; i < 10; ++i) {
390 const double lat_next = Math::Pi<double>() * 0.5
391 - 2.0 * std::atan(t * std::pow((1.0 - e * std::sin(lat)) / (1.0 + e * std::sin(lat)), 0.5 * e));
392 if (std::abs(lat_next - lat) < 1e-12){
393 lat = lat_next; break;
394 }
395 lat = lat_next;
396 }
397 double lon = lon_0 + theta / n;
398 lon = std::atan2(std::sin(lon), std::cos(lon));
399
400 return GeoLocation(RadiansToDegrees(lat), RadiansToDegrees(lon), location.z);
401 }
402} // namespace geom
403} // namespace carla
float y
float x
carla::rpc::Response< T > R
static Location SwapHandedness(Location location)
static double RadiansToDegrees(double radians)
ProjectionType
All the supported projection types.
static double DegreesToRadians(double degrees)
This file contains definitions of common data structures used in traffic manager.
GeoLocation TransformToGeoLocationUniversalTransverseMercator(const Location &location, const UniversalTransverseMercatorParams) const
Transform the given location to a geo location using UniversalTransverseMercator.
Location GeoLocationToTransformUniversalTransverseMercator(const GeoLocation &geolocation, const UniversalTransverseMercatorParams params) const
Transform the given geo location to a location using UniversalTransverseMercator.
GeoLocation TransformToGeoLocationLambertConformalConic(const Location &location, const LambertConformalConicParams params) const
Transform the given location to a geo location using LambertConformalConic.
GeoLocation TransformToGeoLocationTransverseMercator(const Location &location, const TransverseMercatorParams params) const
Transform the given location to a geo location using TransverseMercator.
Location GeoLocationToTransformLambertConformalConic(const GeoLocation &geolocation, const LambertConformalConicParams params) const
Transform the given geo location to a location using LambertConformalConic.
Location GeoLocationToTransform(const GeoLocation &geolocation) const
Transform the given location to a geo location.
GeoLocation TransformToGeoLocationWebMercator(const Location &location, const WebMercatorParams params) const
Transform the given location to a geo location using WebMercator.
Location GeoLocationToTransformTransverseMercator(const GeoLocation &geolocation, const TransverseMercatorParams params) const
Transform the given geo location to a location using TransverseMercator.
GeoLocation TransformToGeoLocation(const Location &location) const
Transform the given geo location to a location.
Location GeoLocationToTransformWebMercator(const GeoLocation &geolocation, const WebMercatorParams params) const
Transform the given geo location to a location using WebMercator.