File size: 14,521 Bytes
1777546
f3f6401
1777546
 
f3f6401
1777546
f3f6401
 
 
 
 
 
 
 
 
 
 
 
1777546
f3f6401
1777546
f3f6401
 
 
 
 
 
 
 
 
 
 
1777546
 
f3f6401
1777546
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
f3f6401
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1777546
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
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
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
import numpy as np
import time

class Utils:
    def __init__(self, restSpeed:float = 50.0, minPoints:int = 2, requiredFrames:int = 3):

        self.axes:dict = {
            "x": np.array([1, 0, 0]),
            "-x": np.array([-1, 0, 0]),
            "y": np.array([0, 1, 0]),
            "-y": np.array([0, -1, 0]),
            "z": np.array([0, 0, 1]),
            "-z": np.array([0, 0, -1]),
            }
        
        self.REST_SPEED_THRESHOLD: float = restSpeed  # pixels/second
        self.MIN_POINTS_FOR_REST: int = minPoints
        self.REQUIRED_CONSECUTIVE_FRAMES: int = requiredFrames

        self.rest_buffer_frame: int = 0

        self.prev_positions: dict = {
            "positions": {},
            "timestamp": None,
            "counters": {           
                "left_wrist": 0,
                "right_wrist": 0,
                "left_shoulder": 0,
                "right_shoulder": 0
            },
            "rest_start_time": None
        }

    # Function to extract hand features (angles between vectors and axes)
    def extract_hand_features(self, hand_landmarks, pose_landmarks=None):
        hand_pairs = [
            (1 , 3) ,  # Thumb
            (6 , 8 ),  # Index finger
            (10, 12),  # Middle finger
            (14, 16),  # Ring finger
            (18, 20),  # Pinky finger
            (0 , 9)  # Palm direction
        ]
        
        self.features = []
        for pair in hand_pairs:
            landmark1 = hand_landmarks[pair[0]]
            landmark2 = hand_landmarks[pair[1]]
            
            vector = np.array([landmark2.x - landmark1.x, landmark2.y - landmark1.y, landmark2.z - landmark1.z])
            x_axis = np.array([1, 0, 0])
            y_axis = np.array([0, 1, 0])
            z_axis = np.array([0, 0, 1])
            
            angle_x = self.calculate_angle1(vector, x_axis)
            angle_y = self.calculate_angle1(vector, y_axis)
            angle_z = self.calculate_angle1(vector, z_axis)
            
            self.features.extend([angle_x, angle_y, angle_z])
        
        # Safe access to landmarks for 0, 5, and 17
        vector_0_to_5 = self.get_coordinates_safe(hand_landmarks, 5) - self.get_coordinates_safe(hand_landmarks, 0)
        vector_0_to_17 = self.get_coordinates_safe(hand_landmarks, 17) - self.get_coordinates_safe(hand_landmarks, 0)
        
        normal_vector = np.cross(vector_0_to_5, vector_0_to_17)
        
        normal_angle_x = self.calculate_angle1(normal_vector, x_axis)
        normal_angle_y = self.calculate_angle1(normal_vector, y_axis)
        normal_angle_z = self.calculate_angle1(normal_vector, z_axis)
        
        self.features.extend([normal_angle_x, normal_angle_y, normal_angle_z])
        
        # If pose landmarks are available, calculate the distance between nose and wrist
        
        nose_landmark = self.get_coordinates_safe(pose_landmarks, 0)  # Nose is at index 0 in pose landmarks
        wrist_landmark = self.get_coordinates_safe(hand_landmarks, 0)  # Wrist is at index 0 in hand landmarks
            
        # Calculate the distance in the x and y axes
        distance_x = abs(nose_landmark[0] - wrist_landmark[0])
        distance_y = abs(nose_landmark[1] - wrist_landmark[1])
            
        # Append the x and y distances as new features
        self.features.extend([distance_x, distance_y])
        
        return self.features
    
        # Function to extract the pose features
    def extract_pose_features(self, landmarks):
        # Define the landmark indices for the required sets of points (using Pose landmark indices)
        points_sets = {
            "angle_11_12_14": (self.get_coordinates_safe(landmarks, 11), self.get_coordinates_safe(landmarks, 12), self.get_coordinates_safe(landmarks, 14)),  # Left shoulder, right shoulder, right elbow
            "angle_12_14_16": (self.get_coordinates_safe(landmarks, 12), self.get_coordinates_safe(landmarks, 11), self.get_coordinates_safe(landmarks, 13)),  # Right shoulder, right elbow, right wrist
            "angle_11_13_15": (self.get_coordinates_safe(landmarks, 11), self.get_coordinates_safe(landmarks, 13), self.get_coordinates_safe(landmarks, 15)),  # Left shoulder, left elbow, left wrist
            "angle_13_15_17": (self.get_coordinates_safe(landmarks, 12), self.get_coordinates_safe(landmarks, 14), self.get_coordinates_safe(landmarks, 16)),  # Left elbow, left wrist, left hand
            "normal_1": (self.get_coordinates_safe(landmarks, 15), self.get_coordinates_safe(landmarks, 17), self.get_coordinates_safe(landmarks, 19)),  # Plane formed by left shoulder, left hip, left knee
            "normal_2": (self.get_coordinates_safe(landmarks, 16), self.get_coordinates_safe(landmarks, 18), self.get_coordinates_safe(landmarks, 20))   # Plane formed by right shoulder, right hip, right knee
        }

        # Calculate the angles between the specific sets of points
        self.angles = []
        for key, (p1, p2, p3) in points_sets.items():
            if key.startswith("angle"):
                angle = self.calculate_angle2(p1, p2, p3)
                self.angles.append(angle)
        
        # Calculate normals and angles with axes
        for key, (p1, p2, p3) in points_sets.items():
            if key.startswith("normal"):
                normal = self.calculate_normal_safe(p1, p2, p3)  # Safe normal calculation
                if np.array_equal(normal, [-1, -1, -1]):
                    # If normal is [-1, -1, -1], it indicates missing points, so append [-1, -1, -1] for each axis angle
                    self.angles.extend([-1, -1, -1])
                else:
                    normal_angles = self.calculate_normal_angles(normal)
                    self.angles.extend(normal_angles)  # Append angles with x, y, z axes

        # Add the distance between points 15 (left wrist) and 16 (right wrist)
        p15 = self.get_coordinates_safe(landmarks, 15)  # Left wrist
        p16 = self.get_coordinates_safe(landmarks, 16)  # Right wrist
        x_distance, y_distance = self.calculate_xy_distance(p15, p16)
        self.angles.extend([x_distance, y_distance])  # Append x and y distance to the feature list
        
        return self.angles
    
    def _landmark_to_pixel(self, landmark, img_shape):
        """Convert normalized landmark to pixel (x,y)."""
        h, w = img_shape[0], img_shape[1]
        return np.array([landmark.x * w, landmark.y * h], dtype=float)

    # Replace previous is_resting with time-based version
    def is_resting(self, res_hands, img_shape, rest_delay_seconds: float = 2.0, fps: float = None):
        """
        Time-based rest detection.
        Returns True if average landmark speed stays below REST_SPEED_THRESHOLD
        for at least rest_delay_seconds.
        """
        current_time = time.time()
        points = {}

        # Hands: get wrist landmark (index 0) if available
        if getattr(res_hands, "multi_hand_landmarks", None) and getattr(res_hands, "multi_handedness", None):
            for hand_landmarks, handedness in zip(res_hands.multi_hand_landmarks, res_hands.multi_handedness):
                label = handedness.classification[0].label
                try:
                    wrist = self._landmark_to_pixel(hand_landmarks.landmark[0], img_shape)
                except Exception:
                    continue

                if label == 'Left':
                    points['left_wrist'] = wrist
                elif label == 'Right':
                    points['right_wrist'] = wrist

        # Initialize previous timestamp if missing
        if self.prev_positions['timestamp'] is None:
            self.prev_positions['timestamp'] = current_time
            self.prev_positions['positions'].update({k: v for k, v in points.items()})
            self.prev_positions['rest_start_time'] = None
            return False

        # No landmarks detected -> not resting (reset)
        if len(points) < 1:
            self.prev_positions['timestamp'] = current_time
            self.prev_positions['positions'].update({k: v for k, v in points.items()})
            self.prev_positions['rest_start_time'] = None
            # reset counters for keys that are missing
            for k in self.prev_positions['counters']:
                if k not in points:
                    self.prev_positions['counters'][k] = 0
            return False

        dt = current_time - self.prev_positions['timestamp']
        if dt <= 0:
            self.prev_positions['timestamp'] = current_time
            self.prev_positions['positions'].update({k: v for k, v in points.items()})
            self.prev_positions['rest_start_time'] = None
            return False

        speeds = []
        speed_map = {}

        # compute speeds only where previous positions exist
        for key, cur_pos in points.items():
            prev_pos = self.prev_positions['positions'].get(key)
            if prev_pos is not None:
                dist = np.linalg.norm(cur_pos - prev_pos)
                speed = dist / dt
                speeds.append(speed)
                speed_map[key] = speed

        # update timestamp and previous positions for next call
        self.prev_positions['timestamp'] = current_time
        self.prev_positions['positions'].update({k: v for k, v in points.items()})

        # reset counters for disappeared landmarks
        for k in list(self.prev_positions['counters'].keys()):
            if k not in speed_map:
                self.prev_positions['counters'][k] = 0

        # Optionally scale threshold for very low FPS if fps provided
        adjusted_threshold = self.REST_SPEED_THRESHOLD
        if fps is not None and fps > 0:
            adjusted_threshold = self.REST_SPEED_THRESHOLD * (30.0 / max(fps, 1.0))

        # Using avg speed to detect on enough landmarks
        if len(speeds) >= self.MIN_POINTS_FOR_REST:
            avg_speed = float(np.mean(speeds))
            if avg_speed < adjusted_threshold:
                if self.prev_positions['rest_start_time'] is None:
                    self.prev_positions['rest_start_time'] = current_time
                # Check duration
                if (current_time - self.prev_positions['rest_start_time']) >= rest_delay_seconds:
                    return True
            else:
                # Reset rest tracking if movement detected
                self.prev_positions['rest_start_time'] = None
                for k in self.prev_positions['counters']:
                    self.prev_positions['counters'][k] = 0
            return False

        # Single landmark case: treat similarly
        if len(speeds) == 1:
            key = next(iter(speed_map))
            speed = speed_map[key]
            if speed < adjusted_threshold:
                if self.prev_positions['rest_start_time'] is None:
                    self.prev_positions['rest_start_time'] = current_time
                if (current_time - self.prev_positions['rest_start_time']) >= rest_delay_seconds:
                    return True
            else:
                self.prev_positions['rest_start_time'] = None
                self.prev_positions['counters'][key] = 0
            return False

        # default
        self.prev_positions['rest_start_time'] = None
        return False
    
    def calculate_angle1(self, vec1, vec2):
        dot_product = np.dot(vec1, vec2)
        norm_vec1 = np.linalg.norm(vec1)
        norm_vec2 = np.linalg.norm(vec2)
        self.cosine_angle = dot_product / (norm_vec1 * norm_vec2) if norm_vec1 and norm_vec2 else 0
        return self.cosine_angle  

    def get_coordinates_safe(self, landmark, index):
        try:
            return np.array([landmark[index].x, landmark[index].y, landmark[index].z])
        except IndexError:
            return np.array([-1, -1, -1])  

    def angle_between_vectors(self, v1, v2):
        dot_product = np.dot(v1, v2)
        magnitude_v1 = np.linalg.norm(v1)
        magnitude_v2 = np.linalg.norm(v2)
        cos_theta = dot_product / (magnitude_v1 * magnitude_v2)
        cos_theta = np.clip(cos_theta, -1.0, 1.0)
        self.theta = np.arccos(cos_theta)
        return np.degrees(self.theta)

    def get_palm_orientation(self, normal):
        """Function to classify palm orientation"""
        angles = {axis: self.angle_between_vectors(normal, direction) for axis, direction in self.axes.items()}
        # Find the axis with the smallest angle
        self.best_match_axis = min(angles, key=angles.get)
        return self.best_match_axis

    

    #Initialize pose extraction functions
    def calculate_normal_safe(self,p1, p2, p3):
        # Check if any of the points is [-1, -1, -1] (default value for missing landmarks)
        if np.array_equal(p1, [-1, -1, -1]) or np.array_equal(p2, [-1, -1, -1]) or np.array_equal(p3, [-1, -1, -1]):
            return np.array([-1, -1, -1])  # Return [-1, -1, -1] if any point is missing
        else:
            return self.calculate_normal(p1, p2, p3)  # Otherwise, calculate the normal as usual
        
    # Function to calculate angle between three points
    def calculate_angle2(self,p1, p2, p3):
        # Create vectors from points p1, p2, p3
        v1 = p1 - p2
        v2 = p3 - p2
        
        # Calculate the cosine of the angle using dot product
        self.cos_theta = np.dot(v1, v2) / (np.linalg.norm(v1) * np.linalg.norm(v2))
        
        return self.cos_theta

    # Function to calculate the normal of the plane formed by three points
    def calculate_normal(self, p1, p2, p3):
        # Vectors on the plane
        v1 = p2 - p1
        v2 = p3 - p1
        
        # Cross product gives the normal vector
        self.normal = np.cross(v1, v2)
        
        # Normalize the normal vector
        self.normal = self.normal / np.linalg.norm(self.normal)
        
        return self.normal

    # Function to calculate the angle between the normal and each of the axes
    def calculate_normal_angles(self,normal):
        # Calculate angles with x, y, z axes
        self.cos_values = []
        for axis in np.eye(3):  # x, y, z unit vectors
            cos_value = np.dot(normal, axis)
            self.cos_values.append(cos_value)
        return self.cos_values

    # Function to calculate the x and y distance between two points
    def calculate_xy_distance(self, p1, p2):
        self.x_distance = abs(p1[0] - p2[0])  
        self.y_distance = abs(p1[1] - p2[1])  
        return self.x_distance, self.y_distance