Aaron/addgeolocation to transform

CHANGELOG.md

@@ -47,6 +47,7 @@
  * Support of masked materials in instance segmentation, resulting in fine-grained annotations on e.g. leaves or fences (as in semantic segmentation)
  * Added API function `world.set_annotations_traverse_translucency` and implemented functionality to configure, whether depth and semantic + instance segmentation traverse translucent materials or not.
  * Fixed `frame`, `timestamp` and `transform` of `SensorData` not matching to the actually sent image for camera sensors.
+ * Added  "geolocation_to_transform" function to the Map object in the PythonAPI
 
 ## CARLA 0.9.15
 

LibCarla/source/carla/geom/GeoLocation.cpp

@@ -32,10 +32,18 @@ namespace geom {
     return std::cos(Math::ToRadians(lat));
   }
 
+  static double DegreesToRadians(double degrees) {
+    return degrees * Math::Pi<double>() / 180.0;
+  }
+
+  static double RadiansToDegrees(double radians) {
+    return radians * 180.0 / Math::Pi<double>();
+  }
+
   /// Converts lat/lon/scale to mx/my (mx/my in meters if correct scale
   /// is given).
-  template <class float_type>
-  static void LatLonToMercator(double lat, double lon, double scale, float_type &mx, float_type &my) {
+  template <class double_type>
+  static void LatLonToMercator(double lat, double lon, double scale, double_type &mx, double_type &my) {
     mx = scale * Math::ToRadians(lon) * EARTH_RADIUS_EQUA;
     my = scale * EARTH_RADIUS_EQUA * std::log(std::tan((90.0 + lat) * Math::Pi<double>() / 360.0));
   }
@@ -63,14 +71,60 @@ namespace geom {
     MercatorToLatLon(mx, my, scale, lat_end, lon_end);
   }
 
+  // Forward (spherical TM)
+  Location GeoLocation::GetTransversemercProjection(double lat, double lon, double alt) const {
+      const double R = EARTH_RADIUS_EQUA;
+      const double k0 = 0.9996;
+
+      double phi = DegreesToRadians(lat);
+      double lambda = DegreesToRadians(lon);
+      double phi0 = DegreesToRadians(latitude);
+      double lambda0 = DegreesToRadians(longitude);
+
+      double deltaLambda = lambda - lambda0;
+      double B = cos(phi) * sin(deltaLambda);
+
+      double x = 0.5 * R * k0 * log((1 + B) / (1 - B));
+      double y = k0 * R * (atan(tan(phi) / cos(deltaLambda)) - phi0);
+
+      return Location(x, y, altitude + alt);
+  }
+
+  // Inverse (matches above)
+  GeoLocation GeoLocation::InverseTransversemercProjection(double x, double y, double alt) const {
+      const double R = EARTH_RADIUS_EQUA;
+      const double k0 = 0.9996;
+
+      double phi0 = DegreesToRadians(latitude);
+      double lambda0 = DegreesToRadians(longitude);
+
+      double D = x / (k0 * R);
+      double E = (y / (k0 * R)) + phi0;
+
+      double phi = asin(sin(E) / cosh(D));
+      double deltaLambda = atan2(sinh(D), cos(E));
+
+      double lambda = lambda0 + deltaLambda;
+
+      double latDeg = RadiansToDegrees(phi);
+      double lonDeg = RadiansToDegrees(lambda);
+
+      return GeoLocation(latDeg, lonDeg, alt + altitude);
+  }
+
   GeoLocation GeoLocation::Transform(const Location &location) const {
-    GeoLocation result{0.0, 0.0, altitude + location.z};
-    LatLonAddMeters(
-        latitude, longitude,
-        location.x, -location.y,  // Invert y axis to have increasing latitudes northward
-        result.latitude, result.longitude);
-    return result;
+    return InverseTransversemercProjection(
+        location.x, location.y, location.z + altitude);
   }
 
+
+  Location GeoLocation::GeoLocationToTransform(double lat, double lon, double altitude) const {
+    return GetTransversemercProjection(lat, lon, altitude);
+  }
+
+  Location GeoLocation::GeoLocationToTransform(const GeoLocation other) const
+  {
+    return GeoLocationToTransform(other.latitude, other.longitude, other.altitude);
+  }
 } // namespace geom
 } // namespace carla

LibCarla/source/carla/geom/GeoLocation.h

@@ -12,10 +12,11 @@ namespace carla {
 namespace geom {
 
   class Location;
+  class Transform;
 
   class GeoLocation {
   public:
-
+    
     // =========================================================================
     // -- Public data members --------------------------------------------------
     // =========================================================================
@@ -37,6 +38,14 @@ namespace geom {
         longitude(longitude),
         altitude(altitude) {}
 
+    // Get Location in Transverse Mercator projection
+    // using as base the already defined latitude, longitude and altitude in the object
+    Location GetTransversemercProjection(double lat, double lon, double alt) const;
+
+    // Get GeoLocation Inversing Traverse Mercator projection
+    // using as base the already defined latitude, longitude and altitude in the object
+    GeoLocation InverseTransversemercProjection(double x, double y, double alt) const;
+
     // =========================================================================
     // -- Transform locations --------------------------------------------------
     // =========================================================================
@@ -45,6 +54,12 @@ namespace geom {
     /// geo-reference.
     GeoLocation Transform(const Location &location) const;
 
+    // Transform the given @a location to a GeoLocation using this as
+    // geo-reference.
+    Location GeoLocationToTransform(double lat, double lon, double altitude) const;
+
+    Location GeoLocationToTransform(const GeoLocation other) const;
+
     // =========================================================================
     // -- Comparison operators -------------------------------------------------
     // =========================================================================

PythonAPI/carla/source/libcarla/Map.cpp

@@ -72,6 +72,12 @@ static auto GetLaneValidities(const carla::client::Landmark &self){
   return result;
 }
 
+static carla::geom::Location ToTransform(
+    const carla::client::Map &self,
+    const carla::geom::GeoLocation& geo_location) {
+  return self.GetGeoReference().GeoLocationToTransform(geo_location);
+}
+
 static carla::geom::GeoLocation ToGeolocation(
     const carla::client::Map &self,
     const carla::geom::Location &location) {
@@ -163,6 +169,7 @@ void export_map() {
     .def("get_topology", &GetTopology)
     .def("generate_waypoints", CALL_RETURNING_LIST_1(cc::Map, GenerateWaypoints, double), (args("distance")))
     .def("transform_to_geolocation", &ToGeolocation, (arg("location")))
+    .def("geolocation_to_transform", &ToTransform, (arg("geo_location")))
     .def("to_opendrive", CALL_RETURNING_COPY(cc::Map, GetOpenDrive))
     .def("save_to_disk", &SaveOpenDriveToDisk, (arg("path")=""))
     .def("get_crosswalks", CALL_RETURNING_LIST(cc::Map, GetAllCrosswalkZones))

PythonAPI/test/smoke/test_geoconversion.py

@@ -0,0 +1,100 @@
+import carla
+import random
+import math
+
+from . import SmokeTest
+
+
+class TestGeoLocationConversion(SmokeTest):
+
+    def setUp(self):
+        super().setUp()
+        self.map = self.client.get_world().get_map()
+
+    def _assert_location_close(self, a, b, tol=0.1):
+        self.assertAlmostEqual(a.x, b.x, delta=tol)
+        self.assertAlmostEqual(a.y, b.y, delta=tol)
+        self.assertAlmostEqual(a.z, b.z, delta=tol)
+
+    def _assert_geolocation_close(self, a, b, latlon_tol=0.01, alt_tol=1.0):
+        # Latitudes must stay in range [-90, 90]
+        if not (-90.0 <= a.latitude <= 90.0 and -90.0 <= b.latitude <= 90.0):
+            raise ValueError(f"Latitude out of bounds: {a.latitude}, {b.latitude}")
+
+        # Longitudes must stay in range [-180, 180]
+        if not (-180.0 <= a.longitude <= 180.0 and -180.0 <= b.longitude <= 180.0):
+            raise ValueError(f"Longitude out of bounds: {a.longitude}, {b.longitude}")
+
+        lat_diff = abs(a.latitude - b.latitude)
+        lon_diff = abs(a.longitude - b.longitude)
+        alt_diff = abs(a.altitude - b.altitude)
+
+        # Catch hemisphere flips and wrap-around errors
+        if lat_diff > 90.0 or lon_diff > 180.0:
+            raise AssertionError(
+                f"Geo conversion failed: large discrepancy (lat diff = {lat_diff}, lon diff = {lon_diff})\n"
+                f"Original: lat={a.latitude}, lon={a.longitude}\n"
+                f"Back:     lat={b.latitude}, lon={b.longitude}"
+            )
+
+        self.assertLessEqual(lat_diff, latlon_tol, f"Latitude mismatch: {a.latitude} vs {b.latitude}")
+        self.assertLessEqual(lon_diff, latlon_tol, f"Longitude mismatch: {a.longitude} vs {b.longitude}")
+        self.assertLessEqual(alt_diff, alt_tol, f"Altitude mismatch: {a.altitude} vs {b.altitude}")
+
+    def test_location_to_geo_and_back(self):
+        print("TestGeoLocationConversion.test_location_to_geo_and_back")
+        for _ in range(10):
+            loc = carla.Location(
+                x=random.uniform(-500, 500),
+                y=random.uniform(-500, 500),
+                z=random.uniform(0, 20))
+            geo = self.map.transform_to_geolocation(loc)
+            loc2 = self.map.geolocation_to_transform(geo)
+            self._assert_location_close(loc, loc2)
+
+    def test_geo_to_location_and_back(self):
+        print("TestGeoLocationConversion.test_geo_to_location_and_back")
+
+        geo_origin = self.map.transform_to_geolocation(carla.Location(0, 0, 0))
+        lat0, lon0 = geo_origin.latitude, geo_origin.longitude
+
+        test_geos = [
+            carla.GeoLocation(lat0 + 0.5, lon0 + 0.5, 50.0),
+            carla.GeoLocation(lat0 - 0.5, lon0 - 0.5, -10.0),
+            carla.GeoLocation(lat0 + 1.0, lon0 + 1.0, 100.0),
+        ]
+
+        for geo in test_geos:
+            loc = self.map.geolocation_to_transform(geo)
+            geo2 = self.map.transform_to_geolocation(loc)
+            self._assert_geolocation_close(geo, geo2)
+
+    def test_relative_distance_preserved(self):
+        print("TestGeoLocationConversion.test_relative_distance_preserved")
+        loc1 = carla.Location(x=100, y=200, z=0)
+        loc2 = carla.Location(x=110, y=210, z=0)
+
+        geo1 = self.map.transform_to_geolocation(loc1)
+        geo2 = self.map.transform_to_geolocation(loc2)
+
+        geo_diff = math.sqrt(
+            (geo2.latitude - geo1.latitude) ** 2 +
+            (geo2.longitude - geo1.longitude) ** 2 +
+            (geo2.altitude - geo1.altitude) ** 2
+        )
+        self.assertGreater(geo_diff, 0.0)
+
+    def test_altitude_variation(self):
+        print("TestGeoLocationConversion.test_altitude_variation")
+        for z in [0.0, 50.0, 500.0, 1000.0, 3000.0]:
+            loc = carla.Location(x=10.0, y=10.0, z=z)
+            geo = self.map.transform_to_geolocation(loc)
+            loc2 = self.map.geolocation_to_transform(geo)
+            self.assertAlmostEqual(loc.z, loc2.z, delta=1.0)
+
+    def test_zero_conversion(self):
+        print("TestGeoLocationConversion.test_zero_conversion")
+        origin = carla.Location(0.0, 0.0, 0.0)
+        geo = self.map.transform_to_geolocation(origin)
+        loc_back = self.map.geolocation_to_transform(geo)
+        self._assert_location_close(origin, loc_back)

PythonAPI/test/smoke_test_list.txt

@@ -1 +1 @@
-smoke.test_client smoke.test_sync smoke.test_sensor_determinism smoke.test_collision_determinism smoke.test_vehicle_physics smoke.test_props_loading smoke.test_sensor_tick_time smoke.test_map smoke.test_snapshot smoke.test_lidar smoke.test_streamming smoke.test_spawnpoints smoke.test_blueprint smoke.test_collision_sensor smoke.test_world smoke.test_determinism
+smoke.test_client smoke.test_sync smoke.test_sensor_determinism smoke.test_collision_determinism smoke.test_vehicle_physics smoke.test_props_loading smoke.test_sensor_tick_time smoke.test_map smoke.test_snapshot smoke.test_lidar smoke.test_streamming smoke.test_spawnpoints smoke.test_blueprint smoke.test_collision_sensor smoke.test_world smoke.test_determinism smoke.test_geoconversion