mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-04-22 09:06:37 +00:00
Denys Zaitsev
51 lines
2.6 KiB
Python
51 lines
2.6 KiB
Python
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 |