Included getting operator data from initialisation

people_tracking_v2/scripts/HoC.py

@@ -60,9 +60,9 @@ def segmented_images_callback(self, msg):
                 # Create HoCVector message
                 hoc_vector = HoCVector()
                 hoc_vector.id = detection_id
-                hoc_vector.hue_vector = hoc_hue.tolist()
-                hoc_vector.sat_vector = hoc_sat.tolist()
-                hoc_vector.val_vector = hoc_val.tolist()
+                hoc_vector.hue_vector = self.normalize_vector(hoc_hue).tolist()
+                hoc_vector.sat_vector = self.normalize_vector(hoc_sat).tolist()
+                hoc_vector.val_vector = self.normalize_vector(hoc_val).tolist()
                 hoc_vectors.vectors.append(hoc_vector)
 
                 # Log the resulting values
@@ -91,13 +91,22 @@ def compute_hoc(self, segmented_image):
         hist_sat = cv2.calcHist([hsv], [1], mask, [bins], [0, 256])
         hist_val = cv2.calcHist([hsv], [2], mask, [bins], [0, 256])
 
-        cv2.normalize(hist_hue, hist_hue)
-        cv2.normalize(hist_sat, hist_sat)
-        cv2.normalize(hist_val, hist_val)
+        # Normalize histograms
+        hist_hue = self.normalize_vector(hist_hue)
+        hist_sat = self.normalize_vector(hist_sat)
+        hist_val = self.normalize_vector(hist_val)
 
         # Flatten the histograms
         return hist_hue.flatten(), hist_sat.flatten(), hist_val.flatten()
 
+    def normalize_vector(self, vector):
+        """Normalize a vector to ensure the sum of elements is 1."""
+        norm_vector = np.array(vector)
+        norm_sum = np.sum(norm_vector)
+        if norm_sum == 0:
+            return norm_vector
+        return norm_vector / norm_sum
+
 if __name__ == '__main__':
     try:
         HoCNode()

people_tracking_v2/scripts/comparison_node.py

@@ -22,107 +22,131 @@ def __init__(self):
         # Publisher for comparison scores
         self.comparison_pub = rospy.Publisher('/comparison/scores_array', ComparisonScoresArray, queue_size=10)
 
-        # Load saved HoC and Pose data
-        self.hoc_data_file = os.path.expanduser('~/hoc_data/hoc_data.npz')
-        self.pose_data_file = os.path.expanduser('~/pose_data/pose_data.npz')
-        self.load_hoc_data()
-        self.load_pose_data()
+        # Initialize variables for operator data
+        self.init_phase = True
+        self.init_duration = 10.0  # seconds
+        self.start_time = rospy.get_time()
+        self.hue_vectors = []
+        self.sat_vectors = []
+        self.val_vectors = []
+        self.pose_data = []
+
+        self.operator_hue_avg = None
+        self.operator_sat_avg = None
+        self.operator_val_avg = None
+        self.operator_pose_median = None
+        self.operator_data_set = False
+
+        # Define the file paths for saving the data
+        self.operator_npz_file_path = os.path.expanduser('~/hoc_data/operator_data.npz')
+        self.detection_npz_file_path = os.path.expanduser('~/hoc_data/latest_detection_1_data.npz')
 
         rospy.spin()
-
-    def load_hoc_data(self):
-        """Load the saved HoC data from the .npz file (HoC)."""
-        if os.path.exists(self.hoc_data_file):
-            data = np.load(self.hoc_data_file)
-            self.saved_hue = data['hue']
-            self.saved_sat = data['sat']
-            self.saved_val = data['val']
-            rospy.loginfo(f"Loaded HoC data from {self.hoc_data_file}")
-        else:
-            rospy.logerr(f"HoC data file {self.hoc_data_file} not found")
-            self.saved_hue = None
-            self.saved_sat = None
-            self.saved_val = None
-
-    def load_pose_data(self):
-        """Load the saved Pose data from the .npz file (Pose)."""
-        if os.path.exists(self.pose_data_file):
-            data = np.load(self.pose_data_file)
-            self.saved_pose_data = {
-                'head_feet_distance': data['head_feet_distance']
-            }
-            rospy.loginfo(f"Loaded Pose data from {self.pose_data_file}")
-        else:
-            rospy.logerr(f"Pose data file {self.pose_data_file} not found")
-            self.saved_pose_data = None
 
     def sync_callback(self, hoc_array, pose_array):
         """Callback function to handle synchronized HoC and pose data."""
         rospy.loginfo("sync_callback invoked")
 
-        if self.saved_hue is None or self.saved_sat is None or self.saved_val is None:
-            rospy.logerr("No saved HoC data available for comparison")
-            return
-
-        if not hoc_array.vectors or not pose_array.distances:
-            rospy.logerr("Received empty HoC or Pose array")
-            return
-
-        comparison_scores_array = ComparisonScoresArray()
-        comparison_scores_array.header.stamp = hoc_array.header.stamp
-
-        for hoc_msg, pose_msg in zip(hoc_array.vectors, pose_array.distances):
-            rospy.loginfo(f"Processing Detection ID {hoc_msg.id}")
-
-            # Create and append ComparisonScores message
-            comparison_scores_msg = ComparisonScores()
-            comparison_scores_msg.header.stamp = hoc_msg.header.stamp  # Use the timestamp from the HoC message
-            comparison_scores_msg.header.frame_id = hoc_msg.header.frame_id
-            comparison_scores_msg.id = hoc_msg.id
-
-            # Compare HoC data
-            hue_vector = hoc_msg.hue_vector
-            sat_vector = hoc_msg.sat_vector
-            val_vector = hoc_msg.val_vector
-            hoc_distance_score = self.compute_hoc_distance_score(hue_vector, sat_vector, val_vector)
-            rospy.loginfo(f"Detection ID {hoc_msg.id}: HoC Distance score: {hoc_distance_score:.2f}")
-            comparison_scores_msg.hoc_distance_score = hoc_distance_score
-
-            # Compare pose data or set to -1 if pose distance is negative
-            head_feet_distance = pose_msg.head_feet_distance
-            if head_feet_distance < 0:
-                rospy.loginfo(f"Skipping comparison for Detection ID {hoc_msg.id} due to negative pose distance")
-                comparison_scores_msg.pose_distance_score = -1
-            else:
-                head_feet_saved = np.mean(self.saved_pose_data['head_feet_distance'])
-                distance_score = self.compute_distance(head_feet_distance, head_feet_saved)
-                rospy.loginfo(f"Detection ID {pose_msg.id}: Pose Distance score: {distance_score:.2f}")
-                comparison_scores_msg.pose_distance_score = distance_score  # Save head-feet distance as pose_distance_score
-
-            # Log the scores
-            rospy.loginfo(f"Publishing scores - Detection ID {comparison_scores_msg.id}: HoC Distance score: {comparison_scores_msg.hoc_distance_score:.2f}, Pose Distance score: {comparison_scores_msg.pose_distance_score:.2f}")
-
-            comparison_scores_array.scores.append(comparison_scores_msg)
-
-        # Publish the comparison scores as a batch
-        self.comparison_pub.publish(comparison_scores_array)
+        current_time = rospy.get_time()
+        if self.init_phase:
+            # Accumulate data during initialization phase
+            for hoc_msg, pose_msg in zip(hoc_array.vectors, pose_array.distances):
+                if hoc_msg.id == 1:
+                    self.hue_vectors.append(hoc_msg.hue_vector)
+                    self.sat_vectors.append(hoc_msg.sat_vector)
+                    self.val_vectors.append(hoc_msg.val_vector)
+                    self.pose_data.append(pose_msg.head_feet_distance)
+                    rospy.loginfo(f"Accumulating data for detection ID 1: HoC and Pose data")
+
+            # Check if initialization phase is over
+            if current_time - self.start_time > self.init_duration:
+                self.init_phase = False
+                # Calculate the median for pose data and normalized average for HoC data
+                self.operator_pose_median = np.median(self.pose_data)
+                self.operator_hue_avg = self.normalize_vector(np.mean(self.hue_vectors, axis=0))
+                self.operator_sat_avg = self.normalize_vector(np.mean(self.sat_vectors, axis=0))
+                self.operator_val_avg = self.normalize_vector(np.mean(self.val_vectors, axis=0))
+                self.operator_data_set = True
+                rospy.loginfo("Operator data initialized with median pose and normalized average HoC values.")
+
+                # Save the accumulated operator data to file
+                self.save_operator_data()
+        else:
+            if not self.operator_data_set:
+                rospy.logerr("Operator data not yet set, waiting for initialization to complete")
+                return
+
+            if not hoc_array.vectors or not pose_array.distances:
+                rospy.logerr("Received empty HoC or Pose array")
+                return
+
+            comparison_scores_array = ComparisonScoresArray()
+            comparison_scores_array.header.stamp = hoc_array.header.stamp
+
+            for hoc_msg, pose_msg in zip(hoc_array.vectors, pose_array.distances):
+                rospy.loginfo(f"Processing Detection ID {hoc_msg.id}")
+
+                comparison_scores_msg = ComparisonScores()
+                comparison_scores_msg.header.stamp = hoc_msg.header.stamp
+                comparison_scores_msg.header.frame_id = hoc_msg.header.frame_id
+                comparison_scores_msg.id = hoc_msg.id
+
+                hue_vector = np.array(hoc_msg.hue_vector)
+                sat_vector = np.array(hoc_msg.sat_vector)
+                val_vector = np.array(hoc_msg.val_vector)
+                hoc_distance_score = self.compute_hoc_distance_score(hue_vector, sat_vector, val_vector)
+                rospy.loginfo(f"Detection ID {hoc_msg.id}: HoC Distance score: {hoc_distance_score:.2f}")
+                comparison_scores_msg.hoc_distance_score = hoc_distance_score
+
+                head_feet_distance = pose_msg.head_feet_distance
+                if head_feet_distance < 0:
+                    rospy.loginfo(f"Skipping comparison for Detection ID {hoc_msg.id} due to negative pose distance")
+                    comparison_scores_msg.pose_distance_score = -1
+                else:
+                    distance_score = self.compute_distance(head_feet_distance, self.operator_pose_median)
+                    rospy.loginfo(f"Detection ID {pose_msg.id}: Pose Distance score: {distance_score:.2f}")
+                    comparison_scores_msg.pose_distance_score = distance_score
+
+                rospy.loginfo(f"Publishing scores - Detection ID {comparison_scores_msg.id}: HoC Distance score: {comparison_scores_msg.hoc_distance_score:.2f}, Pose Distance score: {comparison_scores_msg.pose_distance_score:.2f}")
+
+                comparison_scores_array.scores.append(comparison_scores_msg)
+
+                # Save the latest data for detection ID 1
+                if hoc_msg.id == 1:
+                    self.save_latest_detection_data(hue_vector, sat_vector, val_vector, head_feet_distance)
+
+            self.comparison_pub.publish(comparison_scores_array)
+
+    def normalize_vector(self, vector):
+        """Normalize a vector to ensure the sum of elements is 1."""
+        norm_vector = np.array(vector)
+        norm_sum = np.sum(norm_vector)
+        if norm_sum == 0:
+            return norm_vector
+        return norm_vector / norm_sum
 
     def compute_hoc_distance_score(self, hue_vector, sat_vector, val_vector):
         """Compute the distance score between the current detection and saved data (HoC)."""
-        hue_vector = np.array(hue_vector)
-        sat_vector = np.array(sat_vector)
-        val_vector = np.array(val_vector)
-
-        hue_distance = self.compute_distance(hue_vector, self.saved_hue)
-        sat_distance = self.compute_distance(sat_vector, self.saved_sat)
-        val_distance = self.compute_distance(val_vector, self.saved_val)
+        hue_distance = self.compute_distance(hue_vector, self.operator_hue_avg)
+        sat_distance = self.compute_distance(sat_vector, self.operator_sat_avg)
+        val_distance = self.compute_distance(val_vector, self.operator_val_avg)
 
         return (hue_distance + sat_distance + val_distance)
 
     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):
+        """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):
+        """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):
         """Publish debug information about the current comparison (General)."""
         debug_msg = String()

people_tracking_v2/tools/plotter.py

@@ -3,33 +3,40 @@
 import os
 
 # Expand the user directory
-npz_file_path = os.path.expanduser('~/hoc_data/hoc_data.npz')
+#npz_file_path = os.path.expanduser('~/hoc_data/latest_detection_1_data.npz')
+npz_file_path = os.path.expanduser('~/hoc_data/operator_data.npz')
 
 # Load the HoC arrays from the .npz file
 with np.load(npz_file_path) as data:
     all_hue_histograms = data['hue']
     all_sat_histograms = data['sat']
+    all_val_histograms = data['val']
 
 # Ensure we are dealing with the correct batch size
 print("Number of Hue Histograms:", len(all_hue_histograms))
 print("Number of Saturation Histograms:", len(all_sat_histograms))
+print("Number of Value Histograms:", len(all_val_histograms))
 
 # Select the first histogram for plotting (or change index as needed)
 hoc_hue = all_hue_histograms
 hoc_sat = all_sat_histograms
+hoc_val = all_val_histograms
 
 # Print some statistics about the histograms
 print("Hue Histogram - Min:", np.min(hoc_hue), "Max:", np.max(hoc_hue), "Mean:", np.mean(hoc_hue))
 print("Saturation Histogram - Min:", np.min(hoc_sat), "Max:", np.max(hoc_sat), "Mean:", np.mean(hoc_sat))
+print("Value Histogram - Min:", np.min(hoc_val), "Max:", np.max(hoc_val), "Mean:", np.mean(hoc_val))
 
 # Print first 10 values of the histograms
 print("First 10 values of Hue Histogram:", hoc_hue[:10])
 print("First 10 values of Saturation Histogram:", hoc_sat[:10])
+print("First 10 values of Value Histogram:", hoc_val[:10])
 
-# Plot the Hue and Saturation HoC arrays
+# Plot the Hue, Saturation, and Value HoC arrays
 plt.figure(figsize=(10, 6))
 plt.plot(hoc_hue, label='Hue')
 plt.plot(hoc_sat, label='Saturation')
+plt.plot(hoc_val, label='Value')
 plt.title('Histogram of Colors (HoC)')
 plt.xlabel('Bins')
 plt.ylabel('Frequency')