diff --git a/conda/environment.yml b/conda/environment.yml index 8943eb38..23a11300 100644 --- a/conda/environment.yml +++ b/conda/environment.yml @@ -13,5 +13,6 @@ dependencies: - pandas - pip - pyarrow + - pyproj - rich - scipy diff --git a/setup.cfg b/setup.cfg index 46732c55..57e3d0ab 100644 --- a/setup.cfg +++ b/setup.cfg @@ -22,6 +22,7 @@ install_requires = opencv-python pandas pyarrow + pyproj rich scipy diff --git a/src/av2/geometry/utm.py b/src/av2/geometry/utm.py new file mode 100644 index 00000000..035dd681 --- /dev/null +++ b/src/av2/geometry/utm.py @@ -0,0 +1,110 @@ +# + +"""Utilities for converting AV2 city coordinates to UTM or WGS84 coordinate systems. + +Reference: +UTM: https://en.wikipedia.org/wiki/Universal_Transverse_Mercator_coordinate_system +WGS84: https://en.wikipedia.org/wiki/World_Geodetic_System +""" + +from enum import unique, Enum +from typing import Dict, Final, Tuple, Union + +import numpy as np +from pyproj import Proj + +from av2.utils.typing import NDArrayFloat, NDArrayInt + + +@unique +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: Final[Dict[CityName, int]] = { + 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: Final[Dict[CityName, Tuple[float, float]]] = { + 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(latitude: float, longitude: float, city_name: CityName) -> Tuple[float, float]: + """Convert GPS coordinates to UTM coordinates. + + Args: + latitude: latitude of query point. + longitude: 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(longitude, latitude) + + 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: (N,2) array, representing 2d query points in the city coordinate frame. + city_name: name of city, where query points are located. + + Returns: + Array of shape (N,2), representing points in the UTM coordinate system, as (easting, northing). + """ + latitude, longitude = CITY_ORIGIN_LATLONG_DICT[city_name] + # get (easting, northing) of origin + origin_utm = convert_gps_to_utm(latitude=latitude, longitude=longitude, city_name=city_name) + + points_utm: NDArrayFloat = points_city.astype(float) + np.array(origin_utm, dtype=float) + 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: (N,2) array, representing 2d query points in the city coordinate frame. + city_name: name of city, where query points are located. + + Returns: + Array of shape (N,2), representing points in the WGS84 coordinate system, as (latitude, longitude). + """ + 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: + longitude, latitude = projector(easting, northing, inverse=True) + points_wgs84.append((latitude, longitude)) + + return np.array(points_wgs84) diff --git a/tests/geometry/test_utm.py b/tests/geometry/test_utm.py new file mode 100644 index 00000000..283783a2 --- /dev/null +++ b/tests/geometry/test_utm.py @@ -0,0 +1,59 @@ +# + +"""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 +from av2.utils.typing import NDArrayFloat + + +def test_convert_city_coords_to_wgs84_atx() -> None: + """Convert city coordinates from Austin, TX to GPS coordinates.""" + points_city: NDArrayFloat = 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: NDArrayFloat = 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: NDArrayFloat = 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: NDArrayFloat = 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 = 40.44177902989321, -80.01294377242584 + 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)