From 83d91215361f67d493b7db30f56b198232d391c5 Mon Sep 17 00:00:00 2001 From: Miguelmelon Date: Fri, 14 Jun 2024 15:21:28 +0200 Subject: [PATCH] Included getting operator data from initialisation --- people_tracking_v2/scripts/HoC.py | 21 +- people_tracking_v2/scripts/comparison_node.py | 194 ++++++++++-------- people_tracking_v2/tools/plotter.py | 11 +- 3 files changed, 133 insertions(+), 93 deletions(-) diff --git a/people_tracking_v2/scripts/HoC.py b/people_tracking_v2/scripts/HoC.py index 8f80ab0..a580a21 100755 --- a/people_tracking_v2/scripts/HoC.py +++ b/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() diff --git a/people_tracking_v2/scripts/comparison_node.py b/people_tracking_v2/scripts/comparison_node.py index 83ad066..592b6f6 100755 --- a/people_tracking_v2/scripts/comparison_node.py +++ b/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() diff --git a/people_tracking_v2/tools/plotter.py b/people_tracking_v2/tools/plotter.py index 0c9db0a..06ac340 100644 --- a/people_tracking_v2/tools/plotter.py +++ b/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')