diff --git a/people_tracking_v2/CMakeLists.txt b/people_tracking_v2/CMakeLists.txt new file mode 100644 index 0000000..5805279 --- /dev/null +++ b/people_tracking_v2/CMakeLists.txt @@ -0,0 +1,55 @@ +cmake_minimum_required(VERSION 3.0.2) +project(people_tracking) + +find_package(catkin REQUIRED COMPONENTS + message_generation + people_recognition_msgs + rgbd + rgbd_image_buffer + roscpp + sensor_msgs + visualization_msgs +) + +catkin_python_setup() + +add_message_files( + FILES + FaceTarget.msg + ColourTarget.msg + DetectPerson.msg + DetectedPerson.msg +) + +add_service_files( + FILES + Depth.srv +) + +generate_messages( + DEPENDENCIES + sensor_msgs +) + +catkin_package( + CATKIN_DEPENDS message_runtime sensor_msgs +) + +include_directories( +# include + SYSTEM + ${catkin_INCLUDE_DIRS} +) + +add_executable(people_tracker test/experiments/people_tracker.cpp) +target_link_libraries(people_tracker ${catkin_LIBRARIES}) + +install( + TARGETS people_tracker + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +if (CATKIN_ENABLE_TESTING) + find_package(catkin_lint_cmake REQUIRED) + catkin_add_catkin_lint_test(-W2) +endif() diff --git a/people_tracking_v2/package.xml b/people_tracking_v2/package.xml new file mode 100644 index 0000000..8a49096 --- /dev/null +++ b/people_tracking_v2/package.xml @@ -0,0 +1,55 @@ + + + + people_tracking + 0.0.1 + The people_tracking package + + Arpit Aggarwal + + MIT + + https://github.com/tue-robotics/people_recognition/issues + https://github.com/tue-robotics/people_recognition/tree/master/people_recognition_2d + + Arpit Aggarwal + + catkin + + python3-setuptools + + sensor_msgs + + people_recognition_msgs + rgbd + rgbd_image_buffer + roscpp + visualization_msgs + + message_generation + + cv_bridge + image_recognition_util + message_runtime + people_recognition_msgs + rgbd + rgbd_image_buffer + roscpp + rospy + std_msgs + visualization_msgs + + catkin_lint_cmake + + python3-sphinx + python-sphinx-autoapi-pip + python-sphinx-rtd-theme-pip + python3-yaml + + + + + + diff --git a/people_tracking_v2/rosdoc.yaml b/people_tracking_v2/rosdoc.yaml new file mode 100644 index 0000000..d6daedd --- /dev/null +++ b/people_tracking_v2/rosdoc.yaml @@ -0,0 +1,3 @@ +- builder: sphinx + sphinx_root_dir: docs + name: Python API diff --git a/people_tracking_v2/setup.py b/people_tracking_v2/setup.py new file mode 100644 index 0000000..7ab7b72 --- /dev/null +++ b/people_tracking_v2/setup.py @@ -0,0 +1,11 @@ +#!/usr/bin/env python + +from setuptools import setup +from catkin_pkg.python_setup import generate_distutils_setup + +d = generate_distutils_setup( + packages=['people_tracking'], + package_dir={'': 'src'}, +) + +setup(**d) diff --git a/people_tracking_v2/src/people_tracking/UKFclass.py b/people_tracking_v2/src/people_tracking/UKFclass.py new file mode 100644 index 0000000..1bddf65 --- /dev/null +++ b/people_tracking_v2/src/people_tracking/UKFclass.py @@ -0,0 +1,55 @@ +import numpy as np +from filterpy.kalman import MerweScaledSigmaPoints +from filterpy.kalman import UnscentedKalmanFilter + + +class UKF: + def __init__(self): + self.current_time = 0.0 + + dt = 0.1 # standard dt + + # Create sigma points + self.points = MerweScaledSigmaPoints(6, alpha=0.1, beta=2.0, kappa=-1) + + self.kf = UnscentedKalmanFilter(dim_x=6, dim_z=3, dt=dt, fx=self.fx, hx=self.hx, points=self.points) + + # Initial state: [x, vx, y, vy, z, vz] + self.kf.x = np.array([1., 0, 300., 0, 1., 0]) + + # Initial uncertainty + self.kf.P *= 1 + + # Measurement noise covariance matrix + z_std = 0.2 + self.kf.R = np.diag([z_std ** 2, z_std ** 2, z_std ** 2]) + + # Process noise covariance matrix + process_noise_stds = [3.0 ** 2, 0.1 ** 2, 3 ** 2, 0.1 ** 2, 0.1 ** 2, 0.1 ** 2] + + self.kf.Q = np.diag(process_noise_stds) + + def fx(self, x, dt): + """ State transition function """ + F = np.array([[1, dt, 0, 0, 0, 0], + [0, 1, 0, 0, 0, 0], + [0, 0, 1, dt, 0, 0], + [0, 0, 0, 1, 0, 0], + [0, 0, 0, 0, 1, dt], + [0, 0, 0, 0, 0, 1]], dtype=float) + return np.dot(F, x) + + def hx(self, x): + """ Measurement function [x, y, z] """ + return np.array([x[0], x[2], x[4]]) + + def predict(self, time): + """ Predict the next state """ + delta_t = time - self.current_time + self.kf.predict(dt=delta_t) + self.current_time = time + + def update(self, time, z): + """ Update filter with measurements z. """ + self.predict(time) + self.kf.update(z) diff --git a/people_tracking_v2/src/people_tracking/colour_check.py b/people_tracking_v2/src/people_tracking/colour_check.py new file mode 100755 index 0000000..01c0e55 --- /dev/null +++ b/people_tracking_v2/src/people_tracking/colour_check.py @@ -0,0 +1,91 @@ +#!/usr/bin/env python +import rospy +import cv2 +import numpy as np +from cv_bridge import CvBridge +from typing import List + +from people_tracking.msg import ColourTarget, DetectedPerson +from std_srvs.srv import Empty, EmptyResponse + +NODE_NAME = 'HoC' +TOPIC_PREFIX = '/hero/' + + +class HOC: + """Class for the histogram of colour node.""" + def __init__(self) -> None: + # ROS Initialize + rospy.init_node(NODE_NAME, anonymous=True) + self.subscriber = rospy.Subscriber(TOPIC_PREFIX + 'person_detections', DetectedPerson, self.callback, + queue_size=1) + self.publisher = rospy.Publisher(TOPIC_PREFIX + 'HoC', ColourTarget, queue_size=2) + self.reset_service = rospy.Service(TOPIC_PREFIX + NODE_NAME + '/reset', Empty, self.reset) + + # Variables + self.HoC_detections = [] + self.last_batch_processed = 0 + + def reset(self, request): + """ Reset all stored variables in Class to their default values.""" + self.HoC_detections = [] + self.last_batch_processed = 0 + return EmptyResponse() + + @staticmethod + def get_vector(image, bins: int = 32) -> List[float]: + """ Return HSV-colour histogram vector from image. + + :param image: cv2 image to turn into vector. + :param bins: amount of bins in histogram. + :return: HSV-colour histogram vector from image. + """ + hsv_image = cv2.cvtColor(image, cv2.COLOR_RGB2HSV) # Convert image to HSV + + histograms = [cv2.calcHist([hsv_image], [i], None, [bins], [1, 256]) + for i in range(3)] # Get color histograms + + histograms = [hist / hist.sum() for hist in histograms] # Normalize histograms + + vector = np.concatenate(histograms, axis=0).reshape(-1) # Create colour histogram vector + return vector.tolist() + + def callback(self, data: DetectedPerson) -> None: + """ Get the colour vectors for each detected person and publish this.""" + time = data.time + nr_batch = data.nr_batch + nr_persons = data.nr_persons + detected_persons = data.detected_persons + x_positions = data.x_positions + y_positions = data.y_positions + z_positions = data.z_positions + + if nr_batch <= self.last_batch_processed: + return + if nr_persons < 1: + return + + bridge = CvBridge() + colour_vectors = [self.get_vector(bridge.imgmsg_to_cv2(person, desired_encoding='passthrough')) for person in + detected_persons] + + msg = ColourTarget() + msg.time = time + msg.nr_batch = nr_batch + msg.nr_persons = nr_persons + msg.x_positions = x_positions + msg.y_positions = y_positions + msg.z_positions = z_positions + msg.colour_vectors = [item for sublist in colour_vectors for item in sublist] + + self.publisher.publish(msg) + self.last_batch_processed = nr_batch + + +if __name__ == '__main__': + try: + node_hoc = HOC() + rospy.spin() + except rospy.exceptions.ROSInterruptException: + rospy.loginfo("Failed to launch HoC Node") + pass diff --git a/people_tracking_v2/src/people_tracking/face_detection.py b/people_tracking_v2/src/people_tracking/face_detection.py new file mode 100755 index 0000000..15779c2 --- /dev/null +++ b/people_tracking_v2/src/people_tracking/face_detection.py @@ -0,0 +1,151 @@ +#!/usr/bin/env python +import rospy +from cv_bridge import CvBridge +import face_recognition +from typing import List, Optional + +from people_tracking.msg import DetectedPerson, FaceTarget +from std_srvs.srv import Empty, EmptyResponse + +NODE_NAME = 'Face' +TOPIC_PREFIX = '/hero/' + + +class FacialRecognition: + """ Class for the facial recognition node.""" + def __init__(self) -> None: + # ROS Initialize + rospy.init_node(NODE_NAME, anonymous=True) + self.subscriber = rospy.Subscriber(TOPIC_PREFIX + 'person_detections', DetectedPerson, self.callback, + queue_size=1) + self.publisher = rospy.Publisher(TOPIC_PREFIX + 'face_detections', FaceTarget, queue_size=2) + self.reset_service = rospy.Service(TOPIC_PREFIX + NODE_NAME + '/reset', Empty, self.reset) + + # Variables + self.last_batch_processed = 0 + self.known_face_encodings = [] + self.latest_data = None + self.encoded_batch = None + self.encoded = False # True if encoding process of face was succesful + + def reset(self, request): + """ Reset all stored variables in Class to their default values.""" + self.last_batch_processed = 0 + self.known_face_encodings = [] + self.latest_data = None + self.encoded_batch = None + self.encoded = False + return EmptyResponse() + + def encode_known_faces(self, image, model: str = "hog") -> None: + """ + From the given image, encode the face and store it. + + :param image: image to find the face to encode on. + :param model: method to use for encoding. Either hog (cpu) or cnn (gpu). + """ + bridge = CvBridge() + image = bridge.imgmsg_to_cv2(image, desired_encoding='passthrough') + face_locations = face_recognition.face_locations(image, model=model) + face_encodings = face_recognition.face_encodings(image, face_locations) + + for encoding in face_encodings: + self.known_face_encodings.append(encoding) + + def recognize_faces(self, input_images: List, ) -> List[Optional[bool]]: + """ Check if a face on the given image matches the encoded face(s). + Assumptions: + * A face has already been encoded and stored. + + :param input_images: list with images to compare to the stored face embedding vector. + :return: a list with True if face is the stored face, False if face is not the stored face and + None if no face detected. + """ + bridge = CvBridge() + match_check = [] + for idx, image in enumerate(input_images): + image = bridge.imgmsg_to_cv2(image, desired_encoding='passthrough') + face_location = face_recognition.face_locations(image) + + if len(face_location) <= 0: # No face detected + match_check.append(None) + face_encoding = face_recognition.face_encodings(image, face_location) + + for encoding in face_encoding: + matches = face_recognition.compare_faces(self.known_face_encodings, encoding, tolerance=0.5) + if any(matches): + match_check.append(True) + else: + match_check.append(False) + return match_check + + def process_latest_data(self) -> None: + """ Process the most recent data to see if the correct face is detected. This data will be published to the + publisher. + """ + if self.latest_data is None: + return + + if not self.encoded: + return + + data = self.latest_data + time = data.time + nr_batch = data.nr_batch + nr_persons = data.nr_persons + detected_persons = data.detected_persons + x_positions = data.x_positions + y_positions = data.y_positions + z_positions = data.z_positions + + if nr_batch > self.last_batch_processed: + face_detections = self.recognize_faces(detected_persons) + + msg = FaceTarget() + msg.time = time + msg.batch_nr = int(nr_batch) + msg.nr_persons = nr_persons + msg.x_positions = x_positions + msg.y_positions = y_positions + msg.z_positions = z_positions + msg.face_detections_valid = [False if value is None else True for value in face_detections] + msg.face_detections = [value if value is not None else False for value in face_detections] + + self.publisher.publish(msg) + self.last_batch_processed = nr_batch + + self.latest_data = None # Clear the latest data after processing + return + + def callback(self, data: DetectedPerson) -> None: + """ Store the latest person detections. Encodes the first person in detected_persons as the comparison face. + + :param data: DetectedPerson message. + """ + self.latest_data = data + + if not self.encoded: # Define first image with face as comparison face + if len(data.detected_persons) <= 0: + return + rospy.loginfo("Encoding %s", data.nr_batch) + self.encode_known_faces(data.detected_persons[0]) + + if len(self.known_face_encodings) >= 1: + self.encoded = True + self.encoded_batch = data.nr_batch + rospy.loginfo("Encoded Face: %s", self.encoded_batch) + + def main_loop(self) -> None: + """ Loop to process people detections. """ + while not rospy.is_shutdown(): + self.process_latest_data() + rospy.sleep(0.001) + + +if __name__ == '__main__': + try: + node_face = FacialRecognition() + node_face.main_loop() + except rospy.exceptions.ROSInterruptException: + rospy.loginfo("Failed to launch Facial Recognition Node") + pass diff --git a/people_tracking_v2/src/people_tracking/person_detection.py b/people_tracking_v2/src/people_tracking/person_detection.py new file mode 100755 index 0000000..3a0ed49 --- /dev/null +++ b/people_tracking_v2/src/people_tracking/person_detection.py @@ -0,0 +1,229 @@ +#!/usr/bin/env python +import sys +import os +import rospkg +import time +import rospy +import cv2 +import numpy as np +from cv_bridge import CvBridge +from ultralytics import YOLO + +from sensor_msgs.msg import Image +from people_tracking.msg import DetectedPerson +from std_srvs.srv import Empty, EmptyResponse + + +NODE_NAME = 'person_detection' +TOPIC_PREFIX = '/hero/' + +laptop = sys.argv[1] +name_subscriber_RGB = 'video_frames' if laptop == "True" else '/hero/head_rgbd_sensor/rgb/image_raw' +depth_camera = False if sys.argv[2] == "False" else True +save_data = False if sys.argv[3] == "False" else True + + +class PersonDetection: + """ Class for the person detection node.""" + def __init__(self) -> None: + # Initialize YOLO + model_path = "~/MEGA/developers/Donal/yolov8n-seg.pt" + device = "cuda" + self.model = YOLO(model_path).to(device) + self.person_class = 0 + + # ROS Initialize + rospy.init_node(NODE_NAME, anonymous=True) + self.subscriber_rgb = rospy.Subscriber(name_subscriber_RGB, Image, self.image_callback, queue_size=2) + self.subscriber_depth = rospy.Subscriber('/hero/head_rgbd_sensor/depth_registered/image_raw', Image, + self.depth_image_callback, queue_size=2) + + self.publisher = rospy.Publisher(TOPIC_PREFIX + 'person_detections', DetectedPerson, queue_size=5) + self.publisher_debug = rospy.Publisher(TOPIC_PREFIX + 'debug/segmented_image', Image, queue_size=5) + self.reset_service = rospy.Service(TOPIC_PREFIX + NODE_NAME + '/reset', Empty, self.reset) + + # Initialize variables + self.batch_nr = 0 + self.latest_image = None # To store the most recent image + self.latest_image_time = None + self.depth_images = [] + + def reset(self, request): + """ Reset all stored variables in Class to their default values.""" + self.batch_nr = 0 + self.latest_image = None + self.latest_image_time = None + self.depth_images = [] + return EmptyResponse() + + def image_callback(self, data: Image) -> None: + """ Store the latest image with its information.""" + if self.latest_image is not None: + self.latest_image = None + + self.latest_image = data + self.latest_image_time = data.header.stamp.secs + self.batch_nr = data.header.seq + + def depth_image_callback(self, data: Image) -> None: + """ Store the latest depth image. Only the most recent depth images are stored.""" + while len(self.depth_images) > 50: + self.depth_images.pop(0) + self.depth_images.append(data) + + @staticmethod + def detect(model, frame): + """ Return class, contour and bounding box of objects in image per class type. """ + results = model(frame, verbose=False) + if results and len(results[0]) > 0: + segmentation_contours_idx = [np.array(seg, dtype=np.int32) for seg in results[0].masks.xy] + class_ids = np.array(results[0].boxes.cls.cpu(), dtype="int") + + # Get bounding box corners for each detected object + bounding_box_corners = [(int(x1), int(y1), int(x2), int(y2)) for x1, y1, x2, y2 in results[0].boxes.xyxy] + + return class_ids, segmentation_contours_idx, bounding_box_corners + else: + return None, None, None + + @staticmethod + def key_distance(x: int, middle_image: int = 320) -> int: + """ Return the x-distance between input x-coordinate and center of the image.""" + return abs(x - middle_image) + + def process_latest_image(self) -> None: + """ Extract persons with position information from image and publish it to the topic.""" + + if self.latest_image is None: + return + latest_image = self.latest_image + latest_image_time = self.latest_image_time + batch_nr = self.batch_nr + + # Import RGB Image + bridge = CvBridge() + cv_image = bridge.imgmsg_to_cv2(latest_image, desired_encoding='passthrough') + cv_image = cv2.GaussianBlur(cv_image, (5, 5), 0) + # if not laptop: + # cv_image = cv2.cvtColor(cv_image, cv2.COLOR_RGB2BGR) + + # Import Depth Image + if depth_camera: + cv_depth_image = bridge.imgmsg_to_cv2(self.depth_images[-1], desired_encoding='passthrough') + else: + cv_depth_image = None + + if save_data: + cv2.imwrite(f"{save_path}{batch_nr}.png", cv_image) + if depth_camera: + cv2.imwrite(f"{save_path}{batch_nr}_depth.png", cv_depth_image) + + # People detection + classes, segmentations, bounding_box_corners = self.detect(self.model, cv_image) + if classes is None or segmentations is None: # Return if no persons detected in image + self.latest_image = None + self.latest_image_time = None + return + + detected_persons = [] + x_positions = [] + y_positions = [] + z_positions = [] + nr_persons = 0 + + for class_id, seg, box in zip(classes, segmentations, bounding_box_corners): + x1, y1, x2, y2 = box + + if class_id == self.person_class: + mask = np.zeros_like(cv_image) + nr_persons += 1 + cv2.fillPoly(mask, [seg], (255, 255, 255)) + cv_image[mask == 0] = 0 + cropped_image = cv_image[y1:y2, x1:x2] + image_message = bridge.cv2_to_imgmsg(cropped_image, encoding="passthrough") + x = int(x1 + ((x2 - x1) / 2)) + y = int(y1 + ((y2 - y1) / 2)) + + if depth_camera: + # mask_depth = np.zeros_like(cv_depth_image, dtype=np.uint8) + # cv2.fillPoly(mask_depth, [seg], (255, 255, 255)) + # + # # Extract the values based on the mask + # masked_pixels = cv_depth_image[mask_depth] + # + # median_color = np.median(masked_pixels) + # print("Median color:", median_color) + # + # cv_depth_image[mask_depth == 0] = 0 + # depth_cropped = cv_depth_image[y1:y2, x1:x2] + # average_color = cv2.mean(cv_depth_image, mask=mask_depth) + # rospy.loginfo(f"color {int(average_color[0])}") + roi_size = 5 + roi_x1 = max(0, x - roi_size // 2) + roi_y1 = max(0, y - roi_size // 2) + roi_x2 = min(cv_depth_image.shape[1], x + roi_size // 2) + roi_y2 = min(cv_depth_image.shape[0], y + roi_size // 2) + depth_roi = cv_depth_image[roi_y1:roi_y2, roi_x1:roi_x2] + z_depth_roi = np.median(depth_roi) + + z = z_depth_roi + rospy.loginfo(f"Z-coord: {z}") + else: + z = 0 + + detected_persons.append(image_message) + x_positions.append(x) + y_positions.append(y) + z_positions.append(int(z)) + + # Sort entries from smallest to largest x_distance from the center + sorted_idx = sorted(range(len(x_positions)), key=lambda k: self.key_distance(x_positions[k])) + detected_persons = [detected_persons[i] for i in sorted_idx] + x_positions = [x_positions[i] for i in sorted_idx] + y_positions = [y_positions[i] for i in sorted_idx] + z_positions = [z_positions[i] for i in sorted_idx] + + # Create and Publish person_detections msg + msg = DetectedPerson() + msg.time = float(latest_image_time) + msg.nr_batch = batch_nr + msg.nr_persons = nr_persons + msg.detected_persons = detected_persons + msg.x_positions = x_positions + msg.y_positions = y_positions + msg.z_positions = z_positions + self.publisher.publish(msg) + + for image_message in detected_persons: # Publish the segmented images of detected persons + self.publisher_debug.publish(image_message) + + self.latest_image = None # Clear the latest image after processing + self.latest_image_time = None + + def main_loop(self): + """ Loop to process most recent images. """ + while not rospy.is_shutdown(): + self.process_latest_image() + rospy.sleep(0.001) + + +if __name__ == '__main__': + if save_data: + try: + rospack = rospkg.RosPack() + package_path = rospack.get_path("people_tracking") + time = time.ctime(time.time()) + save_path = os.path.join(package_path, f'data/{time}_test/') + os.makedirs(save_path, exist_ok=True) # Make sure the directory exists + print(save_path) + except: + rospy.loginfo("Failed to make save path") + pass + + try: + print(f"Use Depth: {depth_camera}, Camera Source: {name_subscriber_RGB}") + node_pd = PersonDetection() + node_pd.main_loop() + except rospy.exceptions.ROSInterruptException: + rospy.loginfo("Failed to launch Person Detection Node") + pass diff --git a/people_tracking_v2/src/people_tracking/webcam.py b/people_tracking_v2/src/people_tracking/webcam.py new file mode 100755 index 0000000..d6dacd2 --- /dev/null +++ b/people_tracking_v2/src/people_tracking/webcam.py @@ -0,0 +1,58 @@ +#!/usr/bin/env python3 +# Video from built-in webcam laptop +# Adapted from https://automaticaddison.com/working-with-ros-and-opencv-in-ros-noetic/ + +import sys +import rospy +import cv2 +from cv_bridge import CvBridge + +from sensor_msgs.msg import Image +from std_msgs.msg import Header + +laptop = sys.argv[1] + +def publish_message(): + rospy.init_node('video_pub_py', anonymous=True) + pub = rospy.Publisher('video_frames', Image, queue_size=10) + + rate = rospy.Rate(30) + + # Create a VideoCapture object + cap = cv2.VideoCapture(0) # The argument '0' gets the default webcam. + br = CvBridge() + seq = 0 + while not rospy.is_shutdown(): + # Capture frame-by-frame. This method returns True/False as well as the video frame. + ret, frame = cap.read() + + if ret: + seq += 1 + header = Header() + header.seq = seq + header.stamp = rospy.Time.now() + header.frame_id = 'camera_frame' # You can modify the frame_id as needed + + # Create the Image message + img_msg = Image() + img_msg.header = header + img_msg.height = frame.shape[0] + img_msg.width = frame.shape[1] + img_msg.encoding = 'bgr8' # You may need to adjust the encoding based on your requirements + img_msg.is_bigendian = 0 + img_msg.step = frame.shape[1] * 3 # Assuming 3 channels (BGR) + + # Flatten the image array and assign it to the data field + img_msg.data = frame.flatten().tolist() + + pub.publish(img_msg) + + rate.sleep() + +if __name__ == '__main__': + if laptop == "True": + try: + publish_message() + except rospy.ROSInterruptException: + pass + rospy.spin()