Philosia-codecult/Signify

1import numpy as np2import time3 4class Utils:5    def __init__(self, restSpeed:float = 50.0, minPoints:int = 2, requiredFrames:int = 3):6 7        self.axes:dict = {8            "x": np.array([1, 0, 0]),9            "-x": np.array([-1, 0, 0]),10            "y": np.array([0, 1, 0]),11            "-y": np.array([0, -1, 0]),12            "z": np.array([0, 0, 1]),13            "-z": np.array([0, 0, -1]),14            }15        16        self.REST_SPEED_THRESHOLD: float = restSpeed  # pixels/second17        self.MIN_POINTS_FOR_REST: int = minPoints18        self.REQUIRED_CONSECUTIVE_FRAMES: int = requiredFrames19 20        self.rest_buffer_frame: int = 021 22        self.prev_positions: dict = {23            "positions": {},24            "timestamp": None,25            "counters": {           26                "left_wrist": 0,27                "right_wrist": 0,28                "left_shoulder": 0,29                "right_shoulder": 030            },31            "rest_start_time": None32        }33 34    # Function to extract hand features (angles between vectors and axes)35    def extract_hand_features(self, hand_landmarks, pose_landmarks=None):36        hand_pairs = [37            (1 , 3) ,  # Thumb38            (6 , 8 ),  # Index finger39            (10, 12),  # Middle finger40            (14, 16),  # Ring finger41            (18, 20),  # Pinky finger42            (0 , 9)  # Palm direction43        ]44        45        self.features = []46        for pair in hand_pairs:47            landmark1 = hand_landmarks[pair[0]]48            landmark2 = hand_landmarks[pair[1]]49            50            vector = np.array([landmark2.x - landmark1.x, landmark2.y - landmark1.y, landmark2.z - landmark1.z])51            x_axis = np.array([1, 0, 0])52            y_axis = np.array([0, 1, 0])53            z_axis = np.array([0, 0, 1])54            55            angle_x = self.calculate_angle1(vector, x_axis)56            angle_y = self.calculate_angle1(vector, y_axis)57            angle_z = self.calculate_angle1(vector, z_axis)58            59            self.features.extend([angle_x, angle_y, angle_z])60        61        # Safe access to landmarks for 0, 5, and 1762        vector_0_to_5 = self.get_coordinates_safe(hand_landmarks, 5) - self.get_coordinates_safe(hand_landmarks, 0)63        vector_0_to_17 = self.get_coordinates_safe(hand_landmarks, 17) - self.get_coordinates_safe(hand_landmarks, 0)64        65        normal_vector = np.cross(vector_0_to_5, vector_0_to_17)66        67        normal_angle_x = self.calculate_angle1(normal_vector, x_axis)68        normal_angle_y = self.calculate_angle1(normal_vector, y_axis)69        normal_angle_z = self.calculate_angle1(normal_vector, z_axis)70        71        self.features.extend([normal_angle_x, normal_angle_y, normal_angle_z])72        73        # If pose landmarks are available, calculate the distance between nose and wrist74        75        nose_landmark = self.get_coordinates_safe(pose_landmarks, 0)  # Nose is at index 0 in pose landmarks76        wrist_landmark = self.get_coordinates_safe(hand_landmarks, 0)  # Wrist is at index 0 in hand landmarks77            78        # Calculate the distance in the x and y axes79        distance_x = abs(nose_landmark[0] - wrist_landmark[0])80        distance_y = abs(nose_landmark[1] - wrist_landmark[1])81            82        # Append the x and y distances as new features83        self.features.extend([distance_x, distance_y])84        85        return self.features86    87        # Function to extract the pose features88    def extract_pose_features(self, landmarks):89        # Define the landmark indices for the required sets of points (using Pose landmark indices)90        points_sets = {91            "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 elbow92            "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 wrist93            "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 wrist94            "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 hand95            "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 knee96            "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 knee97        }98 99        # Calculate the angles between the specific sets of points100        self.angles = []101        for key, (p1, p2, p3) in points_sets.items():102            if key.startswith("angle"):103                angle = self.calculate_angle2(p1, p2, p3)104                self.angles.append(angle)105        106        # Calculate normals and angles with axes107        for key, (p1, p2, p3) in points_sets.items():108            if key.startswith("normal"):109                normal = self.calculate_normal_safe(p1, p2, p3)  # Safe normal calculation110                if np.array_equal(normal, [-1, -1, -1]):111                    # If normal is [-1, -1, -1], it indicates missing points, so append [-1, -1, -1] for each axis angle112                    self.angles.extend([-1, -1, -1])113                else:114                    normal_angles = self.calculate_normal_angles(normal)115                    self.angles.extend(normal_angles)  # Append angles with x, y, z axes116 117        # Add the distance between points 15 (left wrist) and 16 (right wrist)118        p15 = self.get_coordinates_safe(landmarks, 15)  # Left wrist119        p16 = self.get_coordinates_safe(landmarks, 16)  # Right wrist120        x_distance, y_distance = self.calculate_xy_distance(p15, p16)121        self.angles.extend([x_distance, y_distance])  # Append x and y distance to the feature list122        123        return self.angles124    125    def _landmark_to_pixel(self, landmark, img_shape):126        """Convert normalized landmark to pixel (x,y)."""127        h, w = img_shape[0], img_shape[1]128        return np.array([landmark.x * w, landmark.y * h], dtype=float)129 130    # Replace previous is_resting with time-based version131    def is_resting(self, res_hands, img_shape, rest_delay_seconds: float = 2.0, fps: float = None):132        """133        Time-based rest detection.134        Returns True if average landmark speed stays below REST_SPEED_THRESHOLD135        for at least rest_delay_seconds.136        """137        current_time = time.time()138        points = {}139 140        # Hands: get wrist landmark (index 0) if available141        if getattr(res_hands, "multi_hand_landmarks", None) and getattr(res_hands, "multi_handedness", None):142            for hand_landmarks, handedness in zip(res_hands.multi_hand_landmarks, res_hands.multi_handedness):143                label = handedness.classification[0].label144                try:145                    wrist = self._landmark_to_pixel(hand_landmarks.landmark[0], img_shape)146                except Exception:147                    continue148 149                if label == 'Left':150                    points['left_wrist'] = wrist151                elif label == 'Right':152                    points['right_wrist'] = wrist153 154        # Initialize previous timestamp if missing155        if self.prev_positions['timestamp'] is None:156            self.prev_positions['timestamp'] = current_time157            self.prev_positions['positions'].update({k: v for k, v in points.items()})158            self.prev_positions['rest_start_time'] = None159            return False160 161        # No landmarks detected -> not resting (reset)162        if len(points) < 1:163            self.prev_positions['timestamp'] = current_time164            self.prev_positions['positions'].update({k: v for k, v in points.items()})165            self.prev_positions['rest_start_time'] = None166            # reset counters for keys that are missing167            for k in self.prev_positions['counters']:168                if k not in points:169                    self.prev_positions['counters'][k] = 0170            return False171 172        dt = current_time - self.prev_positions['timestamp']173        if dt <= 0:174            self.prev_positions['timestamp'] = current_time175            self.prev_positions['positions'].update({k: v for k, v in points.items()})176            self.prev_positions['rest_start_time'] = None177            return False178 179        speeds = []180        speed_map = {}181 182        # compute speeds only where previous positions exist183        for key, cur_pos in points.items():184            prev_pos = self.prev_positions['positions'].get(key)185            if prev_pos is not None:186                dist = np.linalg.norm(cur_pos - prev_pos)187                speed = dist / dt188                speeds.append(speed)189                speed_map[key] = speed190 191        # update timestamp and previous positions for next call192        self.prev_positions['timestamp'] = current_time193        self.prev_positions['positions'].update({k: v for k, v in points.items()})194 195        # reset counters for disappeared landmarks196        for k in list(self.prev_positions['counters'].keys()):197            if k not in speed_map:198                self.prev_positions['counters'][k] = 0199 200        # Optionally scale threshold for very low FPS if fps provided201        adjusted_threshold = self.REST_SPEED_THRESHOLD202        if fps is not None and fps > 0:203            adjusted_threshold = self.REST_SPEED_THRESHOLD * (30.0 / max(fps, 1.0))204 205        # Using avg speed to detect on enough landmarks206        if len(speeds) >= self.MIN_POINTS_FOR_REST:207            avg_speed = float(np.mean(speeds))208            if avg_speed < adjusted_threshold:209                if self.prev_positions['rest_start_time'] is None:210                    self.prev_positions['rest_start_time'] = current_time211                # Check duration212                if (current_time - self.prev_positions['rest_start_time']) >= rest_delay_seconds:213                    return True214            else:215                # Reset rest tracking if movement detected216                self.prev_positions['rest_start_time'] = None217                for k in self.prev_positions['counters']:218                    self.prev_positions['counters'][k] = 0219            return False220 221        # Single landmark case: treat similarly222        if len(speeds) == 1:223            key = next(iter(speed_map))224            speed = speed_map[key]225            if speed < adjusted_threshold:226                if self.prev_positions['rest_start_time'] is None:227                    self.prev_positions['rest_start_time'] = current_time228                if (current_time - self.prev_positions['rest_start_time']) >= rest_delay_seconds:229                    return True230            else:231                self.prev_positions['rest_start_time'] = None232                self.prev_positions['counters'][key] = 0233            return False234 235        # default236        self.prev_positions['rest_start_time'] = None237        return False238    239    def calculate_angle1(self, vec1, vec2):240        dot_product = np.dot(vec1, vec2)241        norm_vec1 = np.linalg.norm(vec1)242        norm_vec2 = np.linalg.norm(vec2)243        self.cosine_angle = dot_product / (norm_vec1 * norm_vec2) if norm_vec1 and norm_vec2 else 0244        return self.cosine_angle  245 246    def get_coordinates_safe(self, landmark, index):247        try:248            return np.array([landmark[index].x, landmark[index].y, landmark[index].z])249        except IndexError:250            return np.array([-1, -1, -1])  251 252    def angle_between_vectors(self, v1, v2):253        dot_product = np.dot(v1, v2)254        magnitude_v1 = np.linalg.norm(v1)255        magnitude_v2 = np.linalg.norm(v2)256        cos_theta = dot_product / (magnitude_v1 * magnitude_v2)257        cos_theta = np.clip(cos_theta, -1.0, 1.0)258        self.theta = np.arccos(cos_theta)259        return np.degrees(self.theta)260 261    def get_palm_orientation(self, normal):262        """Function to classify palm orientation"""263        angles = {axis: self.angle_between_vectors(normal, direction) for axis, direction in self.axes.items()}264        # Find the axis with the smallest angle265        self.best_match_axis = min(angles, key=angles.get)266        return self.best_match_axis267 268    269 270    #Initialize pose extraction functions271    def calculate_normal_safe(self,p1, p2, p3):272        # Check if any of the points is [-1, -1, -1] (default value for missing landmarks)273        if np.array_equal(p1, [-1, -1, -1]) or np.array_equal(p2, [-1, -1, -1]) or np.array_equal(p3, [-1, -1, -1]):274            return np.array([-1, -1, -1])  # Return [-1, -1, -1] if any point is missing275        else:276            return self.calculate_normal(p1, p2, p3)  # Otherwise, calculate the normal as usual277        278    # Function to calculate angle between three points279    def calculate_angle2(self,p1, p2, p3):280        # Create vectors from points p1, p2, p3281        v1 = p1 - p2282        v2 = p3 - p2283        284        # Calculate the cosine of the angle using dot product285        self.cos_theta = np.dot(v1, v2) / (np.linalg.norm(v1) * np.linalg.norm(v2))286        287        return self.cos_theta288 289    # Function to calculate the normal of the plane formed by three points290    def calculate_normal(self, p1, p2, p3):291        # Vectors on the plane292        v1 = p2 - p1293        v2 = p3 - p1294        295        # Cross product gives the normal vector296        self.normal = np.cross(v1, v2)297        298        # Normalize the normal vector299        self.normal = self.normal / np.linalg.norm(self.normal)300        301        return self.normal302 303    # Function to calculate the angle between the normal and each of the axes304    def calculate_normal_angles(self,normal):305        # Calculate angles with x, y, z axes306        self.cos_values = []307        for axis in np.eye(3):  # x, y, z unit vectors308            cos_value = np.dot(normal, axis)309            self.cos_values.append(cos_value)310        return self.cos_values311 312    # Function to calculate the x and y distance between two points313    def calculate_xy_distance(self, p1, p2):314        self.x_distance = abs(p1[0] - p2[0])  315        self.y_distance = abs(p1[1] - p2[1])  316        return self.x_distance, self.y_distance317 318