diff --git a/keypoint_pose/keypoint_pose/keypoint_node.py b/keypoint_pose/keypoint_pose/keypoint_node.py index d1de9d4..7ccc468 100644 --- a/keypoint_pose/keypoint_pose/keypoint_node.py +++ b/keypoint_pose/keypoint_pose/keypoint_node.py @@ -60,7 +60,7 @@ def draw_keypoints(frame, keypoints, keypoint_scores, threshold=0.3): 0 <= pt2[0] < w and 0 <= pt2[1] < h): cv2.line(frame, pt1, pt2, (0, 255, 0), 2) - for idx, (kp, score) in enumerate(zip(person_kps, person_scores)): + for kp, score in zip(person_kps, person_scores): if score > threshold: x, y = int(kp[0]), int(kp[1]) if 0 <= x < w and 0 <= y < h: @@ -379,9 +379,11 @@ class KeypointTriangulationNode(Node): # Triangulate every mutually-confident keypoint all_points_3d = [] avg_distances = [] # per-person average distance (75%+ conf kps only) + avg_coords = [] # per-person mean (x, y, z) of 75%+ conf kps for lp, rp in matches: person_pts = {} # kp_idx -> (xyz, residual) high_conf_dists = [] + high_conf_pts = [] for kp_idx in range(17): if (lp['scores'][kp_idx] <= self._threshold or rp['scores'][kp_idx] <= self._threshold): @@ -400,11 +402,14 @@ class KeypointTriangulationNode(Node): if (lp['scores'][kp_idx] >= DIST_THRESHOLD and rp['scores'][kp_idx] >= DIST_THRESHOLD): high_conf_dists.append(np.linalg.norm(pt3d)) + high_conf_pts.append(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) + avg_coords.append( + np.mean(high_conf_pts, axis=0) if high_conf_pts else None) # Publish 3D markers self._marker_pub.publish( @@ -417,15 +422,22 @@ class KeypointTriangulationNode(Node): 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): + for i, (d, xyz) in enumerate(zip(avg_distances, avg_coords)): if d is not None: txt = f'Person {i + 1}: {d:.2f} m' color = (100, 255, 100) + coord_txt = f'x={xyz[0]:+.3f} y={xyz[1]:+.3f} z={xyz[2]:+.3f} [m, left cam frame]' + coord_color = (160, 160, 160) else: txt = f'Person {i + 1}: -- (no 75%+ kps)' color = (80, 80, 200) + coord_txt = '' + coord_color = (80, 80, 80) cv2.putText(dist_frame, txt, (20, 140 + i * row_h), cv2.FONT_HERSHEY_SIMPLEX, 4.5, color, 8) + if coord_txt: + cv2.putText(dist_frame, coord_txt, (28, 140 + i * row_h + 70), + cv2.FONT_HERSHEY_SIMPLEX, 1.4, coord_color, 2) else: cv2.putText(dist_frame, 'No people detected', (20, 200), cv2.FONT_HERSHEY_SIMPLEX, 4.0, (100, 100, 100), 6)