Merge pull request #38 from DanielDauner/dev_v0.0.6

Merge `dev_v0.0.6` to main

Author: DanielDauner

.flake8

@@ -16,4 +16,7 @@ ignore =
     E731
     # Local variable name is assigned to but never used
     F841
+per-file-ignores =
+    # imported but unused
+    __init__.py: F401
 max-line-length = 120

.gitignore

@@ -23,3 +23,10 @@
 *.csv
 *.log
 *.mp4
+
+
+# Sphinx documentation
+docs/_build/
+docs/build/
+_build/
+.doctrees/

d123/common/datatypes/camera/camera_parameters.py

@@ -57,6 +57,10 @@ def shapely_polygon(self) -> shapely.geometry.Polygon:
     def center(self) -> StateSE3:
         return self.bounding_box_se3.center
 
+    @property
+    def center_se3(self) -> StateSE3:
+        return self.bounding_box_se3.center_se3
+
     @property
     def bounding_box(self) -> BoundingBoxSE3:
         return self.bounding_box_se3

d123/common/datatypes/detection/detection.py

@@ -24,6 +24,7 @@ class DetectionType(SerialIntEnum):
     GENERIC_OBJECT = 6  # Animals, debris, pushable/pullable objects, permanent poles.
 
     EGO = 7
+    SIGN = 8  # TODO: Remove or extent
 
 
 DYNAMIC_DETECTION_TYPES: set[DetectionType] = {

d123/common/datatypes/detection/detection_types.py

@@ -0,0 +1,117 @@
+from __future__ import annotations
+
+import json
+from dataclasses import dataclass
+from typing import Any, Dict
+
+import numpy as np
+import numpy.typing as npt
+
+from d123.common.utils.enums import SerialIntEnum
+
+
+class CameraType(SerialIntEnum):
+    """
+    Enum for cameras in d123.
+    """
+
+    CAM_F0 = 0
+    CAM_B0 = 1
+    CAM_L0 = 2
+    CAM_L1 = 3
+    CAM_L2 = 4
+    CAM_R0 = 5
+    CAM_R1 = 6
+    CAM_R2 = 7
+    CAM_STEREO_L = 8
+    CAM_STEREO_R = 9
+
+
+@dataclass
+class CameraMetadata:
+
+    camera_type: CameraType
+    width: int
+    height: int
+    intrinsic: npt.NDArray[np.float64]  # 3x3 matrix # TODO: don't store matrix but values.
+    distortion: npt.NDArray[np.float64]  # 5x1 vector # TODO: don't store matrix but values.
+
+    def to_dict(self) -> Dict[str, Any]:
+        # TODO: remove None types. Only a placeholder for now.
+        return {
+            "camera_type": int(self.camera_type),
+            "width": self.width,
+            "height": self.height,
+            "intrinsic": self.intrinsic.tolist() if self.intrinsic is not None else None,
+            "distortion": self.distortion.tolist() if self.distortion is not None else None,
+        }
+
+    @classmethod
+    def from_dict(cls, json_dict: Dict[str, Any]) -> CameraMetadata:
+        # TODO: remove None types. Only a placeholder for now.
+        return cls(
+            camera_type=CameraType(json_dict["camera_type"]),
+            width=json_dict["width"],
+            height=json_dict["height"],
+            intrinsic=np.array(json_dict["intrinsic"]) if json_dict["intrinsic"] is not None else None,
+            distortion=np.array(json_dict["distortion"]) if json_dict["distortion"] is not None else None,
+        )
+
+    @property
+    def aspect_ratio(self) -> float:
+        return self.width / self.height
+
+    @property
+    def fov_x(self) -> float:
+        """
+        Calculates the horizontal field of view (FOV) in radian.
+        """
+        fx = self.intrinsic[0, 0]
+        fov_x_rad = 2 * np.arctan(self.width / (2 * fx))
+        return fov_x_rad
+
+    @property
+    def fov_y(self) -> float:
+        """
+        Calculates the vertical field of view (FOV) in radian.
+        """
+        fy = self.intrinsic[1, 1]
+        fov_y_rad = 2 * np.arctan(self.height / (2 * fy))
+        return fov_y_rad
+
+
+def camera_metadata_dict_to_json(camera_metadata: Dict[CameraType, CameraMetadata]) -> Dict[str, Dict[str, Any]]:
+    """
+    Converts a dictionary of CameraMetadata to a JSON-serializable format.
+    :param camera_metadata: Dictionary of CameraMetadata.
+    :return: JSON-serializable dictionary.
+    """
+    camera_metadata_dict = {
+        camera_type.serialize(): metadata.to_dict() for camera_type, metadata in camera_metadata.items()
+    }
+    return json.dumps(camera_metadata_dict)
+
+
+def camera_metadata_dict_from_json(json_dict: Dict[str, Dict[str, Any]]) -> Dict[CameraType, CameraMetadata]:
+    """
+    Converts a JSON-serializable dictionary back to a dictionary of CameraMetadata.
+    :param json_dict: JSON-serializable dictionary.
+    :return: Dictionary of CameraMetadata.
+    """
+    camera_metadata_dict = json.loads(json_dict)
+    return {
+        CameraType.deserialize(camera_type): CameraMetadata.from_dict(metadata)
+        for camera_type, metadata in camera_metadata_dict.items()
+    }
+
+
+@dataclass
+class Camera:
+
+    metadata: CameraMetadata
+    image: npt.NDArray[np.uint8]
+    extrinsic: npt.NDArray[np.float64]  # 4x4 matrix
+
+    def get_view_matrix(self) -> np.ndarray:
+        # Compute the view matrix based on the camera's position and orientation
+        pass

d123/common/datatypes/sensor/camera.py

@@ -0,0 +1,124 @@
+from __future__ import annotations
+
+import json
+from dataclasses import dataclass
+from typing import Dict, Optional, Type
+
+import numpy as np
+import numpy.typing as npt
+
+from d123.common.datatypes.sensor.lidar_index import LIDAR_INDEX_REGISTRY, LiDARIndex
+from d123.common.utils.enums import SerialIntEnum
+
+
+class LiDARType(SerialIntEnum):
+
+    LIDAR_UNKNOWN = 0
+    LIDAR_MERGED = 1
+    LIDAR_TOP = 2
+    LIDAR_FRONT = 3
+    LIDAR_SIDE_LEFT = 4
+    LIDAR_SIDE_RIGHT = 5
+    LIDAR_BACK = 6
+
+
+@dataclass
+class LiDARMetadata:
+
+    lidar_type: LiDARType
+    lidar_index: Type[LiDARIndex]
+    extrinsic: Optional[npt.NDArray[np.float64]] = None  # 4x4 matrix
+
+    # TODO: add identifier if point cloud is returned in lidar or ego frame.
+
+    def to_dict(self) -> dict:
+        return {
+            "lidar_type": self.lidar_type.name,
+            "lidar_index": self.lidar_index.__name__,
+            "extrinsic": self.extrinsic.tolist() if self.extrinsic is not None else None,
+        }
+
+    @classmethod
+    def from_dict(cls, json_dict: dict) -> LiDARMetadata:
+        lidar_type = LiDARType[json_dict["lidar_type"]]
+        if json_dict["lidar_index"] not in LIDAR_INDEX_REGISTRY:
+            raise ValueError(f"Unknown lidar index: {json_dict['lidar_index']}")
+        lidar_index_class = LIDAR_INDEX_REGISTRY[json_dict["lidar_index"]]
+        extrinsic = np.array(json_dict["extrinsic"]) if json_dict["extrinsic"] is not None else None
+        return cls(lidar_type=lidar_type, lidar_index=lidar_index_class, extrinsic=extrinsic)
+
+
+def lidar_metadata_dict_to_json(lidar_metadata: Dict[LiDARType, LiDARMetadata]) -> str:
+    """
+    Converts a dictionary of LiDARMetadata to a JSON-serializable format.
+    :param lidar_metadata: Dictionary of LiDARMetadata.
+    :return: JSON string.
+    """
+    lidar_metadata_dict = {
+        lidar_type.serialize(): metadata.to_dict() for lidar_type, metadata in lidar_metadata.items()
+    }
+    return json.dumps(lidar_metadata_dict)
+
+
+def lidar_metadata_dict_from_json(json_str: str) -> Dict[LiDARType, LiDARMetadata]:
+    """
+    Converts a JSON string back to a dictionary of LiDARMetadata.
+    :param json_str: JSON string.
+    :return: Dictionary of LiDARMetadata.
+    """
+    lidar_metadata_dict = json.loads(json_str)
+    return {
+        LiDARType.deserialize(lidar_type): LiDARMetadata.from_dict(metadata)
+        for lidar_type, metadata in lidar_metadata_dict.items()
+    }
+
+
+@dataclass
+class LiDAR:
+
+    metadata: LiDARMetadata
+    point_cloud: npt.NDArray[np.float32]
+
+    @property
+    def xyz(self) -> npt.NDArray[np.float32]:
+        """
+        Returns the point cloud as an Nx3 array of x, y, z coordinates.
+        """
+        return self.point_cloud[self.metadata.lidar_index.XYZ].T
+
+    @property
+    def xy(self) -> npt.NDArray[np.float32]:
+        """
+        Returns the point cloud as an Nx2 array of x, y coordinates.
+        """
+        return self.point_cloud[self.metadata.lidar_index.XY].T
+
+    @property
+    def intensity(self) -> Optional[npt.NDArray[np.float32]]:
+        """
+        Returns the intensity values of the LiDAR point cloud if available.
+        Returns None if intensity is not part of the point cloud.
+        """
+        if hasattr(self.metadata.lidar_index, "INTENSITY"):
+            return self.point_cloud[self.metadata.lidar_index.INTENSITY]
+        return None
+
+    @property
+    def range(self) -> Optional[npt.NDArray[np.float32]]:
+        """
+        Returns the range values of the LiDAR point cloud if available.
+        Returns None if range is not part of the point cloud.
+        """
+        if hasattr(self.metadata.lidar_index, "RANGE"):
+            return self.point_cloud[self.metadata.lidar_index.RANGE]
+        return None
+
+    @property
+    def elongation(self) -> Optional[npt.NDArray[np.float32]]:
+        """
+        Returns the elongation values of the LiDAR point cloud if available.
+        Returns None if elongation is not part of the point cloud.
+        """
+        if hasattr(self.metadata.lidar_index, "ELONGATION"):
+            return self.point_cloud[self.metadata.lidar_index.ELONGATION]
+        return None

d123/common/datatypes/sensor/lidar.py

@@ -0,0 +1,62 @@
+from enum import IntEnum
+
+from d123.common.utils.enums import classproperty
+
+LIDAR_INDEX_REGISTRY = {}
+
+
+def register_lidar_index(enum_class):
+    LIDAR_INDEX_REGISTRY[enum_class.__name__] = enum_class
+    return enum_class
+
+
+class LiDARIndex(IntEnum):
+
+    @classproperty
+    def XY(self) -> slice:
+        """
+        Returns a slice for the XY coordinates of the LiDAR point cloud.
+        """
+        return slice(self.X, self.Y + 1)
+
+    @classproperty
+    def XYZ(self) -> slice:
+        """
+        Returns a slice for the XYZ coordinates of the LiDAR point cloud.
+        """
+        return slice(self.X, self.Z + 1)
+
+
+@register_lidar_index
+class DefaultLidarIndex(LiDARIndex):
+    X = 0
+    Y = 1
+    Z = 2
+
+
+@register_lidar_index
+class NuplanLidarIndex(LiDARIndex):
+    X = 0
+    Y = 1
+    Z = 2
+    INTENSITY = 3
+    RING = 4
+    ID = 5
+
+
+@register_lidar_index
+class CarlaLidarIndex(LiDARIndex):
+    X = 0
+    Y = 1
+    Z = 2
+    INTENSITY = 3
+
+
+@register_lidar_index
+class WopdLidarIndex(LiDARIndex):
+    RANGE = 0
+    INTENSITY = 1
+    ELONGATION = 2
+    X = 3
+    Y = 4
+    Z = 5

d123/common/datatypes/sensor/lidar_index.py

@@ -206,6 +206,16 @@ def __post_init__(self) -> None:
         """
         assert self.time_us >= 0, "Time point has to be positive!"
 
+    @classmethod
+    def from_ns(cls, t_ns: int) -> TimePoint:
+        """
+        Constructs a TimePoint from a value in nanoseconds.
+        :param t_ns: Time in nanoseconds.
+        :return: TimePoint.
+        """
+        assert isinstance(t_ns, int), "Nanoseconds must be an integer!"
+        return TimePoint(time_us=t_ns // 1000)
+
     @classmethod
     def from_us(cls, t_us: int) -> TimePoint:
         """