Add map view to nuscenes python example

examples/python/nuscenes_dataset/README.md

@@ -101,6 +101,18 @@ Radar data is logged similar to LiDAR data, as [`Points3D`](https://www.rerun.io
 rr.log(f"world/ego_vehicle/{sensor_name}", rr.Points3D(points, colors=point_colors))
 ```
 
+### GPS data
+
+GPS data is calculated from the scene's reference coordinates and the transformations (starting map point + odometry).
+The GPS coordinates are logged as [`GeoPoints`](https://www.rerun.io/docs/reference/types/archetypes/geopoints).
+
+```python
+rr.log(
+    "world/ego_vehicle/gps",
+    rr.GeoPoints([[lat, long]]),
+)
+```
+
 ### Annotations
 
 Annotations are logged as [`Boxes3D`](https://www.rerun.io/docs/reference/types/archetypes/boxes3d), containing details such as object positions, sizes, and rotation.
@@ -130,9 +142,22 @@ sensor_space_views = [
     for sensor_name in nuscene_sensor_names(nusc, args.scene_name)
 ]
 blueprint = rrb.Vertical(
-    rrb.Spatial3DView(name="3D", origin="world"),
+    rrb.Horizontal(
+        rrb.Spatial3DView(name="3D", origin="world"),
+        rrb.Vertical(
+            rrb.TextDocumentView(origin="description", name="Description"),
+            rrb.MapView(
+                origin="world/ego_vehicle/gps",
+                name="MapView",
+                zoom=rrb.archetypes.MapZoom(18.0),
+                background=rrb.archetypes.MapBackground(rrb.components.MapProvider.OpenStreetMap),
+            ),
+            row_shares=[1, 1],
+        ),
+        column_shares=[3, 1],
+    ),
     rrb.Grid(*sensor_space_views),
-    row_shares=[3, 2],
+    row_shares=[4, 2],
 )
 ```
 
@@ -155,9 +180,9 @@ pip install -e examples/python/nuscenes_dataset
 ```
 To experiment with the provided example, simply execute the main Python script:
 ```bash
-python -m nuscenes # run the example
+python -m nuscenes_dataset # run the example
 ```
 If you wish to customize it, explore additional features, or save it use the CLI with the `--help` option for guidance:
 ```bash
-python -m nuscenes --help
+python -m nuscenes_dataset --help
 ```

examples/python/nuscenes_dataset/nuscenes_dataset/__main__.py

@@ -12,6 +12,7 @@
 from nuscenes import nuscenes
 
 from .download_dataset import MINISPLIT_SCENES, download_minisplit
+from .export_gps import derive_latlon
 
 DESCRIPTION = """
 # nuScenes
@@ -82,6 +83,8 @@ def log_nuscenes(nusc: nuscenes.NuScenes, scene_name: str, max_time_sec: float)
 
     scene = next(s for s in nusc.scene if s["name"] == scene_name)
 
+    location = nusc.get("log", scene["log_token"])["location"]
+
     rr.log("world", rr.ViewCoordinates.RIGHT_HAND_Z_UP, static=True)
 
     first_sample_token = scene["first_sample_token"]
@@ -104,13 +107,15 @@ def log_nuscenes(nusc: nuscenes.NuScenes, scene_name: str, max_time_sec: float)
     first_timestamp_us = nusc.get("sample_data", first_lidar_token)["timestamp"]
     max_timestamp_us = first_timestamp_us + 1e6 * max_time_sec
 
-    log_lidar_and_ego_pose(first_lidar_token, nusc, max_timestamp_us)
+    log_lidar_and_ego_pose(location, first_lidar_token, nusc, max_timestamp_us)
     log_cameras(first_camera_tokens, nusc, max_timestamp_us)
     log_radars(first_radar_tokens, nusc, max_timestamp_us)
     log_annotations(first_sample_token, nusc, max_timestamp_us)
 
 
-def log_lidar_and_ego_pose(first_lidar_token: str, nusc: nuscenes.NuScenes, max_timestamp_us: float) -> None:
+def log_lidar_and_ego_pose(
+    location: str, first_lidar_token: str, nusc: nuscenes.NuScenes, max_timestamp_us: float
+) -> None:
     """Log lidar data and vehicle pose."""
     current_lidar_token = first_lidar_token
 
@@ -136,6 +141,13 @@ def log_lidar_and_ego_pose(first_lidar_token: str, nusc: nuscenes.NuScenes, max_
         )
         current_lidar_token = sample_data["next"]
 
+        # log GPS data
+        (lat, long) = derive_latlon(location, ego_pose)
+        rr.log(
+            "world/ego_vehicle/gps",
+            rr.GeoPoints(lat_lon=[[lat, long]]),
+        )
+
         data_file_path = nusc.dataroot / sample_data["filename"]
         pointcloud = nuscenes.LidarPointCloud.from_file(str(data_file_path))
         points = pointcloud.points[:3].T  # shape after transposing: (num_points, 3)
@@ -289,15 +301,17 @@ def main() -> None:
     ]
     blueprint = rrb.Vertical(
         rrb.Horizontal(
-            rrb.Spatial3DView(
-                name="3D",
-                origin="world",
-                # Default for `ImagePlaneDistance` so that the pinhole frustum visualizations don't take up too much space.
-                defaults=[rr.components.ImagePlaneDistance(4.0)],
-                # Transform arrows for the vehicle shouldn't be too long.
-                overrides={"world/ego_vehicle": [rr.components.AxisLength(5.0)]},
+            rrb.Spatial3DView(name="3D", origin="world"),
+            rrb.Vertical(
+                rrb.TextDocumentView(origin="description", name="Description"),
+                rrb.MapView(
+                    origin="world/ego_vehicle/gps",
+                    name="MapView",
+                    zoom=rrb.archetypes.MapZoom(18.0),
+                    background=rrb.archetypes.MapBackground(rrb.components.MapProvider.OpenStreetMap),
+                ),
+                row_shares=[1, 1],
             ),
-            rrb.TextDocumentView(origin="description", name="Description"),
             column_shares=[3, 1],
         ),
         rrb.Grid(*sensor_space_views),

examples/python/nuscenes_dataset/nuscenes_dataset/export_gps.py

@@ -0,0 +1,86 @@
+# from nuScenes dev-kit: https://github.com/nutonomy/nuscenes-devkit/blob/4df2701feb3436ae49edaf70128488865a3f6ff9/python-sdk/nuscenes/scripts/export_poses.py
+# Code contributed by jean-lucas, 2020.
+
+"""Exports the nuScenes ego poses as "GPS" coordinates (lat/lon) for each scene into JSON or KML formatted files."""
+
+from __future__ import annotations
+
+import math
+from typing import Sequence
+
+EARTH_RADIUS_METERS = 6.378137e6
+REFERENCE_COORDINATES = {
+    "boston-seaport": [42.336849169438615, -71.05785369873047],
+    "singapore-onenorth": [1.2882100868743724, 103.78475189208984],
+    "singapore-hollandvillage": [1.2993652317780957, 103.78217697143555],
+    "singapore-queenstown": [1.2782562240223188, 103.76741409301758],
+}
+
+
+def get_coordinate(ref_lat: float, ref_lon: float, bearing: float, dist: float) -> tuple[float, float]:
+    """
+    Using a reference coordinate, extract the coordinates of another point in space given its distance and bearing
+    to the reference coordinate. For reference, please see: https://www.movable-type.co.uk/scripts/latlong.html.
+
+    Parameters
+    ----------
+    ref_lat : float
+        Latitude of the reference coordinate in degrees, e.g., 42.3368.
+    ref_lon : float
+        Longitude of the reference coordinate in degrees, e.g., 71.0578.
+    bearing : float
+        The clockwise angle in radians between the target point, reference point, and the axis pointing north.
+    dist : float
+        The distance in meters from the reference point to the target point.
+
+    Returns
+    -------
+    tuple[float, float]
+        A tuple of latitude and longitude.
+
+    """  # noqa: D205
+    lat, lon = math.radians(ref_lat), math.radians(ref_lon)
+    angular_distance = dist / EARTH_RADIUS_METERS
+
+    target_lat = math.asin(
+        math.sin(lat) * math.cos(angular_distance) + math.cos(lat) * math.sin(angular_distance) * math.cos(bearing)
+    )
+    target_lon = lon + math.atan2(
+        math.sin(bearing) * math.sin(angular_distance) * math.cos(lat),
+        math.cos(angular_distance) - math.sin(lat) * math.sin(target_lat),
+    )
+    return math.degrees(target_lat), math.degrees(target_lon)
+
+
+def derive_latlon(location: str, pose: dict[str, Sequence[float]]) -> tuple[float, float]:
+    """
+    Extract lat/lon coordinate from pose.
+
+    This makes the following two assumptions in order to work:
+        1. The reference coordinate for each map is in the south-western corner.
+        2. The origin of the global poses is also in the south-western corner (and identical to 1).
+
+    Parameters
+    ----------
+    location : str
+        The name of the map the poses correspond to, i.e., `boston-seaport`.
+    pose : dict[str, Sequence[float]]
+        nuScenes egopose.
+
+    Returns
+    -------
+    tuple[float, float]
+    Latitude and longitude coordinates in degrees.
+
+    """
+    assert (
+        location in REFERENCE_COORDINATES.keys()
+    ), f"Error: The given location: {location}, has no available reference."
+
+    reference_lat, reference_lon = REFERENCE_COORDINATES[location]
+    x, y = pose["translation"][:2]
+    bearing = math.atan(x / y)
+    distance = math.sqrt(x**2 + y**2)
+    lat, lon = get_coordinate(reference_lat, reference_lon, bearing, distance)
+
+    return lat, lon