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 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277
| import numpy as np from dataclasses import dataclass from typing import List, Tuple, Optional from enum import Enum
class OOPCategory(Enum): """OOP分类""" NORMAL = 0 FORWARD_LEAN = 1 REAR_RECLINE = 2 SIDE_LEAN = 3 FEET_ON_DASH = 4 CHILD_SEAT = 5 UNKNOWN = 99
@dataclass class PoseKeypoint: """人体关键点""" name: str position_3d: np.ndarray confidence: float
@dataclass class OOPResult: """OOP检测结果""" category: OOPCategory confidence: float keypoints: List[PoseKeypoint] risk_level: float airbag_recommendation: str
class OOPDetector: """ OOP异常姿态检测器 技术栈: 1. 3D人体姿态估计 2. 座椅坐标系建模 3. 姿态合理性判断 4. 风险等级评估 """ def __init__(self): self.keypoint_names = [ 'nose', 'left_eye', 'right_eye', '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.normal_angles = { 'torso_thigh': (80, 110), 'thigh_shin': (80, 120), 'shoulder_torso': (-30, 30), 'head_torso': (-15, 15) } self.seat_reference = { 'backrest_angle': 25, 'seat_height': 0.5, 'pedal_distance': 0.8 } def detect( self, keypoints_3d: List[np.ndarray], keypoint_confidences: List[float] ) -> OOPResult: """ 检测OOP状态 Args: keypoints_3d: 3D关键点坐标列表 keypoint_confidences: 关键点置信度列表 Returns: OOPResult: 检测结果 """ keypoints = [] for i, name in enumerate(self.keypoint_names): if i < len(keypoints_3d): keypoints.append(PoseKeypoint( name=name, position_3d=keypoints_3d[i], confidence=keypoint_confidences[i] if i < len(keypoint_confidences) else 0.0 )) angles = self._compute_angles(keypoints) category, confidence = self._classify_oop(angles, keypoints) risk_level = self._assess_risk(category, angles) airbag_rec = self._get_airbag_recommendation(category, risk_level) return OOPResult( category=category, confidence=confidence, keypoints=keypoints, risk_level=risk_level, airbag_recommendation=airbag_rec ) def _compute_angles(self, keypoints: List[PoseKeypoint]) -> dict: """计算关节角度""" angles = {} kp_dict = {kp.name: kp.position_3d for kp in keypoints} try: if 'left_hip' in kp_dict and 'right_hip' in kp_dict: hip_center = (kp_dict['left_hip'] + kp_dict['right_hip']) / 2 if 'left_shoulder' in kp_dict and 'right_shoulder' in kp_dict: shoulder_center = (kp_dict['left_shoulder'] + kp_dict['right_shoulder']) / 2 if 'left_knee' in kp_dict and 'right_knee' in kp_dict: knee_center = (kp_dict['left_knee'] + kp_dict['right_knee']) / 2 torso_vec = shoulder_center - hip_center thigh_vec = knee_center - hip_center angle = self._angle_between_vectors(torso_vec, thigh_vec) angles['torso_thigh'] = np.degrees(angle) if 'left_knee' in kp_dict and 'left_hip' in kp_dict and 'left_ankle' in kp_dict: thigh = kp_dict['left_knee'] - kp_dict['left_hip'] shin = kp_dict['left_ankle'] - kp_dict['left_knee'] angle = self._angle_between_vectors(thigh, shin) angles['thigh_shin'] = np.degrees(angle) if 'nose' in kp_dict and 'left_shoulder' in kp_dict and 'right_shoulder' in kp_dict: shoulder_center = (kp_dict['left_shoulder'] + kp_dict['right_shoulder']) / 2 if 'left_hip' in kp_dict and 'right_hip' in kp_dict: hip_center = (kp_dict['left_hip'] + kp_dict['right_hip']) / 2 torso_vec = shoulder_center - hip_center head_vec = kp_dict['nose'] - shoulder_center angle = self._angle_between_vectors(torso_vec, head_vec) angles['head_torso'] = np.degrees(angle) except Exception as e: print(f"角度计算错误: {e}") return angles def _angle_between_vectors(self, v1: np.ndarray, v2: np.ndarray) -> float: """计算两向量夹角""" cos_angle = np.dot(v1, v2) / (np.linalg.norm(v1) * np.linalg.norm(v2) + 1e-6) cos_angle = np.clip(cos_angle, -1, 1) return np.arccos(cos_angle) def _classify_oop(self, angles: dict, keypoints: List[PoseKeypoint]) -> Tuple[OOPCategory, float]: """分类OOP类型""" if not angles: return OOPCategory.UNKNOWN, 0.0 if 'torso_thigh' in angles: angle = angles['torso_thigh'] if angle < self.normal_angles['torso_thigh'][0]: return OOPCategory.FORWARD_LEAN, 0.8 if angle > self.normal_angles['torso_thigh'][1]: return OOPCategory.REAR_RECLINE, 0.7 kp_dict = {kp.name: kp.position_3d for kp in keypoints} if 'left_ankle' in kp_dict and 'right_ankle' in kp_dict: if 'left_knee' in kp_dict: if kp_dict['left_ankle'][1] > kp_dict['left_knee'][1] + 0.2: return OOPCategory.FEET_ON_DASH, 0.75 return OOPCategory.NORMAL, 0.9 def _assess_risk(self, category: OOPCategory, angles: dict) -> float: """评估风险等级""" risk_map = { OOPCategory.NORMAL: 0.0, OOPCategory.FORWARD_LEAN: 0.8, OOPCategory.REAR_RECLINE: 0.5, OOPCategory.SIDE_LEAN: 0.6, OOPCategory.FEET_ON_DASH: 0.9, OOPCategory.CHILD_SEAT: 0.7, OOPCategory.UNKNOWN: 0.3 } return risk_map.get(category, 0.5) def _get_airbag_recommendation(self, category: OOPCategory, risk_level: float) -> str: """获取气囊建议""" if category == OOPCategory.NORMAL: return "正常展开" elif category == OOPCategory.FORWARD_LEAN: return "延迟展开/低功率" elif category == OOPCategory.FEET_ON_DASH: return "抑制展开" elif category == OOPCategory.CHILD_SEAT: return "抑制乘客气囊" elif risk_level > 0.7: return "延迟展开/低功率" else: return "正常展开"
if __name__ == "__main__": detector = OOPDetector() print("=" * 60) print("OOP异常姿态检测测试") print("=" * 60) print("\n场景1: 正常坐姿") normal_keypoints = [ np.array([0.0, 0.8, 0.3]), np.array([-0.03, 0.82, 0.3]), np.array([0.03, 0.82, 0.3]), np.array([-0.2, 0.7, 0.2]), np.array([0.2, 0.7, 0.2]), np.array([-0.35, 0.5, 0.25]), np.array([0.35, 0.5, 0.25]), np.array([-0.25, 0.3, 0.4]), np.array([0.25, 0.3, 0.4]), np.array([-0.15, 0.4, 0.1]), np.array([0.15, 0.4, 0.1]), np.array([-0.15, 0.2, 0.4]), np.array([0.15, 0.2, 0.4]), np.array([-0.15, 0.0, 0.6]), np.array([0.15, 0.0, 0.6]), ] normal_confidences = [0.95] * 15 result = detector.detect(normal_keypoints, normal_confidences) print(f" 分类: {result.category.name}") print(f" 置信度: {result.confidence:.2f}") print(f" 风险等级: {result.risk_level:.2f}") print(f" 气囊建议: {result.airbag_recommendation}") print("\n场景2: 前倾坐姿") forward_keypoints = normal_keypoints.copy() forward_keypoints[0] = np.array([0.0, 0.6, 0.5]) forward_keypoints[3] = np.array([-0.2, 0.55, 0.35]) forward_keypoints[4] = np.array([0.2, 0.55, 0.35]) result = detector.detect(forward_keypoints, normal_confidences) print(f" 分类: {result.category.name}") print(f" 置信度: {result.confidence:.2f}") print(f" 风险等级: {result.risk_level:.2f}") print(f" 气囊建议: {result.airbag_recommendation}")
|