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
| import numpy as np
class OccupantPostureEstimator:
"""
乘员姿态估计器
"""
def __init__(self):
self.keypoints_3d = [
"head_top", "nose", "neck",
"left_shoulder", "right_shoulder",
"left_elbow", "right_elbow",
"left_hand", "right_hand",
"spine_middle", "spine_bottom",
"left_hip", "right_hip",
"left_knee", "right_knee"
]
self.posture_classes = [
"normal_sitting",
"leaning_forward",
"leaning_backward",
"slouching",
"sleeping",
"out_of_position"
]
def estimate_depth(self, left_image, right_image, baseline, focal_length):
"""
立体匹配计算深度
Args:
left_image: 左相机图像
right_image: 右相机图像
baseline: 基线距离 (m)
focal_length: 焦距 (pixels)
Returns:
depth_map: 深度图 (m)
"""
disparity = self.compute_disparity(left_image, right_image)
depth_map = baseline * focal_length / (disparity + 1e-6)
return depth_map
def detect_keypoints_3d(self, rgb_image, depth_map):
"""
检测3D关键点
Args:
rgb_image: RGB图像
depth_map: 深度图
Returns:
keypoints_3d: 3D关键点坐标 (N, 3)
"""
keypoints_2d = self.detect_keypoints_2d(rgb_image)
keypoints_3d = []
for kp in keypoints_2d:
x, y = kp["pixel_x"], kp["pixel_y"]
depth = depth_map[y, x]
x_3d = (x - self.cx) * depth / self.fx
y_3d = (y - self.cy) * depth / self.fy
z_3d = depth
keypoints_3d.append([x_3d, y_3d, z_3d])
return np.array(keypoints_3d)
def classify_posture(self, keypoints_3d):
"""
分类姿态
Args:
keypoints_3d: 3D关键点
Returns:
posture: 姿态类别
confidence: 置信度
"""
features = self.extract_posture_features(keypoints_3d)
posture_scores = self.posture_classifier(features)
posture = self.posture_classes[np.argmax(posture_scores)]
confidence = np.max(posture_scores)
return posture, confidence
def extract_posture_features(self, keypoints_3d):
"""
提取姿态特征
"""
features = {}
features["head_height"] = keypoints_3d[0, 2]
neck = keypoints_3d[2]
spine_bottom = keypoints_3d[10]
features["torso_angle"] = np.arctan2(
spine_bottom[2] - neck[2],
spine_bottom[1] - neck[1]
)
left_shoulder = keypoints_3d[3]
right_shoulder = keypoints_3d[4]
features["shoulder_diff"] = abs(left_shoulder[2] - right_shoulder[2])
left_hand = keypoints_3d[7]
right_hand = keypoints_3d[8]
features["hand_height"] = (left_hand[2] + right_hand[2]) / 2
return features
if __name__ == "__main__":
estimator = OccupantPostureEstimator()
depth_map = np.random.rand(720, 1280) * 2 + 0.5
rgb_image = np.random.rand(720, 1280, 3)
keypoints_3d = estimator.detect_keypoints_3d(rgb_image, depth_map)
posture, confidence = estimator.classify_posture(keypoints_3d)
print(f"姿态: {posture}, 置信度: {confidence:.2f}")
|