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/viewer.py

@@ -1,5 +1,6 @@
 from __future__ import annotations
 
+import math
 import os.path as osp
 import warnings
 from typing import TYPE_CHECKING, Sequence, overload
@@ -31,6 +32,34 @@
 
 __all__ = ["RerunViewer", "format_entity"]
 
+EARTH_RADIUS_METERS = 6.378137e6
+
+
+def _get_coordinate(
+    origin: tuple[float, float],
+    bearing: float,
+    distance: float,
+) -> tuple[float, float]:
+    lat, lon = np.radians(origin)
+    angular_distance = distance / 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 _geo_point(position: TranslationType, origin: tuple[float, float]) -> tuple[float, float]:
+    x, y, _ = position
+    bearing = math.atan(x / y)
+    distance = math.hypot(x, y)
+    return _get_coordinate(origin, bearing, distance)
+
 
 def format_entity(root: str, *entities) -> str:
     """Format entity path.
@@ -101,6 +130,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 +195,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]): Origin (latitude, longitude).
+
+        Returns:
+            Self instance.
+
+        Examples:
+            >>> lat_lon = (42.336849169438615, -71.05785369873047)
+            >>> viewer = RerunViewer("myapp").with_labels(label2id)
+        """
+        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 +547,12 @@ def _render_ego_without_schema(
                 self.geocoordinate_entity,
                 rr.GeoPoints(lat_lon=(latitude, longitude)),
             )
+        elif self.global_origin is not None:
+            latitude, longitude = _geo_point(translation, self.global_origin)
+            rr.log(
+                self.geocoordinate_entity,
+                rr.GeoPoints(lat_lon=(latitude, longitude)),
+            )
 
     @overload
     def render_calibration(