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
| class SensorFusionEngine:
"""
Murata×Smart Eye 传感器融合引擎
核心功能:
1. 空间标定: 摄像头-雷达坐标系对齐
2. 时间同步: 帧级时间对齐
3. 数据关联: 摄像头检测目标↔雷达点云聚类
4. 状态估计: 卡尔曼滤波融合估计
"""
def __init__(self):
self.calibration = self._load_calibration()
self.tracker = None
def _load_calibration(self):
"""加载摄像头-雷达标定参数"""
return {
"camera_intrinsics": {
"fx": 800, "fy": 800,
"cx": 800, "cy": 600,
"distortion": [0.01, -0.02, 0, 0, 0]
},
"radar_to_camera": {
"rotation": [[1, 0, 0], [0, 1, 0], [0, 0, 1]],
"translation": [0.05, -0.12, 0.03]
},
"time_offset_ms": 2.5
}
def fuse_frame(self, camera_data, radar_data, timestamp):
"""
融合一帧数据
Args:
camera_data: 摄像头检测结果
radar_data: 雷达点云数据
timestamp: 时间戳
Returns:
fused_objects: 融合后的乘员状态
"""
radar_in_cam = self._transform_to_camera(radar_data)
associations = self._associate_objects(
camera_data["detections"],
radar_in_cam["point_clusters"]
)
fused_objects = []
for cam_det, radar_cluster in associations:
obj = self._fuse_single_object(cam_det, radar_cluster)
fused_objects.append(obj)
radar_only = self._get_radar_only(radar_in_cam, associations)
for cluster in radar_only:
fused_objects.append(self._classify_from_radar(cluster))
return fused_objects
def _fuse_single_object(self, cam_det, radar_cluster):
"""融合单个目标"""
return {
"id": cam_det.get("id", -1),
"class": cam_det.get("class", "unknown"),
"position_3d": radar_cluster.get("centroid", cam_det.get("position")),
"bounding_box_2d": cam_det.get("bbox"),
"depth_m": radar_cluster.get("range", None),
"micro_motion": radar_cluster.get("breathing_rate", None),
"confidence": min(cam_det.get("conf", 0.5),
radar_cluster.get("conf", 0.5)) + 0.2
}
|