diff --git a/people_tracking_v2/scripts/comparison_node.py b/people_tracking_v2/scripts/comparison_node.py index 22ddbb8..95912ee 100755 --- a/people_tracking_v2/scripts/comparison_node.py +++ b/people_tracking_v2/scripts/comparison_node.py @@ -147,17 +147,17 @@ def compute_distance(self, vector1, vector2): """Compute the Euclidean distance between two vectors (General).""" return np.linalg.norm(vector1 - vector2) - def save_operator_data(self): + #def save_operator_data(self): """Save the accumulated operator data to an .npz file.""" np.savez(self.operator_npz_file_path, hue=self.operator_hue_avg, sat=self.operator_sat_avg, val=self.operator_val_avg, pose=self.operator_pose_median) rospy.loginfo(f"Saved accumulated operator data to {self.operator_npz_file_path}") - def save_latest_detection_data(self, hue_vector, sat_vector, val_vector, head_feet_distance): + #def save_latest_detection_data(self, hue_vector, sat_vector, val_vector, head_feet_distance): """Save the latest data for detection ID 1 to an .npz file.""" np.savez(self.detection_npz_file_path, hue=hue_vector, sat=sat_vector, val=val_vector, pose=head_feet_distance) rospy.loginfo(f"Saved latest detection 1 data to {self.detection_npz_file_path}") - def publish_debug_info(self, hoc_distance_score, pose_distance_score, detection_id): + #def publish_debug_info(self, hoc_distance_score, pose_distance_score, detection_id): """Publish debug information about the current comparison (General).""" debug_msg = String() debug_msg.data = f"Detection ID {detection_id}: HoC Distance score: {hoc_distance_score:.2f}, Pose Distance score: {pose_distance_score:.2f}" diff --git a/people_tracking_v2/testing/depth_publisher.py b/people_tracking_v2/testing/depth_publisher.py index bc86cb3..c2ef5e5 100755 --- a/people_tracking_v2/testing/depth_publisher.py +++ b/people_tracking_v2/testing/depth_publisher.py @@ -42,7 +42,7 @@ def publish_depth_images_from_folder(folder_path, topic_name, video_duration): if __name__ == '__main__': rospy.init_node('depth_publisher_node', anonymous=True) - folder_path = '/home/miguel/Documents/BEP-Testing/data/Frames Fri Jun 28 Test case 4/depth' + folder_path = '/home/miguel/Documents/BEP-Testing/Test Case 1/Frames Sat Jun 29 Test Case 1/depth' topic_name = '/hero/head_rgbd_sensor/depth_registered/image_raw' video_duration = 122 # Duration of the video in seconds, modify as needed try: diff --git a/people_tracking_v2/testing/rgb_publisher.py b/people_tracking_v2/testing/rgb_publisher.py index 48f2086..20ed1d3 100755 --- a/people_tracking_v2/testing/rgb_publisher.py +++ b/people_tracking_v2/testing/rgb_publisher.py @@ -38,7 +38,7 @@ def publish_images_from_folder(folder_path): if __name__ == '__main__': rospy.init_node('rgb_image_folder_publisher_node', anonymous=True) - folder_path = '/home/miguel/Documents/BEP-Testing/data/Frames Fri Jun 28 Test case 4/rgb' + folder_path = '/home/miguel/Documents/BEP-Testing/Test Case 1/Frames Sat Jun 29 Test Case 1/rgb' try: publish_images_from_folder(folder_path) except rospy.ROSInterruptException: diff --git a/people_tracking_v2/tools/depth_converter.py b/people_tracking_v2/tools/depth_converter.py new file mode 100644 index 0000000..75c767f --- /dev/null +++ b/people_tracking_v2/tools/depth_converter.py @@ -0,0 +1,27 @@ +import cv2 +import os + +def save_image(image, prefix, index, subfolder='depth_png'): + directory = os.path.join(os.getcwd(), subfolder) + if not os.path.exists(directory): + os.makedirs(directory) + filename = f"{prefix}_{index}.png" + filepath = os.path.join(directory, filename) + cv2.imwrite(filepath, image) + print(f"Saved {filename} to {subfolder}/") + +def process_depth_image(image_path, output_index): + # Load the depth image + depth_image = cv2.imread(image_path, cv2.IMREAD_UNCHANGED) # Make sure to load it in the original bit depth + + # Normalize the depth image + depth_image_normalized = cv2.normalize(depth_image, None, 0, 255, cv2.NORM_MINMAX) + + # Convert to 8-bit + depth_image_8bit = cv2.convertScaleAbs(depth_image_normalized) + + # Save the 8-bit depth image + save_image(depth_image_8bit, 'depth', output_index) + +# Example usage +process_depth_image('/home/miguel/Documents/BEP-Testing/Test Case 1/Frames Sat Jun 29 Test Case 1/depth/depth_000422.png' , 1) diff --git a/people_tracking_v2/tools/recorder.py b/people_tracking_v2/tools/recorder.py index e6f59c2..e0e150d 100755 --- a/people_tracking_v2/tools/recorder.py +++ b/people_tracking_v2/tools/recorder.py @@ -15,7 +15,7 @@ def save_image(cv_image, image_type, count, subfolder=None): os.makedirs(base_dir) # Create the directory for today's date if it doesn't exist - date_str = datetime.now().strftime('%a %b %d Test case 4') + date_str = datetime.now().strftime('%a %b %d Test Case 2') output_dir = os.path.join(base_dir, f'Frames {date_str}') if not os.path.exists(output_dir): os.makedirs(output_dir) @@ -49,9 +49,9 @@ def callback(rgb_msg, depth_msg): # Save the depth image in original format and PNG format save_image(depth_image, 'depth', depth_count) - depth_image_normalized = cv2.normalize(depth_image, None, 0, 255, cv2.NORM_MINMAX) - depth_image_8bit = cv2.convertScaleAbs(depth_image_normalized) - save_image(depth_image_8bit, 'depth', depth_count, subfolder='depth_png') + #depth_image_normalized = cv2.normalize(depth_image, None, 0, 255, cv2.NORM_MINMAX) + #depth_image_8bit = cv2.convertScaleAbs(depth_image_normalized) + #save_image(depth_image_8bit, 'depth', depth_count, subfolder='depth_png') rgb_count += 1 depth_count += 1 diff --git a/people_tracking_v2/z_bep_plotter/bep_plotter.py b/people_tracking_v2/z_bep_plotter/bep_plotter.py new file mode 100644 index 0000000..e1d871a --- /dev/null +++ b/people_tracking_v2/z_bep_plotter/bep_plotter.py @@ -0,0 +1,61 @@ +import os +import cv2 +import matplotlib.pyplot as plt +import numpy as np + +# Path to the folder containing frames +folder_path = '/home/miguel/Documents/BEP-Testing/Test Case 1/Frames Sat Jun 29 Test Case 1/rgb' + +# Frame rate of the video +frame_rate = 18.7 + +# Get list of all frames +frames = sorted([os.path.join(folder_path, f) for f in os.listdir(folder_path) if f.endswith('.png')]) + +# Parameters for selecting frames +total_frames = len(frames) +selected_frame_indices = np.linspace(0, total_frames-1, 12, dtype=int) # Select 12 evenly spaced frames + +# List to store selected frames +selected_frames = [] + +for i in selected_frame_indices: + # Read the frame + frame = cv2.imread(frames[i]) + + # Convert BGR to RGB + frame_rgb = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB) + + # Calculate timestamp + timestamp = i / frame_rate + + # Put timestamp on frame + font = cv2.FONT_HERSHEY_SIMPLEX + text = f'{timestamp:.1f}' + text_size = cv2.getTextSize(text, font, 1.5, 2)[0] + text_x = frame_rgb.shape[1] - text_size[0] - 10 + text_y = 40 + cv2.putText(frame_rgb, text, (text_x, text_y), font, 1.5, (0, 0, 0), 3, cv2.LINE_AA) + + # Append frame to the list + selected_frames.append(frame_rgb) + +# Determine grid size for subplot +rows = int(np.ceil(len(selected_frames) / 4)) +cols = 4 + +# Plot frames in a grid +fig, axes = plt.subplots(rows, cols, figsize=(20, 5*rows)) +axes = axes.flatten() + +for ax, frame in zip(axes, selected_frames): + ax.imshow(frame) + ax.axis('off') + +# Hide any remaining empty subplots +for i in range(len(selected_frames), rows * cols): + fig.delaxes(axes[i]) + +plt.tight_layout() +plt.savefig('composite_image.png') +plt.show()