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 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229
| import numpy as np from typing import List, Dict, Tuple
class OOPDetector: """ OOP异常姿态检测器 基于单目/双目摄像头进行3D姿态估计 """ def __init__(self, camera_type: str = "monocular"): self.camera_type = camera_type self.keypoint_names = [ 'nose', 'left_eye', 'right_eye', 'left_ear', 'right_ear', 'left_shoulder', 'right_shoulder', 'left_elbow', 'right_elbow', 'left_wrist', 'right_wrist', 'left_hip', 'right_hip', 'left_knee', 'right_knee', 'left_ankle', 'right_ankle' ] self.KEYPOINTS = { 'nose': 0, 'left_eye': 1, 'right_eye': 2, 'left_ear': 3, 'right_ear': 4, 'left_shoulder': 5, 'right_shoulder': 6, 'left_elbow': 7, 'right_elbow': 8, 'left_wrist': 9, 'right_wrist': 10, 'left_hip': 11, 'right_hip': 12, 'left_knee': 13, 'right_knee': 14, 'left_ankle': 15, 'right_ankle': 16 } self.fx = 1000 self.fy = 1000 self.cx = 960 self.cy = 540 def detect(self, frame: np.ndarray, depth_map: np.ndarray = None) -> Dict: """ 检测OOP状态 Args: frame: RGB图像 depth_map: 深度图(可选,双目摄像头) Returns: result: OOP检测结果 """ keypoints_2d = self.detect_keypoints_2d(frame) if depth_map is not None: keypoints_3d = self.lift_to_3d_depth(keypoints_2d, depth_map) else: keypoints_3d = self.lift_to_3d_mono(keypoints_2d) oop_analysis = self.analyze_oop(keypoints_3d) return { 'keypoints_2d': keypoints_2d, 'keypoints_3d': keypoints_3d, 'oop_detected': oop_analysis['is_oop'], 'oop_type': oop_analysis['oop_type'], 'risk_level': oop_analysis['risk_level'], 'distance_to_airbag': oop_analysis['distance'], 'recommended_action': oop_analysis['action'] } def detect_keypoints_2d(self, frame: np.ndarray) -> np.ndarray: """ 检测2D关键点 Args: frame: RGB图像 Returns: keypoints: (17, 3) [x, y, confidence] """ keypoints = np.zeros((17, 3)) keypoints[0] = [480, 300, 0.95] keypoints[1] = [470, 290, 0.92] keypoints[2] = [490, 290, 0.92] keypoints[5] = [450, 350, 0.90] keypoints[6] = [510, 350, 0.90] return keypoints def lift_to_3d_mono(self, keypoints_2d: np.ndarray) -> np.ndarray: """ 单目3D姿态估计 使用人体先验知识从2D推断3D Args: keypoints_2d: 2D关键点 (17, 3) Returns: keypoints_3d: 3D关键点 (17, 3) [x, y, z] in meters """ keypoints_3d = np.zeros((17, 3)) shoulder_width_2d = np.linalg.norm( keypoints_2d[5, :2] - keypoints_2d[6, :2] ) z = (self.fx * 0.4) / (shoulder_width_2d + 1e-7) for i in range(17): x = (keypoints_2d[i, 0] - self.cx) * z / self.fx y = (keypoints_2d[i, 1] - self.cy) * z / self.fy keypoints_3d[i] = [x, y, z] return keypoints_3d def lift_to_3d_depth(self, keypoints_2d: np.ndarray, depth_map: np.ndarray) -> np.ndarray: """ 双目/深度摄像头3D估计 Args: keypoints_2d: 2D关键点 depth_map: 深度图 (H, W) in meters Returns: keypoints_3d: 3D关键点 """ keypoints_3d = np.zeros((17, 3)) for i in range(17): x_2d = int(keypoints_2d[i, 0]) y_2d = int(keypoints_2d[i, 1]) if 0 <= y_2d < depth_map.shape[0] and 0 <= x_2d < depth_map.shape[1]: z = depth_map[y_2d, x_2d] else: z = 0 x = (keypoints_2d[i, 0] - self.cx) * z / self.fx y = (keypoints_2d[i, 1] - self.cy) * z / self.fy keypoints_3d[i] = [x, y, z] return keypoints_3d def analyze_oop(self, keypoints_3d: np.ndarray) -> Dict: """ 分析OOP状态 Args: keypoints_3d: 3D关键点 Returns: analysis: OOP分析结果 """ nose = keypoints_3d[self.KEYPOINTS['nose']] left_shoulder = keypoints_3d[self.KEYPOINTS['left_shoulder']] right_shoulder = keypoints_3d[self.KEYPOINTS['right_shoulder']] airbag_position = np.array([0, 0, 0.5]) distance_to_airbag = np.linalg.norm(nose - airbag_position) oop_type = None risk_level = "low" if distance_to_airbag < 0.30: oop_type = OOPType.TOO_CLOSE risk_level = "critical" elif distance_to_airbag < 0.40: oop_type = OOPType.TOO_CLOSE risk_level = "high" shoulder_center_z = (left_shoulder[2] + right_shoulder[2]) / 2 if nose[2] < shoulder_center_z - 0.15: oop_type = OOPType.FORWARD_LEAN risk_level = "high" is_oop = oop_type is not None action_map = { 'critical': '抑制气囊', 'high': '低功率模式', 'medium': '调整展开参数', 'low': '正常展开' } return { 'is_oop': is_oop, 'oop_type': oop_type.value if oop_type else None, 'risk_level': risk_level, 'distance': distance_to_airbag, 'action': action_map.get(risk_level, '正常展开') }
if __name__ == "__main__": detector = OOPDetector() frame = np.zeros((1080, 1920, 3), dtype=np.uint8) result = detector.detect(frame) print(f"OOP检测: {'是' if result['oop_detected'] else '否'}") print(f"OOP类型: {result['oop_type']}") print(f"风险等级: {result['risk_level']}") print(f"距气囊距离: {result['distance_to_airbag']:.2f}m") print(f"推荐动作: {result['recommended_action']}")
|