First attempt correcting HoC

people_tracking_v2/CMakeLists.txt

@@ -17,6 +17,7 @@ add_message_files(
   FILES
   Detection.msg
   DetectionArray.msg
+  SegmentedImages.msg
 )
 
 generate_messages(

people_tracking_v2/msg/SegmentedImages.msg

@@ -0,0 +1,2 @@
+std_msgs/Header header
+sensor_msgs/Image[] images

people_tracking_v2/scripts/HoC.py

@@ -1,47 +1,62 @@
 #!/usr/bin/env python
 
 import rospy
-from sensor_msgs.msg import Image
-from people_tracking_v2.msg import DetectionArray  # Assuming you have a custom message type for YOLO detections
+from people_tracking_v2.msg import SegmentedImages  # Custom message for batch segmented images
 from cv_bridge import CvBridge, CvBridgeError
 import cv2
 import numpy as np
+import os
 
 class HoCNode:
     def __init__(self):
         rospy.init_node('hoc_node', anonymous=True)
 
         self.bridge = CvBridge()
-        self.segmented_image_sub = rospy.Subscriber('/segmented_images', Image, self.segmented_image_callback)
-        self.detection_sub = rospy.Subscriber('/yolo_detections', DetectionArray, self.detection_callback)
+        self.segmented_images_sub = rospy.Subscriber('/segmented_images', SegmentedImages, self.segmented_images_callback)
 
-        self.segmented_images = []
+        # Ensure the directory exists
+        self.hoc_data_dir = os.path.expanduser('~/hoc_data')
+        if not os.path.exists(self.hoc_data_dir):
+            os.makedirs(self.hoc_data_dir)
+            rospy.loginfo(f"Created directory: {self.hoc_data_dir}")
+        else:
+            rospy.loginfo(f"Using existing directory: {self.hoc_data_dir}")
 
         rospy.spin()
 
-    def segmented_image_callback(self, msg):
-        try:
-            segmented_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
-            self.segmented_images.append(segmented_image)
-        except CvBridgeError as e:
-            rospy.logerr(e)
-
-    def detection_callback(self, msg):
-        if not self.segmented_images:
-            return
-
-        for i, (detection, segmented_image) in enumerate(zip(msg.detections, self.segmented_images)):
-            hoc = self.compute_hoc(segmented_image)
-            rospy.loginfo(f'HoC for detection #{i + 1}: {hoc}')
-
-        # Clear the segmented images list after processing
-        self.segmented_images = []
-
-    def compute_hoc(self, image):
-        hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
-        hist = cv2.calcHist([hsv], [0, 1], None, [180, 256], [0, 180, 0, 256])
-        cv2.normalize(hist, hist)
-        return hist.flatten()
+    def segmented_images_callback(self, msg):
+        rospy.loginfo(f"Received batch of {len(msg.images)} segmented images")
+        for i, segmented_image_msg in enumerate(msg.images):
+            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}')
+
+                # Save the HoC data
+                hue_save_path = os.path.join(self.hoc_data_dir, f'hoc_hue_detection_{i + 1}.npy')
+                sat_save_path = os.path.join(self.hoc_data_dir, f'hoc_sat_detection_{i + 1}.npy')
+                np.save(hue_save_path, hoc_hue)
+                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}')
+            except CvBridgeError as e:
+                rospy.logerr(f"Failed to convert segmented image: {e}")
+
+    def compute_hoc(self, segmented_image):
+        hsv = cv2.cvtColor(segmented_image, cv2.COLOR_BGR2HSV)
+
+        # Compute histogram for Hue (180 bins) and Saturation (256 bins)
+        hist_hue = cv2.calcHist([hsv], [0], None, [180], [0, 180])
+        hist_sat = cv2.calcHist([hsv], [1], None, [256], [0, 256])
+
+        cv2.normalize(hist_hue, hist_hue)
+        cv2.normalize(hist_sat, hist_sat)
+
+        # Flatten the histograms
+        return hist_hue.flatten(), hist_sat.flatten()
 
 if __name__ == '__main__':
     try:

people_tracking_v2/scripts/plotter.py

@@ -0,0 +1,21 @@
+import numpy as np
+import matplotlib.pyplot as plt
+import os
+
+# Expand the user directory
+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)
+hoc_sat = np.load(sat_file_path)
+
+# Plot the Hue and Saturation HoC arrays
+plt.figure(figsize=(10, 6))
+plt.plot(hoc_hue, label='Hue')
+plt.plot(hoc_sat, label='Saturation')
+plt.title('Histogram of Colors (HoC)')
+plt.xlabel('Bins')
+plt.ylabel('Frequency')
+plt.legend()
+plt.show()

people_tracking_v2/scripts/yolo_seg.py

@@ -5,7 +5,7 @@
 import numpy as np
 from ultralytics import YOLO
 from sensor_msgs.msg import Image
-from people_tracking_v2.msg import Detection, DetectionArray
+from people_tracking_v2.msg import Detection, DetectionArray, SegmentedImages
 from cv_bridge import CvBridge, CvBridgeError
 
 class YoloSegNode:
@@ -14,9 +14,8 @@ def __init__(self):
         self.model = YOLO("yolov8n-seg.pt")  # Ensure the model supports segmentation
 
         self.image_sub = rospy.Subscriber("/Webcam/image_raw", Image, self.image_callback)
-        self.segmented_image_pub = rospy.Publisher("/segmented_images", Image, queue_size=10)
+        self.segmented_images_pub = rospy.Publisher("/segmented_images", SegmentedImages, queue_size=10)
         self.bounding_box_image_pub = rospy.Publisher("/bounding_box_image", Image, queue_size=10)
-        self.detection_pub = rospy.Publisher("/yolo_detections", DetectionArray, queue_size=10)
 
     def image_callback(self, data):
         try:
@@ -41,6 +40,10 @@ def image_callback(self, data):
         # Create a copy of the image for bounding box visualization
         bounding_box_image = cv_image.copy()
 
+        # Prepare the SegmentedImages message
+        segmented_images_msg = SegmentedImages()
+        segmented_images_msg.header.stamp = rospy.Time.now()
+
         # Process each detection and create a Detection message, but only for humans (class 0)
         human_detections = [(box, score, label, mask) for box, score, label, mask in zip(boxes, scores, labels, masks) if int(label) == 0]
         rospy.loginfo(f"Human Detections: {len(human_detections)}")  # Log the number of human detections
@@ -66,23 +69,19 @@ def image_callback(self, data):
                 cv2.FONT_HERSHEY_SIMPLEX, 0.5, color, 1
             )
 
-            # Publish each segmented image individually
+            # Apply segmentation mask to the cv_image
             if mask is not None:
                 # Resize mask to match the original image dimensions
                 resized_mask = cv2.resize(mask, (cv_image.shape[1], cv_image.shape[0]))
                 resized_mask = resized_mask.astype(np.uint8)
 
-                # Apply the mask to the original image
+                # Apply mask to the original image
                 segmented_image = cv2.bitwise_and(cv_image, cv_image, mask=resized_mask)
-
-                try:
-                    segmented_image_msg = self.bridge.cv2_to_imgmsg(segmented_image, "bgr8")
-                    self.segmented_image_pub.publish(segmented_image_msg)
-                except CvBridgeError as e:
-                    rospy.logerr(e)
-
-        # Publish detection results
-        self.detection_pub.publish(detection_array)
+                segmented_image_msg = self.bridge.cv2_to_imgmsg(segmented_image, "bgr8")
+                segmented_images_msg.images.append(segmented_image_msg)
+
+        # Publish segmented images as a batch
+        self.segmented_images_pub.publish(segmented_images_msg)
 
         # Publish bounding box image
         try:

people_tracking_v2/src/people_tracking/launchers/yolo_launcher.launch

@@ -18,13 +18,6 @@
     output="screen"
   />
 
-  <node
-    pkg="people_tracking_v2"
-    type="yolo.py"
-    name="People_detection"
-    output="screen"
-  />
-
   <node
     pkg="people_tracking_v2"
     type="yolo_seg.py"
@@ -55,5 +48,11 @@
     output="screen"
   />
   
+    <node
+    pkg="people_tracking_v2"
+    type="yolo.py"
+    name="People_detection"
+    output="screen"
+  />
   -->
 </launch>