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
| import numpy as np
import torch
import torch.nn as nn
class PostureEstimator:
"""3D姿态估计器"""
def __init__(self):
self.num_keypoints = 17
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'
]
def estimate_3d_pose(self, depth_image, ir_image):
"""
估计3D姿态
Args:
depth_image: 深度图像 (H, W)
ir_image: 红外图像 (H, W, 3)
Returns:
dict: 3D关键点坐标
"""
features = self._extract_features(depth_image, ir_image)
keypoints_2d = self._detect_keypoints_2d(features)
keypoints_3d = self._reconstruct_3d(keypoints_2d, depth_image)
skeleton = self._fit_skeleton(keypoints_3d)
return skeleton
def _extract_features(self, depth, ir):
"""特征提取"""
depth_norm = (depth - depth.min()) / (depth.max() - depth.min())
ir_norm = ir / 255.0
fused = np.concatenate([
depth_norm[..., np.newaxis],
ir_norm
], axis=-1)
return fused
def _detect_keypoints_2d(self, features):
"""2D关键点检测"""
keypoints_2d = np.zeros((self.num_keypoints, 2))
return keypoints_2d
def _reconstruct_3d(self, keypoints_2d, depth_image):
"""3D重建"""
keypoints_3d = np.zeros((self.num_keypoints, 3))
for i, (x, y) in enumerate(keypoints_2d):
z = depth_image[int(y), int(x)]
keypoints_3d[i] = [x, y, z]
return keypoints_3d
def _fit_skeleton(self, keypoints_3d):
"""骨骼模型拟合"""
skeleton = {
'keypoints_3d': keypoints_3d,
'bones': self._compute_bones(keypoints_3d),
'joint_angles': self._compute_joint_angles(keypoints_3d)
}
return skeleton
def _compute_bones(self, keypoints):
"""计算骨骼向量"""
bone_connections = [
('left_shoulder', 'left_elbow'),
('left_elbow', 'left_wrist'),
('right_shoulder', 'right_elbow'),
('right_elbow', 'right_wrist'),
('left_shoulder', 'left_hip'),
('right_shoulder', 'right_hip'),
('left_hip', 'left_knee'),
('left_knee', 'left_ankle'),
('right_hip', 'right_knee'),
('right_knee', 'right_ankle'),
]
bones = {}
for start_name, end_name in bone_connections:
start_idx = self.keypoint_names.index(start_name)
end_idx = self.keypoint_names.index(end_name)
start = keypoints[start_idx]
end = keypoints[end_idx]
bone_vector = end - start
bone_length = np.linalg.norm(bone_vector)
bones[f"{start_name}_{end_name}"] = {
'vector': bone_vector,
'length': bone_length
}
return bones
def _compute_joint_angles(self, keypoints):
"""计算关节角度"""
joint_angles = {}
for side in ['left', 'right']:
shoulder = keypoints[self.keypoint_names.index(f'{side}_shoulder')]
elbow = keypoints[self.keypoint_names.index(f'{side}_elbow')]
wrist = keypoints[self.keypoint_names.index(f'{side}_wrist')]
v1 = shoulder - elbow
v2 = wrist - elbow
angle = np.arccos(
np.dot(v1, v2) / (np.linalg.norm(v1) * np.linalg.norm(v2))
)
joint_angles[f'{side}_elbow'] = np.degrees(angle)
return joint_angles
|