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
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
| import numpy as np
from dataclasses import dataclass
from typing import Optional
@dataclass
class CameraOutput:
"""摄像头模块输出"""
occupant_detected: bool
posture: str
head_pose: np.ndarray
body_keypoints: Optional[np.ndarray]
confidence: float
illumination: float
@dataclass
class RadarOutput:
"""雷达模块输出"""
range_profile: np.ndarray
vital_sign_detected: bool
breathing_rate: float
micro_doppler: np.ndarray
range_to_occupant: float
confidence: float
@dataclass
class FusedOutput:
"""融合输出"""
occupant_present: bool
occupant_type: str
seat_position: str
posture: str
breathing_rate: Optional[float]
confidence: float
primary_sensor: str
class MultimodalFusion:
"""
雷达 + 摄像头融合决策模块
基于规则置信度加权的融合策略：
- 高光照 → 摄像头权重高
- 低光照/遮挡 → 雷达权重高
- 两者都高置信 → 融合增强
"""
def __init__(self):
self.camera_threshold = 0.5
self.radar_threshold = 0.3
self.fusion_threshold = 0.7
def fuse(self, cam: CameraOutput, radar: RadarOutput) -> FusedOutput:
"""
执行融合决策
Args:
cam: 摄像头输出
radar: 雷达输出
Returns:
fused: 融合结果
"""
if cam.illumination < 10:
w_cam, w_radar = 0.2, 0.8
primary = 'radar'
elif cam.illumination < 50:
w_cam, w_radar = 0.4, 0.6
primary = 'radar'
else:
w_cam, w_radar = 0.7, 0.3
primary = 'both' if cam.confidence > 0.7 else 'camera'
cam_conf = cam.confidence if cam.occupant_detected else 1 - cam.confidence
radar_conf = radar.confidence if radar.vital_sign_detected else 1 - radar.confidence
fused_conf = w_cam * cam_conf + w_radar * radar_conf
occupant_present = fused_conf > self.fusion_threshold
if not occupant_present:
occ_type = 'empty'
elif radar.vital_sign_detected and cam.occupant_detected:
if cam.body_keypoints is not None:
height = self._estimate_height(cam.body_keypoints)
if height < 1.0:
occ_type = 'child' if height > 0.5 else 'infant'
else:
occ_type = 'adult'
else:
occ_type = 'adult'
elif radar.vital_sign_detected and not cam.occupant_detected:
occ_type = 'child'
elif not radar.vital_sign_detected and cam.occupant_detected:
occ_type = 'object'
else:
occ_type = 'empty'
breathing_rate = radar.breathing_rate if radar.vital_sign_detected else None
return FusedOutput(
occupant_present=occupant_present,
occupant_type=occ_type,
seat_position='rear_left',
posture=cam.posture if cam.confidence > 0.5 else 'unknown',
breathing_rate=breathing_rate,
confidence=fused_conf,
primary_sensor=primary
)
def _estimate_height(self, keypoints: np.ndarray) -> float:
"""从关键点估算乘员身高（简化版）"""
if len(keypoints) < 17:
return 1.7
head = keypoints[0]
ankle = keypoints[16]
pixel_height = np.linalg.norm(head - ankle)
real_height = pixel_height * 0.005
return real_height
if __name__ == "__main__":
fusion = MultimodalFusion()
cam1 = CameraOutput(True, 'sitting', np.array([0, 0, 0]),
np.random.randn(17, 3), 0.92, 500)
radar1 = RadarOutput(np.random.randn(64), True, 16.5,
np.random.randn(32), 1.2, 0.85)
r1 = fusion.fuse(cam1, radar1)
print(f"场景1 - 正常光照成人: {r1.occupant_type}, 置信度: {r1.confidence:.2f}, 主传感器: {r1.primary_sensor}")
cam2 = CameraOutput(False, 'empty', np.zeros(3), None, 0.1, 0.5)
radar2 = RadarOutput(np.random.randn(64), True, 22.0,
np.random.randn(32), 1.5, 0.78)
r2 = fusion.fuse(cam2, radar2)
print(f"场景2 - 黑暗毛毯儿童: {r2.occupant_type}, 置信度: {r2.confidence:.2f}, 主传感器: {r2.primary_sensor}")
cam3 = CameraOutput(True, 'sitting', np.zeros(3), None, 0.6, 300)
radar3 = RadarOutput(np.random.randn(64), False, 0,
np.zeros(32), 1.3, 0.9)
r3 = fusion.fuse(cam3, radar3)
print(f"场景3 - 座椅书包: {r3.occupant_type}, 置信度: {r3.confidence:.2f}, 主传感器: {r3.primary_sensor}")
|
