adipu/gdc_atrium
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add coords display underneath norm dist for keypoint_node

Browse Source
This commit is contained in:
pulipakaa24
2026-03-15 13:09:47 -05:00
parent 6c303550b2
commit 4a16258286
1 changed files with 14 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
keypoint_pose/keypoint_pose/keypoint_node.py
View File

@@ -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)