shows window for distance tracking, only uses keypoints where same keypoint >75% confidence on both feeds

keypoint_pose/keypoint_pose/keypoint_node.py

@@ -43,6 +43,9 @@ KEYPOINT_NAMES = [
     'left_knee', 'right_knee', 'left_ankle', 'right_ankle',
 ]
 
+# Minimum confidence in BOTH camera feeds to include a keypoint in distance calc
+DIST_THRESHOLD = 0.75
+
 
 def draw_keypoints(frame, keypoints, keypoint_scores, threshold=0.3):
     """Draw COCO 17-keypoint skeleton on *frame* (in-place)."""
@@ -118,6 +121,10 @@ class KeypointTriangulationNode(Node):
         # ── Display state ───────────────────────────────────────────────
         self._left_display = None
         self._right_display = None
+        self._dist_display = None
+        self.get_logger().info(
+            f'Distance window uses keypoints with >= {DIST_THRESHOLD*100:.0f}% '
+            'confidence in both camera feeds.')
         self.create_timer(1.0 / 30.0, self._display_timer_cb)
 
         self.get_logger().info(
@@ -371,8 +378,10 @@ class KeypointTriangulationNode(Node):
 
         # Triangulate every mutually-confident keypoint
         all_points_3d = []
+        avg_distances = []   # per-person average distance (75%+ conf kps only)
         for lp, rp in matches:
             person_pts = {}  # kp_idx -> (xyz, residual)
+            high_conf_dists = []
             for kp_idx in range(17):
                 if (lp['scores'][kp_idx] <= self._threshold or
                         rp['scores'][kp_idx] <= self._threshold):
@@ -387,14 +396,41 @@ class KeypointTriangulationNode(Node):
                 pt3d, residual = self._triangulate(d1, d2)
                 if pt3d is not None and residual < self._max_residual:
                     person_pts[kp_idx] = (pt3d, residual)
+                    # Only count toward distance if both scores >= DIST_THRESHOLD
+                    if (lp['scores'][kp_idx] >= DIST_THRESHOLD and
+                            rp['scores'][kp_idx] >= DIST_THRESHOLD):
+                        high_conf_dists.append(np.linalg.norm(pt3d))
 
             if person_pts:
                 all_points_3d.append(person_pts)
+                avg_distances.append(
+                    float(np.mean(high_conf_dists)) if high_conf_dists else None)
 
         # Publish 3D markers
         self._marker_pub.publish(
             self._build_markers(all_points_3d, left_msg.header.stamp))
 
+        # Build distance display window
+        row_h = 220
+        frame_h = max(400, 140 + row_h * max(len(avg_distances), 1))
+        dist_frame = np.zeros((frame_h, 1600, 3), dtype=np.uint8)
+        cv2.putText(dist_frame, 'Avg distance  (>=75% conf keypoints)',
+                    (20, 70), cv2.FONT_HERSHEY_SIMPLEX, 2.0, (180, 180, 180), 3)
+        if avg_distances:
+            for i, d in enumerate(avg_distances):
+                if d is not None:
+                    txt = f'Person {i + 1}: {d:.2f} m'
+                    color = (100, 255, 100)
+                else:
+                    txt = f'Person {i + 1}: -- (no 75%+ kps)'
+                    color = (80, 80, 200)
+                cv2.putText(dist_frame, txt, (20, 140 + i * row_h),
+                            cv2.FONT_HERSHEY_SIMPLEX, 4.5, color, 8)
+        else:
+            cv2.putText(dist_frame, 'No people detected', (20, 200),
+                        cv2.FONT_HERSHEY_SIMPLEX, 4.0, (100, 100, 100), 6)
+        self._dist_display = dist_frame
+
         # Draw 2D keypoints on display frames
         if left_people:
             draw_keypoints(
@@ -429,6 +465,8 @@ class KeypointTriangulationNode(Node):
             cv2.imshow('Left - Keypoints', self._left_display)
         if self._right_display is not None:
             cv2.imshow('Right - Keypoints', self._right_display)
+        if self._dist_display is not None:
+            cv2.imshow('Distance (75%+ confidence)', self._dist_display)
 
         key = cv2.waitKey(1) & 0xFF
         if key == ord('q'):