diff --git a/ws/src/vision/scripts/PoseDetection.py b/ws/src/vision/scripts/PoseDetection.py new file mode 100644 index 0000000..6278df7 --- /dev/null +++ b/ws/src/vision/scripts/PoseDetection.py @@ -0,0 +1,123 @@ +#!/usr/bin/env python3 +# import rospy +import cv2 +import mediapipe as mp +import numpy as np + +import os # For testing + +''' +Questions: +- Should this be a ROS node or is it fine as a standalone script? +''' + +class PoseDetection: + def __init__(self): + print("Pose Detection Ready") + # rospy.init_node('pose_detection') + # rospy.loginfo("Pose Detection Ready") + + # Initialize MediaPipe Pose as a class attribute + self.mp_pose = mp.solutions.pose + self.pose = self.mp_pose.Pose() + self.mp_drawing = mp.solutions.drawing_utils # For visualizing landmarks + + def detectPose(self): + pass + + def detectGesture(self): + pass + + def detectClothes(self): + pass + + def isChestVisible(self, image_path): + # Load and preprocess the image + image = cv2.imread(image_path) + image_rgb = cv2.cvtColor(image, cv2.COLOR_BGR2RGB) + + # Process the image + results = self.pose.process(image_rgb) + + # Check for landmarks + if results.pose_landmarks: + landmarks = results.pose_landmarks.landmark + + # Get key points for shoulders and chest (sternum approximate region) + left_shoulder = landmarks[self.mp_pose.PoseLandmark.LEFT_SHOULDER] + right_shoulder = landmarks[self.mp_pose.PoseLandmark.RIGHT_SHOULDER] + + # Check visibility and positioning + if left_shoulder.visibility > 0.5 and right_shoulder.visibility > 0.5: + print("Chest is visible.") + return True + else: + print("Chest is not fully visible.") + else: + print("No pose detected.") + return False + + def chestPosition(self, image_path, save_image=False): + # Load and preprocess the image + image = cv2.imread(image_path) + if image is None: + print("Error: Could not load image.") + return None + + image_rgb = cv2.cvtColor(image, cv2.COLOR_BGR2RGB) + + # Process the image to detect pose landmarks + results = self.pose.process(image_rgb) + + # Check for landmarks + if results.pose_landmarks: + landmarks = results.pose_landmarks.landmark + + left_shoulder = landmarks[self.mp_pose.PoseLandmark.LEFT_SHOULDER] + right_shoulder = landmarks[self.mp_pose.PoseLandmark.RIGHT_SHOULDER] + + # Approximate chest region as below the nose and between shoulders + if left_shoulder.visibility > 0.5 and right_shoulder.visibility > 0.5: + chest_x = int((left_shoulder.x + right_shoulder.x) / 2 * image.shape[1]) + chest_y = int((left_shoulder.y + right_shoulder.y) / 2 * image.shape[0]) + + # Save the image with the chest center marked + if save_image: + # Draw a circle at the approximated chest position + cv2.circle(image, (chest_x, chest_y), 10, (255, 0, 0), -1) + cv2.imwrite("./testImages/chest_position.jpg", image) + + return (chest_x, chest_y) + + print("Chest landmarks not detected or not fully visible.") + return None + + def personAngle(self, image_path): + pass + +def main(): + # image_path = "./testImages/image4.jpg" + + # pose_detection = PoseDetection() + + # print(pose_detection.isChestVisible(image_path=image_path)) + + # chest_coords = pose_detection.chestPosition(image_path=image_path, save_image=True) + # if chest_coords: + # print(f"Chest coordinates: {chest_coords}") + + # angle = pose_detection.personAngle(image_path=image_path) + # if angle: + # print(f"Person angle: {angle:.2f} degrees") + + test_images_dir = "./testImages/helicopterhelicopter" + pose_detection = PoseDetection() + + for i in range(1, 20): + image_name = f"{i}.jpeg" + image_path = os.path.join(test_images_dir, image_name) + angle = pose_detection.personAngle(image_path=image_path) + print(angle) + +if __name__ == '__main__': + main() diff --git a/ws/src/vision/scripts/testImages/chest_position.jpg b/ws/src/vision/scripts/testImages/chest_position.jpg new file mode 100644 index 0000000..15c0cc7 Binary files /dev/null and b/ws/src/vision/scripts/testImages/chest_position.jpg differ diff --git a/ws/src/vision/scripts/testImages/helicopterhelicopter/1.jpeg b/ws/src/vision/scripts/testImages/helicopterhelicopter/1.jpeg new file mode 100644 index 0000000..0c3988a Binary files /dev/null and b/ws/src/vision/scripts/testImages/helicopterhelicopter/1.jpeg differ diff --git a/ws/src/vision/scripts/testImages/helicopterhelicopter/10.jpeg b/ws/src/vision/scripts/testImages/helicopterhelicopter/10.jpeg new file mode 100644 index 0000000..5a4c409 Binary files /dev/null and b/ws/src/vision/scripts/testImages/helicopterhelicopter/10.jpeg differ diff --git a/ws/src/vision/scripts/testImages/helicopterhelicopter/11.jpeg b/ws/src/vision/scripts/testImages/helicopterhelicopter/11.jpeg new file mode 100644 index 0000000..7d38ec9 Binary files /dev/null and b/ws/src/vision/scripts/testImages/helicopterhelicopter/11.jpeg differ diff --git a/ws/src/vision/scripts/testImages/helicopterhelicopter/12.jpeg b/ws/src/vision/scripts/testImages/helicopterhelicopter/12.jpeg new file mode 100644 index 0000000..99a541c Binary files /dev/null and b/ws/src/vision/scripts/testImages/helicopterhelicopter/12.jpeg differ diff --git a/ws/src/vision/scripts/testImages/helicopterhelicopter/13.jpeg b/ws/src/vision/scripts/testImages/helicopterhelicopter/13.jpeg new file mode 100644 index 0000000..739679c Binary files /dev/null and b/ws/src/vision/scripts/testImages/helicopterhelicopter/13.jpeg differ diff --git a/ws/src/vision/scripts/testImages/helicopterhelicopter/14.jpeg b/ws/src/vision/scripts/testImages/helicopterhelicopter/14.jpeg new file mode 100644 index 0000000..709089f Binary files /dev/null and b/ws/src/vision/scripts/testImages/helicopterhelicopter/14.jpeg differ diff --git a/ws/src/vision/scripts/testImages/helicopterhelicopter/15.jpeg b/ws/src/vision/scripts/testImages/helicopterhelicopter/15.jpeg new file mode 100644 index 0000000..7f47041 Binary files /dev/null and b/ws/src/vision/scripts/testImages/helicopterhelicopter/15.jpeg differ diff --git a/ws/src/vision/scripts/testImages/helicopterhelicopter/16.jpeg b/ws/src/vision/scripts/testImages/helicopterhelicopter/16.jpeg new file mode 100644 index 0000000..2cb3d3a Binary files /dev/null and b/ws/src/vision/scripts/testImages/helicopterhelicopter/16.jpeg differ diff --git a/ws/src/vision/scripts/testImages/helicopterhelicopter/17.jpeg b/ws/src/vision/scripts/testImages/helicopterhelicopter/17.jpeg new file mode 100644 index 0000000..0389620 Binary files /dev/null and b/ws/src/vision/scripts/testImages/helicopterhelicopter/17.jpeg differ diff --git a/ws/src/vision/scripts/testImages/helicopterhelicopter/18.jpeg b/ws/src/vision/scripts/testImages/helicopterhelicopter/18.jpeg new file mode 100644 index 0000000..e9c6931 Binary files /dev/null and b/ws/src/vision/scripts/testImages/helicopterhelicopter/18.jpeg differ diff --git a/ws/src/vision/scripts/testImages/helicopterhelicopter/19.jpeg b/ws/src/vision/scripts/testImages/helicopterhelicopter/19.jpeg new file mode 100644 index 0000000..72baffb Binary files /dev/null and b/ws/src/vision/scripts/testImages/helicopterhelicopter/19.jpeg differ diff --git a/ws/src/vision/scripts/testImages/helicopterhelicopter/2.jpeg b/ws/src/vision/scripts/testImages/helicopterhelicopter/2.jpeg new file mode 100644 index 0000000..7c478d5 Binary files /dev/null and b/ws/src/vision/scripts/testImages/helicopterhelicopter/2.jpeg differ diff --git a/ws/src/vision/scripts/testImages/helicopterhelicopter/3.jpeg b/ws/src/vision/scripts/testImages/helicopterhelicopter/3.jpeg new file mode 100644 index 0000000..4341296 Binary files /dev/null and b/ws/src/vision/scripts/testImages/helicopterhelicopter/3.jpeg differ diff --git a/ws/src/vision/scripts/testImages/helicopterhelicopter/4.jpeg b/ws/src/vision/scripts/testImages/helicopterhelicopter/4.jpeg new file mode 100644 index 0000000..a2d6994 Binary files /dev/null and b/ws/src/vision/scripts/testImages/helicopterhelicopter/4.jpeg differ diff --git a/ws/src/vision/scripts/testImages/helicopterhelicopter/5.jpeg b/ws/src/vision/scripts/testImages/helicopterhelicopter/5.jpeg new file mode 100644 index 0000000..4b13213 Binary files /dev/null and b/ws/src/vision/scripts/testImages/helicopterhelicopter/5.jpeg differ diff --git a/ws/src/vision/scripts/testImages/helicopterhelicopter/6.jpeg b/ws/src/vision/scripts/testImages/helicopterhelicopter/6.jpeg new file mode 100644 index 0000000..9e3c26c Binary files /dev/null and b/ws/src/vision/scripts/testImages/helicopterhelicopter/6.jpeg differ diff --git a/ws/src/vision/scripts/testImages/helicopterhelicopter/7.jpeg b/ws/src/vision/scripts/testImages/helicopterhelicopter/7.jpeg new file mode 100644 index 0000000..4178966 Binary files /dev/null and b/ws/src/vision/scripts/testImages/helicopterhelicopter/7.jpeg differ diff --git a/ws/src/vision/scripts/testImages/helicopterhelicopter/8.jpeg b/ws/src/vision/scripts/testImages/helicopterhelicopter/8.jpeg new file mode 100644 index 0000000..19ff784 Binary files /dev/null and b/ws/src/vision/scripts/testImages/helicopterhelicopter/8.jpeg differ diff --git a/ws/src/vision/scripts/testImages/helicopterhelicopter/9.jpeg b/ws/src/vision/scripts/testImages/helicopterhelicopter/9.jpeg new file mode 100644 index 0000000..d5ff7a5 Binary files /dev/null and b/ws/src/vision/scripts/testImages/helicopterhelicopter/9.jpeg differ diff --git a/ws/src/vision/scripts/testImages/image.jpg b/ws/src/vision/scripts/testImages/image.jpg new file mode 100644 index 0000000..c090436 Binary files /dev/null and b/ws/src/vision/scripts/testImages/image.jpg differ diff --git a/ws/src/vision/scripts/testImages/image2.jpg b/ws/src/vision/scripts/testImages/image2.jpg new file mode 100644 index 0000000..4dc9d89 Binary files /dev/null and b/ws/src/vision/scripts/testImages/image2.jpg differ diff --git a/ws/src/vision/scripts/testImages/image3.jpg b/ws/src/vision/scripts/testImages/image3.jpg new file mode 100644 index 0000000..62f246e Binary files /dev/null and b/ws/src/vision/scripts/testImages/image3.jpg differ diff --git a/ws/src/vision/scripts/testImages/image4.jpg b/ws/src/vision/scripts/testImages/image4.jpg new file mode 100644 index 0000000..e1c9d32 Binary files /dev/null and b/ws/src/vision/scripts/testImages/image4.jpg differ diff --git a/ws/src/vision/scripts/testImages/image5.jpg b/ws/src/vision/scripts/testImages/image5.jpg new file mode 100644 index 0000000..70d2b72 Binary files /dev/null and b/ws/src/vision/scripts/testImages/image5.jpg differ