feat: add support of loading global map origin from yaml

Signed-off-by: ktro2828 <[email protected]>

t4_devkit/tier4.py

@@ -7,6 +7,7 @@
 
 import numpy as np
 import rerun as rr
+import yaml
 from PIL import Image
 from pyquaternion import Quaternion
 
@@ -918,6 +919,15 @@ def _init_viewer(
         if render_annotation:
             viewer = viewer.with_labels(self._label2id)
 
+        global_map_filepath = osp.join(self.data_root, "map/global_map_center.pcd.yaml")
+        if osp.exists(global_map_filepath):
+            with open(global_map_filepath) as f:
+                map_metadata: dict = yaml.safe_load(f)
+            map_origin: dict = map_metadata["/**"]["ros__parameters"]["map_origin"]
+            latitude = map_origin["latitude"]
+            longitude = map_origin["longitude"]
+            viewer = viewer.with_global_origin((latitude, longitude))
+
         print(f"Finish initializing {application_id} ...")
 
         return viewer
@@ -1231,3 +1241,4 @@ def _append_mask(
         camera_masks[camera]["class_ids"] = [class_id]
         camera_masks[camera]["uuids"] = [class_id]
     return camera_masks
+    return camera_masks

t4_devkit/viewer/__init__.py

@@ -1,2 +1,3 @@
 from .color import *  # noqa
+from .geography import *  # noqa
 from .viewer import *  # noqa

t4_devkit/viewer/geography.py

@@ -0,0 +1,49 @@
+from __future__ import annotations
+
+import math
+from typing import TYPE_CHECKING
+
+import numpy as np
+
+if TYPE_CHECKING:
+    from t4_devkit.typing import TranslationType
+
+
+__all__ = ["calculate_geodetic_point"]
+
+EARTH_RADIUS_METERS = 6.378137e6
+FLATTENING = 1 / 298.257223563
+
+
+def calculate_geodetic_point(
+    position: TranslationType,
+    origin: tuple[float, float],
+) -> tuple[float, float]:
+    """Transform a position in a map coordinate system to a position in a geodetic coordinate system.
+
+    Args:
+        position (TranslationType): 3D position in a map coordinate system.
+        origin (tuple[float, float]): Map origin position in a geodetic coordinate system,
+            which is (latitude, longitude).
+
+    Returns:
+        tuple[float, float]: Transformed position in a geodetic coordinate system,
+            which is (latitude, longitude).
+    """
+    x, y, _ = position
+    bearing = math.atan2(x, y)
+    distance = math.hypot(x, y)
+
+    latitude, longitude = np.radians(origin)
+    angular_distance = distance / EARTH_RADIUS_METERS
+
+    target_latitude = math.asin(
+        math.sin(latitude) * math.cos(angular_distance)
+        + math.cos(latitude) * math.sin(angular_distance) * math.cos(bearing)
+    )
+    target_longitude = longitude + math.atan2(
+        math.sin(bearing) * math.sin(angular_distance) * math.cos(latitude),
+        math.cos(angular_distance) - math.sin(latitude) * math.sin(target_latitude),
+    )
+
+    return math.degrees(target_latitude), math.degrees(target_longitude)

t4_devkit/viewer/viewer.py

@@ -13,6 +13,7 @@
 from t4_devkit.schema import SensorModality
 
 from .color import distance_color
+from .geography import calculate_geodetic_point
 from .rendering_data import BoxData2D, BoxData3D, SegmentationData2D
 
 if TYPE_CHECKING:
@@ -101,6 +102,7 @@ def __init__(
         self.with_3d = with_3d
         self.with_2d = self.cameras is not None
         self.label2id: dict[str, int] | None = None
+        self.global_origin: tuple[float, float] | None = None
 
         if not (self.with_3d or self.with_2d):
             raise ValueError("At least one of 3D or 2D spaces must be rendered.")
@@ -165,6 +167,22 @@ def with_labels(self, label2id: dict[str, int]) -> Self:
 
         return self
 
+    def with_global_origin(self, lat_lon: tuple[float, float]) -> Self:
+        """Return myself after setting global origin.
+
+        Args:
+            lat_lon (tuple[float, float]): Global origin of map (latitude, longitude).
+
+        Returns:
+            Self instance.
+
+        Examples:
+            >>> lat_lon = (42.336849169438615, -71.05785369873047)
+            >>> viewer = RerunViewer("myapp").with_global_origin(lat_lon)
+        """
+        self.global_origin = lat_lon
+        return self
+
     def save(self, save_dir: str) -> None:
         """Save recording result as `save_dir/{app_id}.rrd`.
 
@@ -501,6 +519,12 @@ def _render_ego_without_schema(
                 self.geocoordinate_entity,
                 rr.GeoPoints(lat_lon=(latitude, longitude)),
             )
+        elif self.global_origin is not None:
+            latitude, longitude = calculate_geodetic_point(translation, self.global_origin)
+            rr.log(
+                self.geocoordinate_entity,
+                rr.GeoPoints(lat_lon=(latitude, longitude)),
+            )
 
     @overload
     def render_calibration(