Fix loading of fisheye mei camera and add to viser.

Author: DanielDauner

src/py123d/conversion/datasets/kitti360/kitti360_converter.py

@@ -387,7 +387,7 @@ def _get_kitti360_fisheye_mei_camera_metadata(
                 camera_type=fcam_type,
                 width=fisheye_result[fcam_name]["image_width"],
                 height=fisheye_result[fcam_name]["image_height"],
-                mirror_parameter=fisheye_result[fcam_name]["mirror_parameters"],
+                mirror_parameter=float(fisheye_result[fcam_name]["mirror_parameters"]["xi"]),
                 distortion=distortion,
                 projection=projection,
             )

src/py123d/datatypes/scene/arrow/arrow_scene.py

@@ -149,7 +149,7 @@ def get_fisheye_mei_camera_at_iteration(
         self, iteration: int, camera_type: FisheyeMEICameraType
     ) -> Optional[FisheyeMEICamera]:
         fisheye_mei_camera: Optional[FisheyeMEICamera] = None
-        if camera_type in self.available_pinhole_camera_types:
+        if camera_type in self.available_fisheye_mei_camera_types:
             fisheye_mei_camera = get_camera_from_arrow_table(
                 self._get_recording_table(),
                 self._get_table_index(iteration),

src/py123d/datatypes/scene/arrow/utils/arrow_getters.py

@@ -141,14 +141,15 @@ def get_camera_from_arrow_table(
     else:
         raise NotImplementedError("Only string file paths for camera data are supported.")
 
-    camera_metadata = log_metadata.pinhole_camera_metadata[camera_type]
-    if hasattr(camera_metadata, "mirror_parameter") and camera_metadata.mirror_parameter is not None:
+    if camera_name.startswith("fcam"):
+        camera_metadata = log_metadata.fisheye_mei_camera_metadata[camera_type]
         return FisheyeMEICamera(
             metadata=camera_metadata,
             image=image,
             extrinsic=extrinsic,
         )
     else:
+        camera_metadata = log_metadata.pinhole_camera_metadata[camera_type]
         return PinholeCamera(
             metadata=camera_metadata,
             image=image,

src/py123d/datatypes/sensors/fisheye_mei_camera.py

@@ -150,6 +150,10 @@ def from_dict(cls, data_dict: Dict[str, Any]) -> FisheyeMEICameraMetadata:
         )
         return FisheyeMEICameraMetadata(**data_dict)
 
+    @property
+    def aspect_ratio(self) -> float:
+        return self.width / self.height
+
     def to_dict(self) -> Dict[str, Any]:
         data_dict = asdict(self)
         data_dict["camera_type"] = int(self.camera_type)

src/py123d/script/config/conversion/datasets/kitti360_dataset.yaml

@@ -34,7 +34,7 @@ kitti360_dataset:
     pinhole_camera_store_option: "path"
 
     # Fisheye Cameras
-    include_fisheye_mei_cameras: false
+    include_fisheye_mei_cameras: true
     fisheye_mei_camera_store_option: "path"
 
     # LiDARs

src/py123d/visualization/viser/elements/sensor_elements.py

@@ -7,6 +7,7 @@
 import viser
 
 from py123d.datatypes.scene.abstract_scene import AbstractScene
+from py123d.datatypes.sensors.fisheye_mei_camera import FisheyeMEICamera, FisheyeMEICameraMetadata, FisheyeMEICameraType
 from py123d.datatypes.sensors.lidar import LiDARType
 from py123d.datatypes.sensors.pinhole_camera import PinholeCamera, PinholeCameraType
 from py123d.datatypes.vehicle_state.ego_state import EgoStateSE3
@@ -50,7 +51,7 @@ def _add_camera_frustums_to_viser_server(camera_type: PinholeCameraType) -> None
                         f"camera_frustums/{camera_type.serialize()}",
                         fov=camera.metadata.fov_y,
                         aspect=camera.metadata.aspect_ratio,
-                        scale=viser_config.camera_frustum_frustum_scale,
+                        scale=viser_config.camera_frustum_scale,
                         image=camera_image,
                         position=camera_position,
                         wxyz=camera_quaternion,
@@ -74,6 +75,60 @@ def _add_camera_frustums_to_viser_server(camera_type: PinholeCameraType) -> None
         return None
 
 
+def add_fisheye_frustums_to_viser_server(
+    scene: AbstractScene,
+    scene_interation: int,
+    initial_ego_state: EgoStateSE3,
+    viser_server: viser.ViserServer,
+    viser_config: ViserConfig,
+    fisheye_frustum_handles: Dict[FisheyeMEICameraType, viser.CameraFrustumHandle],
+) -> None:
+    if viser_config.fisheye_frustum_visible:
+        scene_center_array = initial_ego_state.center.point_3d.array
+        ego_pose = scene.get_ego_state_at_iteration(scene_interation).rear_axle_se3.array
+        ego_pose[StateSE3Index.XYZ] -= scene_center_array
+
+        def _add_fisheye_frustums_to_viser_server(fisheye_camera_type: FisheyeMEICameraType) -> None:
+            camera = scene.get_fisheye_mei_camera_at_iteration(scene_interation, fisheye_camera_type)
+            if camera is not None:
+                fcam_position, fcam_quaternion, fcam_image = _get_fisheye_camera_values(
+                    camera,
+                    ego_pose.copy(),
+                    viser_config.fisheye_frustum_image_scale,
+                )
+                if fisheye_camera_type in fisheye_frustum_handles:
+                    fisheye_frustum_handles[fisheye_camera_type].position = fcam_position
+                    fisheye_frustum_handles[fisheye_camera_type].wxyz = fcam_quaternion
+                    fisheye_frustum_handles[fisheye_camera_type].image = fcam_image
+                else:
+                    # NOTE @DanielDauner: The FOV is just taking as a static value here.
+                    # The function se
+                    fisheye_frustum_handles[fisheye_camera_type] = viser_server.scene.add_camera_frustum(
+                        f"camera_frustums/{fisheye_camera_type.serialize()}",
+                        fov=185,  # vertical fov
+                        aspect=camera.metadata.aspect_ratio,
+                        scale=viser_config.fisheye_frustum_scale,
+                        image=fcam_image,
+                        position=fcam_position,
+                        wxyz=fcam_quaternion,
+                    )
+
+            return None
+
+        # NOTE; In order to speed up adding camera frustums, we use multithreading and resize the images.
+        with concurrent.futures.ThreadPoolExecutor(
+            max_workers=len(viser_config.fisheye_mei_camera_frustum_types)
+        ) as executor:
+            future_to_camera = {
+                executor.submit(_add_fisheye_frustums_to_viser_server, fcam_type): fcam_type
+                for fcam_type in viser_config.fisheye_mei_camera_frustum_types
+            }
+            for future in concurrent.futures.as_completed(future_to_camera):
+                _ = future.result()
+
+        return None
+
+
 def add_camera_gui_to_viser_server(
     scene: AbstractScene,
     scene_interation: int,
@@ -183,10 +238,90 @@ def _get_camera_values(
     return camera_position, camera_rotation, camera_image
 
 
+def _get_fisheye_camera_values(
+    camera: FisheyeMEICamera,
+    ego_pose: npt.NDArray[np.float64],
+    resize_factor: Optional[float] = None,
+) -> Tuple[npt.NDArray[np.float64], npt.NDArray[np.float64], npt.NDArray[np.uint8]]:
+    assert ego_pose.ndim == 1 and len(ego_pose) == len(StateSE3Index)
+
+    rel_camera_pose = camera.extrinsic.array
+    abs_camera_pose = convert_relative_to_absolute_se3_array(origin=ego_pose, se3_array=rel_camera_pose)
+
+    camera_position = abs_camera_pose[StateSE3Index.XYZ]
+    camera_rotation = abs_camera_pose[StateSE3Index.QUATERNION]
+
+    camera_image = _rescale_image(camera.image, resize_factor)
+    return camera_position, camera_rotation, camera_image
+
+
 def _rescale_image(image: npt.NDArray[np.uint8], scale: float) -> npt.NDArray[np.uint8]:
     if scale == 1.0:
         return image
     new_width = int(image.shape[1] * scale)
     new_height = int(image.shape[0] * scale)
     downscaled_image = cv2.resize(image, (new_width, new_height), interpolation=cv2.INTER_LINEAR)
     return downscaled_image
+
+
+import numpy as np
+
+
+def calculate_fov(metadata: FisheyeMEICameraMetadata) -> tuple[float, float]:
+    """
+    Calculate horizontal and vertical FOV in degrees.
+
+    Returns:
+        (horizontal_fov, vertical_fov) in degrees
+    """
+    xi = metadata.mirror_parameter
+    gamma1 = metadata.projection.gamma1
+    gamma2 = metadata.projection.gamma2
+    u0 = metadata.projection.u0
+    v0 = metadata.projection.v0
+
+    width = metadata.width
+    height = metadata.height
+
+    # Calculate corner positions (furthest from principal point)
+    corners = np.array([[0, 0], [width, 0], [0, height], [width, height]])
+
+    # Convert to normalized coordinates
+    x_norm = (corners[:, 0] - u0) / gamma1
+    y_norm = (corners[:, 1] - v0) / gamma2
+
+    # For MEI model, inverse projection (ignoring distortion for FOV estimate):
+    # r² = x² + y²
+    # θ = arctan(r / (1 - ξ·√(1 + r²)))
+
+    r_squared = x_norm**2 + y_norm**2
+    r = np.sqrt(r_squared)
+
+    # Calculate incident angle for each corner
+    # From MEI model: r = (X/Z_s) where Z_s = Z + ξ·√(X² + Y² + Z²)
+    # This gives: θ = arctan(r·√(1 + (1-ξ²)r²) / (1 - ξ²·r²))
+    # Simplified approximation:
+
+    if xi < 1e-6:  # Perspective camera
+        theta = np.arctan(r)
+    else:
+        # For small angles or as approximation
+        denominator = 1 - xi * np.sqrt(1 + r_squared)
+        theta = np.arctan2(r, denominator)
+
+    np.max(np.abs(theta))
+
+    # Calculate horizontal and vertical FOV separately
+    x_max = np.max(np.abs(x_norm))
+    y_max = np.max(np.abs(y_norm))
+
+    if xi < 1e-6:
+        h_fov = 2 * np.arctan(x_max)
+        v_fov = 2 * np.arctan(y_max)
+    else:
+        denom_h = 1 - xi * np.sqrt(1 + x_max**2)
+        denom_v = 1 - xi * np.sqrt(1 + y_max**2)
+        h_fov = 2 * np.arctan2(x_max, denom_h)
+        v_fov = 2 * np.arctan2(y_max, denom_v)
+
+    return h_fov, v_fov

src/py123d/visualization/viser/viser_config.py

@@ -1,6 +1,7 @@
 from dataclasses import dataclass, field
 from typing import List, Literal, Optional, Tuple
 
+from py123d.datatypes.sensors.fisheye_mei_camera import FisheyeMEICameraType
 from py123d.datatypes.sensors.lidar import LiDARType
 from py123d.datatypes.sensors.pinhole_camera import PinholeCameraType
 from py123d.visualization.color.color import ELLIS_5
@@ -52,7 +53,7 @@ class ViserConfig:
 
     # Map
     map_visible: bool = True
-    map_radius: float = 500.0  # [m]
+    map_radius: float = 200.0  # [m]
     map_non_road_z_offset: float = 0.1  # small z-translation to place crosswalks, parking, etc. on top of the road
     map_requery: bool = True  # Re-query map when ego vehicle moves out of current map bounds
 
@@ -61,18 +62,28 @@ class ViserConfig:
     bounding_box_type: Literal["mesh", "lines"] = "mesh"
     bounding_box_line_width: float = 4.0
 
-    # Cameras
+    # Pinhole Cameras
     # -> Frustum
     camera_frustum_visible: bool = True
     camera_frustum_types: List[PinholeCameraType] = field(default_factory=lambda: all_camera_types.copy())
-    camera_frustum_frustum_scale: float = 1.0
+    camera_frustum_scale: float = 1.0
     camera_frustum_image_scale: float = 0.25  # Resize factor for the camera image shown on the frustum (<1.0 for speed)
 
     # -> GUI
     camera_gui_visible: bool = True
     camera_gui_types: List[PinholeCameraType] = field(default_factory=lambda: [PinholeCameraType.PCAM_F0].copy())
     camera_gui_image_scale: float = 0.25  # Resize factor for the camera image shown in the GUI (<1.0 for speed)
 
+    # Fisheye MEI Cameras
+    # -> Frustum
+    fisheye_frustum_visible: bool = True
+    fisheye_mei_camera_frustum_visible: bool = True
+    fisheye_mei_camera_frustum_types: List[PinholeCameraType] = field(
+        default_factory=lambda: [fcam for fcam in FisheyeMEICameraType]
+    )
+    fisheye_frustum_scale: float = 1.0
+    fisheye_frustum_image_scale: float = 0.25  # Resize factor for the camera image shown on the frustum
+
     # LiDAR
     lidar_visible: bool = True
     lidar_types: List[LiDARType] = field(default_factory=lambda: all_lidar_types.copy())

src/py123d/visualization/viser/viser_viewer.py

@@ -10,6 +10,7 @@
 
 from py123d.datatypes.maps.map_datatypes import MapLayer
 from py123d.datatypes.scene.abstract_scene import AbstractScene
+from py123d.datatypes.sensors.fisheye_mei_camera import FisheyeMEICameraType
 from py123d.datatypes.sensors.lidar import LiDARType
 from py123d.datatypes.sensors.pinhole_camera import PinholeCameraType
 from py123d.datatypes.vehicle_state.ego_state import EgoStateSE3
@@ -24,6 +25,7 @@
     get_ego_3rd_person_view_position,
     get_ego_bev_view_position,
 )
+from py123d.visualization.viser.elements.sensor_elements import add_fisheye_frustums_to_viser_server
 from py123d.visualization.viser.viser_config import ViserConfig
 
 logger = logging.getLogger(__name__)
@@ -249,6 +251,14 @@ def _(_) -> None:
                 self._viser_config,
                 camera_gui_handles,
             )
+            add_fisheye_frustums_to_viser_server(
+                scene,
+                gui_timestep.value,
+                initial_ego_state,
+                self._viser_server,
+                self._viser_config,
+                fisheye_frustum_handles,
+            )
             add_lidar_pc_to_viser_server(
                 scene,
                 gui_timestep.value,
@@ -315,6 +325,7 @@ def _(event: viser.GuiEvent) -> None:
             "lines": None,
         }
         camera_frustum_handles: Dict[PinholeCameraType, viser.CameraFrustumHandle] = {}
+        fisheye_frustum_handles: Dict[FisheyeMEICameraType, viser.CameraFrustumHandle] = {}
         camera_gui_handles: Dict[PinholeCameraType, viser.GuiImageHandle] = {}
         lidar_pc_handles: Dict[LiDARType, Optional[viser.PointCloudHandle]] = {LiDARType.LIDAR_MERGED: None}
         map_handles: Dict[MapLayer, viser.MeshHandle] = {}
@@ -342,6 +353,14 @@ def _(event: viser.GuiEvent) -> None:
             self._viser_config,
             camera_gui_handles,
         )
+        add_fisheye_frustums_to_viser_server(
+            scene,
+            gui_timestep.value,
+            initial_ego_state,
+            self._viser_server,
+            self._viser_config,
+            fisheye_frustum_handles,
+        )
         add_lidar_pc_to_viser_server(
             scene,
             gui_timestep.value,

test_viser.py

@@ -7,8 +7,8 @@
 
 if __name__ == "__main__":
     # splits = ["kitti360_train"]
-    # splits = ["nuscenes-mini_val", "nuscenes-mini_train"]
-    splits = ["nuplan-mini_test", "nuplan-mini_train", "nuplan-mini_val"]
+    splits = ["nuscenes-mini_val", "nuscenes-mini_train"]
+    # splits = ["nuplan-mini_test", "nuplan-mini_train", "nuplan-mini_val"]
     # splits = ["nuplan_private_test"]
     # splits = ["carla_test"]
     # splits = ["wopd_val"]