Predicted vs Detected person IoU Implementation

people_tracking_v2/scripts/HoC.py

@@ -30,8 +30,8 @@ def segmented_images_callback(self, msg):
             try:
                 segmented_image = self.bridge.imgmsg_to_cv2(segmented_image_msg, "bgr8")
                 hoc_hue, hoc_sat = self.compute_hoc(segmented_image)
-                rospy.loginfo(f'HoC for segmented image #{i + 1} (Hue): {hoc_hue}')
-                rospy.loginfo(f'HoC for segmented image #{i + 1} (Saturation): {hoc_sat}')
+                #rospy.loginfo(f'HoC for segmented image #{i + 1} (Hue): {hoc_hue}')
+                #rospy.loginfo(f'HoC for segmented image #{i + 1} (Saturation): {hoc_sat}')
 
                 # Save the HoC data
                 hue_save_path = os.path.join(self.hoc_data_dir, f'hoc_hue_detection_{i + 1}.npy')
@@ -40,8 +40,8 @@ def segmented_images_callback(self, msg):
                 np.save(sat_save_path, hoc_sat)
 
                 # Print statements to verify file saving
-                rospy.loginfo(f'Saved Hue HoC to {hue_save_path}')
-                rospy.loginfo(f'Saved Sat HoC to {sat_save_path}')
+                #rospy.loginfo(f'Saved Hue HoC to {hue_save_path}')
+                #rospy.loginfo(f'Saved Sat HoC to {sat_save_path}')
             except CvBridgeError as e:
                 rospy.logerr(f"Failed to convert segmented image: {e}")
 

people_tracking_v2/scripts/plotter.py

@@ -3,8 +3,8 @@
 import os
 
 # Expand the user directory
-hue_file_path = os.path.expanduser('~/hoc_data/hoc_hue_detection_8.npy')
-sat_file_path = os.path.expanduser('~/hoc_data/hoc_sat_detection_8.npy')
+hue_file_path = os.path.expanduser('~/hoc_data/hoc_hue_detection_1.npy')
+sat_file_path = os.path.expanduser('~/hoc_data/hoc_sat_detection_1.npy')
 
 # Load the HoC arrays from the saved files
 hoc_hue = np.load(hue_file_path)

people_tracking_v2/scripts/yolo_seg.py

@@ -85,12 +85,25 @@ def image_callback(self, data):
             kalman_filter.predict()
             x_pred, y_pred = kalman_filter.get_state()[:2]
 
+            # Calculate the predicted bounding box based on the predicted center and original box size
+            box_width = x2 - x1
+            box_height = y2 - y1
+            x_pred1, y_pred1 = int(x_pred - box_width / 2), int(y_pred - box_height / 2)
+            x_pred2, y_pred2 = int(x_pred + box_width / 2), int(y_pred + box_height / 2)
+
+            # Draw predicted bounding box
+            cv2.rectangle(bounding_box_image, (x_pred1, y_pred1), (x_pred2, y_pred2), (255, 0, 0), 2)  # Blue box
+
             # Draw predicted position
             cv2.circle(bounding_box_image, (int(x_pred), int(y_pred)), 5, (255, 0, 0), -1)
 
+            # Calculate IoU
+            iou = self.calculate_iou([x1, y1, x2, y2], [x_pred1, y_pred1, x_pred2, y_pred2])
+            rospy.loginfo(f"Detection {i}: IoU={iou:.2f}")
+
             color = (0, 255, 0)  # Set color for bounding boxes
             thickness = 3
-            label_text = f'#{i+1} {int(label)}: {score:.2f}'
+            label_text = f'#{i+1} {int(label)}: {score:.2f} IoU={iou:.2f}'
             cv2.rectangle(bounding_box_image, (x1, y1), (x2, y2), color, thickness)
             cv2.putText(
                 bounding_box_image, label_text, (x1, y1 - 10),
@@ -109,7 +122,7 @@ def image_callback(self, data):
                 segmented_images_msg.images.append(segmented_image_msg)
 
         # Publish segmented images as a batch
-            self.segmented_images_pub.publish(segmented_images_msg)
+        self.segmented_images_pub.publish(segmented_images_msg)
 
         # Publish bounding box image
         try:
@@ -122,6 +135,26 @@ def image_callback(self, data):
         # Publish predicted detections
         self.detection_pub.publish(detection_array)
 
+    def calculate_iou(self, box1, box2):
+        """Calculate the Intersection over Union (IoU) of two bounding boxes."""
+        x1_min, y1_min, x1_max, y1_max = box1
+        x2_min, y2_min, x2_max, y2_max = box2
+
+        # Calculate intersection
+        inter_x_min = max(x1_min, x2_min)
+        inter_y_min = max(y1_min, y2_min)
+        inter_x_max = min(x1_max, x2_max)
+        inter_y_max = min(y1_max, y2_max)
+
+        inter_area = max(0, inter_x_max - inter_x_min) * max(0, inter_y_max - inter_y_min)
+
+        # Calculate union
+        box1_area = (x1_max - x1_min) * (y1_max - y1_min)
+        box2_area = (x2_max - x2_min) * (y2_max - y2_min)
+        union_area = box1_area + box2_area - inter_area
+
+        return inter_area / union_area if union_area > 0 else 0
+
 def main():
     rospy.init_node('yolo_seg_node', anonymous=True)
     yolo_node = YoloSegNode()