More testing

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}"

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:

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:

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)

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

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()