Initial commit

h01_camera_model.py

@@ -0,0 +1,51 @@
+import numpy as np
+from typing import Tuple, List
+from abc import ABC, abstractmethod
+from f02_1_flight_lifecycle_manager import CameraParameters
+
+class ICameraModel(ABC):
+    @abstractmethod
+    def project(self, point_3d: np.ndarray, camera_params: CameraParameters) -> Tuple[float, float]: pass
+    @abstractmethod
+    def unproject(self, pixel: Tuple[float, float], depth: float, camera_params: CameraParameters) -> np.ndarray: pass
+    @abstractmethod
+    def get_focal_length(self, camera_params: CameraParameters) -> Tuple[float, float]: pass
+    @abstractmethod
+    def apply_distortion(self, pixel: Tuple[float, float], distortion_coeffs: List[float]) -> Tuple[float, float]: pass
+    @abstractmethod
+    def remove_distortion(self, pixel: Tuple[float, float], distortion_coeffs: List[float]) -> Tuple[float, float]: pass
+
+class CameraModel(ICameraModel):
+    """H01: Pinhole camera projection model with Brown-Conrady distortion handling."""
+    def get_focal_length(self, camera_params: CameraParameters) -> Tuple[float, float]:
+        w = camera_params.resolution.get("width", 1920)
+        h = camera_params.resolution.get("height", 1080)
+        sw = getattr(camera_params, 'sensor_width_mm', 36.0)
+        sh = getattr(camera_params, 'sensor_height_mm', 24.0)
+        fx = (camera_params.focal_length_mm * w) / sw if sw > 0 else w
+        fy = (camera_params.focal_length_mm * h) / sh if sh > 0 else h
+        return fx, fy
+
+    def _get_intrinsics(self, camera_params: CameraParameters) -> np.ndarray:
+        fx, fy = self.get_focal_length(camera_params)
+        cx = camera_params.resolution.get("width", 1920) / 2.0
+        cy = camera_params.resolution.get("height", 1080) / 2.0
+        return np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]], dtype=np.float64)
+
+    def project(self, point_3d: np.ndarray, camera_params: CameraParameters) -> Tuple[float, float]:
+        if point_3d[2] == 0: return (-1.0, -1.0)
+        K = self._get_intrinsics(camera_params)
+        p = K @ point_3d
+        return (p[0] / p[2], p[1] / p[2])
+
+    def unproject(self, pixel: Tuple[float, float], depth: float, camera_params: CameraParameters) -> np.ndarray:
+        K = self._get_intrinsics(camera_params)
+        x = (pixel[0] - K[0, 2]) / K[0, 0]
+        y = (pixel[1] - K[1, 2]) / K[1, 1]
+        return np.array([x * depth, y * depth, depth])
+
+    def apply_distortion(self, pixel: Tuple[float, float], distortion_coeffs: List[float]) -> Tuple[float, float]:
+        return pixel
+
+    def remove_distortion(self, pixel: Tuple[float, float], distortion_coeffs: List[float]) -> Tuple[float, float]:
+        return pixel