Included that if pose not visible simply do HoC

people_tracking_v2/scripts/comparison_node.py

@@ -72,26 +72,29 @@ def sync_callback(self, hoc_array, pose_array):
         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
             hoc_distance_score = self.compute_hoc_distance_score(hue_vector, sat_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
+            # Compare pose data or set to -1 if pose distance is negative
             head_feet_distance = pose_msg.head_feet_distance
-            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}")
-
-            # 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
-            comparison_scores_msg.hoc_distance_score = hoc_distance_score
-            comparison_scores_msg.pose_distance_score = distance_score  # Save head-feet distance as pose_distance_score
+            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}")

people_tracking_v2/scripts/decision_node.py

@@ -28,10 +28,10 @@ def sync_callback(self, comparison_msg, detection_msg):
         # Define thresholds
         iou_threshold = 0.9
         hoc_threshold = 2.0
+        pose_threshold = 0.5  # Example threshold for pose distance
 
         iou_detections = []
-        best_hoc_detection = None
-        best_hoc_score = float('inf')
+        valid_detections = []
 
         # Create a dictionary for quick lookup of IoU values by detection ID
         iou_dict = {detection.id: detection.iou for detection in detection_msg.detections}
@@ -47,19 +47,24 @@ def sync_callback(self, comparison_msg, detection_msg):
             # Check if IoU is over the threshold
             if iou > iou_threshold:
                 iou_detections.append(score.id)
-
-            # Check if HoC score is under the threshold
-            if hoc_distance_score < hoc_threshold:
-                if hoc_distance_score < best_hoc_score:
-                    best_hoc_score = hoc_distance_score
-                    best_hoc_detection = score.id
+            else:
+                # If pose distance score is negative, only consider HoC score
+                if pose_distance_score < 0:
+                    if hoc_distance_score < hoc_threshold:
+                        valid_detections.append((score.id, hoc_distance_score))
+                else:
+                    # Check if both HoC and pose scores are valid
+                    if hoc_distance_score < hoc_threshold and pose_distance_score < pose_threshold:
+                        valid_detections.append((score.id, hoc_distance_score))
 
         if len(iou_detections) == 1:
             operator_id = iou_detections[0]
             decision_source = "IoU"
-        elif best_hoc_detection is not None:
+        elif valid_detections:
+            # Find the detection with the best (lowest) HoC score among the valid detections
+            best_hoc_detection = min(valid_detections, key=lambda x: x[1])[0]
             operator_id = best_hoc_detection
-            decision_source = "HoC"
+            decision_source = "HoC + Pose"
         else:
             operator_id = -1  # Use -1 to indicate no operator found
             decision_source = "None"

people_tracking_v2/scripts/pose_estimation_node.py

@@ -46,41 +46,24 @@ def image_callback(self, image_msg):
         for pose in pose_details:
             try:
                 pose_distance_msg = BodySize()
-
-                # Extract the ID from the incoming message
-                detection_id = pose_distance_msg.id
-
                 pose_distance_msg.header.stamp = image_msg.header.stamp  # Use the timestamp from the incoming YOLO image
 
-                if "LEar" in pose and "LAnkle" in pose:
-                    left_ear_to_left_ankle = self._wrapper.compute_distance(pose["LEar"], pose["LAnkle"])
-                else:
-                    left_ear_to_left_ankle = float('inf')
-
-                if "LEar" in pose and "RAnkle" in pose:
-                    left_ear_to_right_ankle = self._wrapper.compute_distance(pose["LEar"], pose["RAnkle"])
-                else:
-                    left_ear_to_right_ankle = float('inf')
-
-                if "REar" in pose and "LAnkle" in pose:
-                    right_ear_to_left_ankle = self._wrapper.compute_distance(pose["REar"], pose["LAnkle"])
-                else:
-                    right_ear_to_left_ankle = float('inf')
-
-                if "REar" in pose and "RAnkle" in pose:
-                    right_ear_to_right_ankle = self._wrapper.compute_distance(pose["REar"], pose["RAnkle"])
-                else:
-                    right_ear_to_right_ankle = float('inf')
+                # Calculate ear to ankle distances
+                left_ear_to_left_ankle = self._wrapper.compute_distance(pose["LEar"], pose["LAnkle"]) if "LEar" in pose and "LAnkle" in pose else float('inf')
+                left_ear_to_right_ankle = self._wrapper.compute_distance(pose["LEar"], pose["RAnkle"]) if "LEar" in pose and "RAnkle" in pose else float('inf')
+                right_ear_to_left_ankle = self._wrapper.compute_distance(pose["REar"], pose["LAnkle"]) if "REar" in pose and "LAnkle" in pose else float('inf')
+                right_ear_to_right_ankle = self._wrapper.compute_distance(pose["REar"], pose["RAnkle"]) if "REar" in pose and "RAnkle" in pose else float('inf')
 
                 distances = [left_ear_to_left_ankle, left_ear_to_right_ankle, right_ear_to_left_ankle, right_ear_to_right_ankle]
-                min_distance = min(distances)
+                valid_distances = [d for d in distances if d != float('inf')]
 
-                if min_distance == float('inf'):
-                    rospy.logwarn("No valid ear-to-ankle distance found")
+                if not valid_distances:
+                    pose_distance_msg.head_feet_distance = -1  # No valid ear-to-ankle distance found
+                    rospy.logwarn("No valid ear-to-ankle distance found, setting distance to -1")
                 else:
+                    min_distance = min(valid_distances)
                     pose_distance_msg.head_feet_distance = min_distance
-                    rospy.loginfo(f"For ID {detection_id} Head-Feet Distance: {pose_distance_msg.head_feet_distance:.2f}")
-
+                    rospy.loginfo(f"For ID {pose_distance_msg.id} Ear-to-Ankle Distance: {pose_distance_msg.head_feet_distance:.2f}")
 
                 # Find the corresponding detection ID and use depth value to normalize the size
                 for detection in self.current_detections: