Add converter between AV2 city coordinate systems, and WGS84 and UTM

conda/environment.yml

@@ -13,5 +13,6 @@ dependencies:
   - pandas
   - pip
   - pyarrow
+  - pyproj
   - rich
   - scipy

setup.cfg

@@ -22,6 +22,7 @@ install_requires =
     opencv-python
     pandas
     pyarrow
+    pyproj
     rich
     scipy
 

src/av2/geometry/utm.py

@@ -0,0 +1,104 @@
+# <Copyright 2022, Argo AI, LLC. Released under the MIT license.>
+
+"""Utilities for converting AV2 city coordinates to UTM or WGS84 coordinate systems."""
+
+from enum import Enum
+from typing import Tuple, Union
+
+import numpy as np
+from pyproj import Proj
+
+from av2.utils.typing import NDArrayFloat, NDArrayInt
+
+
+class CityName(str, Enum):
+    """Abbreviations of names of cities featured in Argoverse 2."""
+
+    ATX = "ATX"  # Austin, Texas
+    DTW = "DTW"  # Detroit, Michigan
+    MIA = "MIA"  # Miami, Florida
+    PAO = "PAO"  # Palo Alto, California
+    PIT = "PIT"  # Pittsburgh, PA
+    WDC = "WDC"  # Washington, DC
+
+
+# All are North UTM zones (Northern hemisphere)
+UTM_ZONE_MAP = {
+    CityName.ATX: 14,
+    CityName.DTW: 17,
+    CityName.MIA: 17,
+    CityName.PAO: 10,
+    CityName.PIT: 17,
+    CityName.WDC: 18,
+}
+
+
+# as (lat, long) tuples
+CITY_ORIGIN_LATLONG_DICT = {
+    CityName.ATX: (30.27464237939507, -97.7404457407424),
+    CityName.DTW: (42.29993066912924, -83.17555750783717),
+    CityName.MIA: (25.77452579915163, -80.19656914449405),
+    CityName.PAO: (37.416065, -122.13571963362166),
+    CityName.PIT: (40.44177902989321, -80.01294377242584),
+    CityName.WDC: (38.889377, -77.0355047439081),
+}
+
+
+def convert_gps_to_utm(lat: float, long: float, city_name: CityName) -> Tuple[float, float]:
+    """Convert GPS coordinates to UTM coordinates.
+
+    Args:
+        lat: latitude of query point.
+        long: longitude of query point.
+        city_name: name of city, where query point is located.
+
+    Returns:
+        easting: corresponding UTM Easting.
+        northing: corresponding UTM Northing.
+    """
+    projector = Proj(proj="utm", zone=UTM_ZONE_MAP[city_name], ellps="WGS84", datum="WGS84", units="m")
+
+    # convert to UTM.
+    easting, northing = projector(long, lat)
+
+    return easting, northing
+
+
+def convert_city_coords_to_utm(points_city: Union[NDArrayFloat, NDArrayInt], city_name: CityName) -> NDArrayFloat:
+    """Convert city coordinates to UTM coordinates.
+
+    Args:
+        points_city: 2d coordinates, representing query points in the city coordinate frame.
+        city_name: name of city, where query points are located.
+
+    Returns:
+        Points in the UTM coordinate system, as (easting, northing).
+    """
+    lat, long = CITY_ORIGIN_LATLONG_DICT[city_name]
+    # get (easting, northing) of origin
+    origin_utm = convert_gps_to_utm(lat=lat, long=long, city_name=city_name)
+
+    points_utm = points_city + origin_utm
+    return points_utm
+
+
+def convert_city_coords_to_wgs84(points_city: Union[NDArrayFloat, NDArrayInt], city_name: CityName) -> NDArrayFloat:
+    """Convert city coordinates to WGS84 coordinates.
+
+    Args:
+        points_city: 2d coordinates, representing query points in the city coordinate frame.
+        city_name: name of city, where query points are located.
+
+    Returns:
+        Points in the WGS84 coordinate system, as (lat, long).
+    """
+    points_utm = convert_city_coords_to_utm(points_city, city_name)
+
+    projector = Proj(proj="utm", zone=UTM_ZONE_MAP[city_name], ellps="WGS84", datum="WGS84", units="m")
+
+    points_wgs84 = []
+    for easting, northing in points_utm:
+        long, lat = projector(easting, northing, inverse=True)
+        points_wgs84.append((lat, long))
+
+    return np.array(points_wgs84)

tests/geometry/test_utm.py

@@ -0,0 +1,61 @@
+# <Copyright 2022, Argo AI, LLC. Released under the MIT license.>
+
+"""Unit tests on utilities for converting AV2 city coordinates to UTM or WGS84 coordinate systems."""
+
+
+import numpy as np
+
+import av2.geometry.utm as geo_utils
+from av2.geometry.utm import CityName, CITY_ORIGIN_LATLONG_DICT
+
+
+def test_convert_city_coords_to_wgs84_atx() -> None:
+    """Convert city coordinates from Austin, TX to GPS coordinates."""
+
+    points_city = np.array(
+        [
+            [1745.37, -1421.37],
+            [1738.54, -1415.03],
+            [1731.53, -1410.81],
+        ]
+    )
+
+    wgs84_coords = geo_utils.convert_city_coords_to_wgs84(points_city, city_name=CityName.ATX)
+
+    expected_wgs84_coords = np.array(
+        [
+            [30.261642967615092, -97.72246957081633],
+            [30.26170086362131, -97.72253982250783],
+            [30.261739638233472, -97.72261222631731],
+        ]
+    )
+    assert np.allclose(wgs84_coords, expected_wgs84_coords, atol=1e-4)
+
+
+def test_convert_city_coords_to_wgs84_wdc() -> None:
+    """Convert city coordinates from Washington, DC to GPS coordinates."""
+
+    points_city = np.array(
+        [
+            [1716.85, 4470.38],
+            [2139.70, 4606.14],
+        ]
+    )
+
+    wgs84_coords = geo_utils.convert_city_coords_to_wgs84(points_city, city_name=CityName.WDC)
+    expected_wgs84_coords = np.array(
+        [
+            [38.9299801515994, -77.0168603173312],
+            [38.931286945069985, -77.0120195048271],
+        ]
+    )
+    assert np.allclose(wgs84_coords, expected_wgs84_coords, atol=1e-4)
+
+
+def test_convert_gps_to_utm() -> None:
+    """Convert Pittsburgh city origin (given in WGS84) to UTM coordinates."""
+    lat, long = CITY_ORIGIN_LATLONG_DICT[CityName.PIT]
+    utm_coords = geo_utils.convert_gps_to_utm(lat, long, city_name=CityName.PIT)
+
+    expected_utm_coords = 583710, 4477260
+    assert np.allclose(utm_coords, expected_utm_coords, atol=0.01)