1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121
| import numpy as np from typing import Tuple import cv2
class SensorCalibration: """ 多传感器时空对齐 包含: 1. 时间戳对齐 2. 空间坐标转换 3. 外参标定 """ def __init__(self): self.camera_matrix = np.array([ [1200.0, 0, 1296.0], [0, 1200.0, 972.0], [0, 0, 1] ]) self.dist_coeffs = np.array([0.1, -0.05, 0, 0, 0]) self.R_radar_to_cam = np.array([ [0.99, 0.01, -0.14], [-0.01, 0.99, 0.05], [0.14, -0.05, 0.99] ]) self.T_radar_to_cam = np.array([0.0, 0.2, 0.1]) def time_synchronization( self, camera_ts: float, radar_ts: float, max_diff: float = 0.033 ) -> bool: """ 时间戳对齐 Args: camera_ts: 摄像头时间戳(秒) radar_ts: 雷达时间戳(秒) max_diff: 最大允许时间差(默认33ms = 1帧) Returns: is_synced: 是否同步 """ return abs(camera_ts - radar_ts) <= max_diff def radar_to_camera_coordinates( self, radar_point: np.ndarray ) -> np.ndarray: """ 将雷达坐标转换为摄像头坐标 Args: radar_point: 雷达坐标系下的点, shape=(3,) or (N, 3) Returns: camera_point: 摄像头坐标系下的点 """ if radar_point.ndim == 1: radar_point = radar_point.reshape(1, -1) camera_point = (self.R_radar_to_cam @ radar_point.T).T + self.T_radar_to_cam return camera_point def project_radar_to_image( self, radar_point: np.ndarray ) -> Tuple[np.ndarray, bool]: """ 将雷达点投影到图像平面 Args: radar_point: 雷达坐标系下的点, shape=(3,) Returns: image_point: 图像坐标 (u, v) is_valid: 是否在图像范围内 """ cam_point = self.radar_to_camera_coordinates(radar_point) if cam_point[2] <= 0: return None, False x = cam_point[0] / cam_point[2] y = cam_point[1] / cam_point[2] u = self.camera_matrix[0, 0] * x + self.camera_matrix[0, 2] v = self.camera_matrix[1, 1] * y + self.camera_matrix[1, 2] image_point = np.array([u, v]) is_valid = (0 <= u < 2592) and (0 <= v < 1944) return image_point, is_valid
if __name__ == "__main__": calib = SensorCalibration() print("时间同步测试:") print(f" 0ms差异: {calib.time_synchronization(1.0, 1.0)}") print(f" 20ms差异: {calib.time_synchronization(1.0, 1.02)}") print(f" 50ms差异: {calib.time_synchronization(1.0, 1.05)}") print("\n坐标转换测试:") radar_point = np.array([1.5, 0.3, 0.8]) image_point, is_valid = calib.project_radar_to_image(radar_point) print(f" 雷达点: {radar_point}") print(f" 图像点: {image_point}, 有效: {is_valid}")
|