Give Hermes recognitions, not just photos: faces(), tracking ids, discovery

Hermes asked for structured detections instead of a bare JPEG. faces() returns the
faces in view with no photo: a persistent track id, bbox, detection confidence, how
long the person has been in view, and identity {name, similarity} or null, never a
made-up name. look() returns that same JSON as text plus the photo (send_image can
skip it); list_people() gives names and counts; robot://status and two prompts make
the server discoverable, where list_resources and list_prompts were empty. The
detector now tracks faces across frames (FaceTracker) and publishes the track id and
the identity as a second hypothesis.

Deliberately not built: embeddings in responses, age/gender attributes, auto-updating
face data, and a remote enroll tool (a prompt could make the robot call anyone "Kyle").

Checked end to end: a real Hermes turn on the local ornith model called faces() (0.03 s,
no image) and answered that Kyle was there at 0.78 similarity. Also: the camera is
calibrated (18 of 24 views, 0.296 px, outlier rejection added) and the calibration is
kept in pi/calibration and restored by the installer.

Co-Authored-By: Claude Sonnet 5.5 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01AFN8nRRCiDiMN7ChBpHJhs

ros/hermes_vision/hermes_vision/calibrate.py

@@ -44,12 +44,63 @@ def novel_enough(sig: np.ndarray, taken: list[np.ndarray], min_distance: float =
     return all(np.linalg.norm(sig - t) >= min_distance for t in taken)
 
 
-def solve(obj_pts: list[np.ndarray], img_pts: list[np.ndarray], size: tuple[int, int]):
-    """OpenCV calibration. Returns (rms reprojection error in px, camera matrix, distortion)."""
-    rms, k, d, _, _ = cv2.calibrateCamera(obj_pts, img_pts, size, None, None)
+def solve(obj_pts: list[np.ndarray], img_pts: list[np.ndarray], size: tuple[int, int], fix_k3: bool = True):
+    """OpenCV calibration. Returns (rms reprojection error in px, camera matrix, distortion).
+
+    k3 is held at zero by default: for a lens this mild (about 57 degrees) it is the term that overfits
+    when the board never reached the corners of the image, giving huge cancelling k1, k2, k3 that look
+    fine where the board was and are wild outside it."""
+    flags = cv2.CALIB_FIX_K3 if fix_k3 else 0
+    rms, k, d, _, _ = cv2.calibrateCamera(obj_pts, img_pts, size, None, None, flags=flags)
     return float(rms), k, d.reshape(-1)
 
 
+def view_errors(obj_pts, img_pts, k, d) -> np.ndarray:
+    """RMS reprojection error of each view (px) under camera k, d."""
+    errs = []
+    for o, i in zip(obj_pts, img_pts):
+        ok, rv, tv = cv2.solvePnP(o, i, k, d)
+        proj, _ = cv2.projectPoints(o, rv, tv, k, d)
+        errs.append(float(np.sqrt(((proj.reshape(-1, 2) - i.reshape(-1, 2)) ** 2).sum(axis=1).mean())))
+    return np.array(errs)
+
+
+def solve_robust(obj_pts, img_pts, size, fix_k3: bool = True, factor: float = 3.0, min_views: int = 12):
+    """Solve, then drop gross outlier views (a hand-held board blurs while it moves, and a blurred view
+    localises its corners about a pixel wrong) and solve again. A view is an outlier only if its error is
+    over `factor` times the median, so ordinary views are never discarded, and it never goes below
+    min_views. Returns (rms, k, d, indices of the views kept)."""
+    keep = list(range(len(img_pts)))
+    for _ in range(2):
+        rms, k, d = solve([obj_pts[i] for i in keep], [img_pts[i] for i in keep], size, fix_k3)
+        errs = view_errors([obj_pts[i] for i in keep], [img_pts[i] for i in keep], k, d)
+        limit = factor * float(np.median(errs))
+        good = [i for i, e in zip(keep, errs) if e <= limit]
+        if len(good) == len(keep) or len(good) < min_views:
+            break
+        keep = good
+    rms, k, d = solve([obj_pts[i] for i in keep], [img_pts[i] for i in keep], size, fix_k3)
+    return rms, k, d, keep
+
+
+_REGIONS = ('top-left', 'top', 'top-right', 'left', 'centre', 'right', 'bottom-left', 'bottom', 'bottom-right')
+
+
+def coverage(img_pts: list[np.ndarray], size: tuple[int, int]) -> tuple[float, list[str]]:
+    """How much of the image the board's corners have visited: the fraction of a 3 x 3 grid of regions
+    holding at least a few corners, and the names of the empty ones. Distortion is determined at the
+    edges and corners, so an uncovered edge means an unreliable model there."""
+    w, h = size
+    counts = np.zeros((3, 3), int)
+    for pts in img_pts:
+        c = pts.reshape(-1, 2)
+        ix = np.clip((c[:, 0] / w * 3).astype(int), 0, 2)
+        iy = np.clip((c[:, 1] / h * 3).astype(int), 0, 2)
+        np.add.at(counts, (iy, ix), 1)
+    empty = [_REGIONS[r * 3 + c] for r in range(3) for c in range(3) if counts[r, c] < 6]
+    return 1.0 - len(empty) / 9.0, empty
+
+
 def camera_info_yaml(name: str, size: tuple[int, int], k: np.ndarray, d: np.ndarray) -> str:
     """The ROS camera_info YAML (the format camera_calibration_parsers reads)."""
     w, h = size
@@ -80,8 +131,14 @@ def main() -> int:
     p.add_argument('--topic', default='/camera/image_raw/compressed')
     p.add_argument('--name', default=DEFAULT_NAME)
     p.add_argument('--out-dir', default=os.path.expanduser('~/.ros/camera_info'))
+    p.add_argument('--full-model', action='store_true', help='also fit k3 (only with good coverage of the corners)')
+    p.add_argument('--from-views', metavar='NPZ', help='re-solve the views saved by an earlier run, no camera needed')
     a = p.parse_args()
 
+    if a.from_views:
+        z = np.load(a.from_views)
+        return finish(a, list(z['obj']), list(z['img']), tuple(int(v) for v in z['size']), resave=False)
+
     import rclpy
     from rclpy.node import Node
     from sensor_msgs.msg import CompressedImage
@@ -111,7 +168,9 @@ def main() -> int:
         taken_sigs.append(sig)
         obj_pts.append(obj)
         img_pts.append(corners)
-        print(f'  view {len(img_pts)}/{a.views} taken', flush=True)
+        frac, empty = coverage(img_pts, state['size'])
+        hint = f'; not yet covered: {", ".join(empty)}' if empty else '; whole image covered'
+        print(f'  view {len(img_pts)}/{a.views} taken{hint}', flush=True)
 
     node.create_subscription(CompressedImage, a.topic, on_image, 1)
     print(f'Hold the {a.cols}x{a.rows}-corner board ({a.square_mm:g} mm squares) in front of the camera; '
@@ -123,15 +182,28 @@ def main() -> int:
     if len(img_pts) < 10:
         print(f'only {len(img_pts)} usable views; nothing written (need at least 10, ideally {a.views})')
         return 1
-    size = state['size']
-    rms, k, d = solve(obj_pts, img_pts, size)
+    return finish(a, obj_pts, img_pts, state['size'], resave=True)
+
+
+def finish(a, obj_pts, img_pts, size, resave: bool) -> int:
+    """Solve (dropping outlier views), report, and write the YAML."""
+    rms, k, d, keep = solve_robust(obj_pts, img_pts, size, fix_k3=not a.full_model)
+    dropped = sorted(set(range(len(img_pts))) - set(keep))
+    frac, empty = coverage([img_pts[i] for i in keep], size)
     name = f'{a.name}_{size[0]}x{size[1]}'
     os.makedirs(a.out_dir, exist_ok=True)
     path = os.path.join(a.out_dir, name + '.yaml')
     with open(path, 'w') as f:
         f.write(camera_info_yaml(name, size, k, d))
-    print(f'\n{len(img_pts)} views, reprojection error {rms:.3f} px ({"good" if rms < 0.5 else "mediocre: retake with more varied poses"})')
-    print(f'focal length {k[0, 0]:.1f} x {k[1, 1]:.1f} px, centre ({k[0, 2]:.0f}, {k[1, 2]:.0f}) of {size[0]}x{size[1]}')
+    if resave:
+        np.savez(os.path.join(a.out_dir, name + '-views.npz'), obj=np.array(obj_pts), img=np.array(img_pts), size=size)
+    print(f'\n{len(keep)} of {len(img_pts)} views used'
+          + (f' (dropped as blurred outliers: {dropped})' if dropped else '')
+          + f', reprojection error {rms:.3f} px ({"good" if rms < 0.5 else "mediocre: retake with more varied, steadier poses"})')
+    print(f'image coverage {frac:.0%}' + (f'; never reached: {", ".join(empty)} (the model is unreliable there: '
+                                         'retake with the board in those parts of the image)' if empty else ''))
+    print(f'focal length {k[0, 0]:.1f} x {k[1, 1]:.1f} px, centre ({k[0, 2]:.0f}, {k[1, 2]:.0f}) of {size[0]}x{size[1]}; '
+          f'distortion k1={d[0]:+.3f} k2={d[1]:+.3f} p1={d[2]:+.4f} p2={d[3]:+.4f} k3={d[4]:+.3f}')
     print(f'wrote {path}\nrestart the robot to load it:  sudo systemctl restart robot')
     return 0
 

ros/hermes_vision/hermes_vision/face_detector_node.py

@@ -1,10 +1,13 @@
 """ROS node: faces in the camera image.
 
 Subscribes:  image_topic (sensor_msgs/CompressedImage, default /camera/image_raw/compressed)
-Publishes:   /vision/faces (vision_msgs/Detection2DArray): one detection per face, bbox in pixels
-             (center and size), class_id 'face', score = detector confidence; header from the image.
-             Detection2D.id is the recognised person's name, or empty if unknown or recognition is
-             off. Recognition only runs for people enrolled with hermes_vision.people.
+Publishes:   /vision/faces (vision_msgs/Detection2DArray): one detection per face, bbox in full-size
+             camera pixels (center and size), header from the image. Detection2D.id is the track id
+             (a number as a string, stable while the person stays in view). results[0] is the face
+             itself (class_id 'face', score = detector confidence); results[1] exists only if the person
+             is recognised (class_id = their name, score = cosine similarity, which is not a
+             probability). An unknown face has no results[1]: a name is never made up. Recognition only
+             runs for people enrolled with hermes_vision.people.
              /vision/faces/debug/compressed (CompressedImage), only if publish_debug is true.
 Work is rate-limited to rate_hz, so a busy Pi 4 skips frames rather than falling behind.
 """
@@ -19,7 +22,8 @@ from sensor_msgs.msg import CompressedImage
 from vision_msgs.msg import Detection2D, Detection2DArray, ObjectHypothesisWithPose
 
 from .detect import Face, FaceDetector
-from .recognize import COSINE_THRESHOLD, DEFAULT_DB, DEFAULT_MODEL, FaceRecognizer, IdentityCache
+from .recognize import COSINE_THRESHOLD, DEFAULT_DB, DEFAULT_MODEL, FaceRecognizer
+from .tracking import FaceTracker
 
 
 class FaceDetectorNode(Node):
@@ -36,7 +40,7 @@ class FaceDetectorNode(Node):
         self._flag = cv2.IMREAD_REDUCED_COLOR_2 if self.scale == 2 else cv2.IMREAD_COLOR
         self.detector = FaceDetector(param('model_path', ''), float(param('score_threshold', 0.6)),
                                      int(param('min_size', 40)) // self.scale, int(param('detect_max_side', 480)))
-        self.ids = IdentityCache(float(param('recognition_period_s', 1.0)))
+        self.tracker = FaceTracker(recheck_s=float(param('recognition_period_s', 1.0)))
         recognition_model = os.path.expanduser(param('recognition_model', DEFAULT_MODEL))   # declare once
         faces_db = param('faces_db', DEFAULT_DB)
         threshold = float(param('recognition_threshold', COSINE_THRESHOLD))
@@ -63,20 +67,23 @@ class FaceDetectorNode(Node):
             self.get_logger().warn('could not decode image', throttle_duration_sec=10.0)
             return
         faces = self.detector.detect(img)                 # in the (possibly half-size) decoded image
-        names = (self.ids.update(faces, now, lambda f: self.recognizer.identify(img, f)[0])
-                 if self.recognizer else [''] * len(faces))
+        tracks = self.tracker.update(faces, now, (lambda f: self.recognizer.identify(img, f)) if self.recognizer else None)
         k = self.scale                                    # published boxes are in full-size camera pixels
         out = Detection2DArray(header=msg.header)
-        for f, name in zip(faces, names):
-            f = Face(f.x * k, f.y * k, f.w * k, f.h * k, f.score)
+        for t in tracks:
+            f = Face(t.face.x * k, t.face.y * k, t.face.w * k, t.face.h * k, t.face.score)
             d = Detection2D(header=msg.header)
-            d.id = name
+            d.id = str(t.id)
             d.bbox.center.position.x = f.x + f.w / 2.0
             d.bbox.center.position.y = f.y + f.h / 2.0
             d.bbox.size_x, d.bbox.size_y = float(f.w), float(f.h)
             hyp = ObjectHypothesisWithPose()
             hyp.hypothesis.class_id, hyp.hypothesis.score = 'face', f.score
             d.results.append(hyp)
+            if t.name:
+                who = ObjectHypothesisWithPose()
+                who.hypothesis.class_id, who.hypothesis.score = t.name, float(t.similarity)
+                d.results.append(who)
             out.detections.append(d)
         self.pub.publish(out)
         if self.debug_pub:

ros/hermes_vision/hermes_vision/tracking.py

@@ -0,0 +1,65 @@
+"""Follow faces across frames, so a person keeps the same track id and their identity is not
+re-resolved every frame. No ROS imports.
+
+Faces are matched to existing tracks by box overlap (greedy, best overlap first). A track that is not
+seen for max_age_s is dropped; a face that matches nothing starts a new track. Recognition is the
+expensive step, so a track's identity is re-checked only every recheck_s.
+"""
+from __future__ import annotations
+
+from dataclasses import dataclass
+from typing import Callable, Optional
+
+from .detect import Face
+from .recognize import iou
+
+
+@dataclass
+class Track:
+    id: int
+    face: Face
+    first: float                 # when the track began
+    last: float                  # when it was last matched
+    name: str = ''               # '' = unknown (or not checked yet)
+    similarity: float = 0.0      # cosine similarity to the matched person (not a probability)
+    named_at: float = -1e9       # when identity was last resolved
+
+    def age(self, now: float) -> float:
+        return now - self.first
+
+
+class FaceTracker:
+    def __init__(self, iou_min: float = 0.3, max_age_s: float = 1.5, recheck_s: float = 1.0):
+        self.iou_min, self.max_age_s, self.recheck_s = iou_min, max_age_s, recheck_s
+        self._tracks: list[Track] = []
+        self._next_id = 1
+
+    def update(self, faces: list[Face], now: float,
+               identify: Optional[Callable[[Face], tuple[str, float]]] = None) -> list[Track]:
+        """Tracks for `faces` (same order). identify(face) -> (name, similarity) is called only for new
+        tracks and for tracks whose identity is older than recheck_s."""
+        pairs = sorted(((iou(f, t.face), fi, ti) for fi, f in enumerate(faces) for ti, t in enumerate(self._tracks)),
+                       reverse=True)
+        face_to_track: dict[int, int] = {}
+        used: set[int] = set()
+        for score, fi, ti in pairs:
+            if score < self.iou_min:
+                break
+            if fi not in face_to_track and ti not in used:
+                face_to_track[fi] = ti
+                used.add(ti)
+        out: list[Track] = []
+        for fi, f in enumerate(faces):
+            if fi in face_to_track:
+                t = self._tracks[face_to_track[fi]]
+                t.face, t.last = f, now
+            else:
+                t = Track(self._next_id, f, now, now)
+                self._next_id += 1
+                self._tracks.append(t)
+            if identify is not None and now - t.named_at >= self.recheck_s:
+                t.name, t.similarity = identify(f)
+                t.named_at = now
+            out.append(t)
+        self._tracks = [t for t in self._tracks if now - t.last <= self.max_age_s]
+        return out

ros/hermes_vision/test/test_calibrate.py

@@ -2,7 +2,8 @@ import cv2
 import numpy as np
 import yaml
 
-from hermes_vision.calibrate import board_points, camera_info_yaml, novel_enough, solve, view_signature
+from hermes_vision.calibrate import (board_points, camera_info_yaml, coverage, novel_enough, solve, solve_robust,
+                                     view_signature)
 
 SIZE = (1296, 972)
 K_TRUE = np.array([[1100.0, 0, 650.0], [0, 1090.0, 480.0], [0, 0, 1]])
@@ -72,3 +73,43 @@ def test_find_board_detects_a_real_rendered_checkerboard_and_not_a_blank_frame()
     corners = find_board(bgr, 9, 6)
     assert corners is not None and corners.reshape(-1, 2).shape == (54, 2)
     assert find_board(np.full_like(bgr, 128), 9, 6) is None
+
+
+def test_k3_is_held_at_zero_by_default_but_can_be_fitted():
+    obj, views = synthetic_views()
+    _, _, d = solve([obj] * len(views), views, SIZE)
+    assert d[4] == 0.0
+    _, _, d_full = solve([obj] * len(views), views, SIZE, fix_k3=False)
+    assert d_full[4] != 0.0
+
+
+def test_coverage_names_the_regions_the_board_never_reached():
+    w, h = SIZE
+    centre_only = [np.random.default_rng(1).uniform([w * 0.4, h * 0.4], [w * 0.6, h * 0.6], (54, 2)).astype(np.float32)]
+    frac, empty = coverage(centre_only, SIZE)
+    assert frac < 0.2 and 'top-left' in empty and 'bottom-right' in empty and 'centre' not in empty
+    rng = np.random.default_rng(2)
+    everywhere = [rng.uniform([0, 0], [w, h], (200, 2)).astype(np.float32)]
+    assert coverage(everywhere, SIZE) == (1.0, [])
+
+
+def test_blurred_outlier_views_are_dropped_and_the_result_stays_accurate():
+    obj, views = synthetic_views(n=40, seed=9)
+    rng = np.random.default_rng(5)
+    noisy = [v + rng.normal(0, 0.15, v.shape).astype(np.float32) for v in views]       # ordinary jitter
+    bad = [v + rng.normal(0, 2.0, v.shape).astype(np.float32) for v in views[:4]]       # four blurred views
+    allv = noisy + bad
+    rms_all, k_all, _ = solve([obj] * len(allv), allv, SIZE)
+    rms, k, d, keep = solve_robust([obj] * len(allv), allv, SIZE)
+    assert set(range(len(noisy), len(allv))) <= set(range(len(allv))) - set(keep)       # the four bad ones are gone
+    assert len(keep) >= len(noisy) - 2                                                   # the ordinary ones are kept
+    assert rms < rms_all and rms < 0.3
+    assert abs(k[0, 0] - 1100) / 1100 < 0.02
+
+
+def test_it_never_drops_below_the_minimum_number_of_views():
+    obj, views = synthetic_views(n=14, seed=4)
+    rng = np.random.default_rng(1)
+    allv = [v + rng.normal(0, 1.0, v.shape).astype(np.float32) * (i % 2) for i, v in enumerate(views)]
+    _, _, _, keep = solve_robust([obj] * len(allv), allv, SIZE, min_views=len(allv) - 1)
+    assert len(keep) >= len(allv) - 1

ros/hermes_vision/test/test_tracking.py

@@ -0,0 +1,55 @@
+from hermes_vision.detect import Face
+from hermes_vision.tracking import FaceTracker
+
+
+def f(x, y=100, w=80, h=80):
+    return Face(x, y, w, h, 0.9)
+
+
+def test_a_face_keeps_its_track_id_and_age_grows():
+    tr = FaceTracker()
+    ids = []
+    for i in range(8):
+        (t,) = tr.update([f(100 + 3 * i)], i * 0.25)
+        ids.append(t.id)
+    assert ids == [1] * 8 and abs(t.age(1.75) - 1.75) < 1e-9
+
+
+def test_two_people_get_two_ids_and_keep_them_when_they_swap_order():
+    tr = FaceTracker()
+    a, b = tr.update([f(100), f(500)], 0.0)
+    b2, a2 = tr.update([f(502), f(102)], 0.25)             # reported in the other order
+    assert (a.id, b.id) == (a2.id, b2.id) and a.id != b.id
+
+
+def test_a_track_survives_a_short_gap_but_not_a_long_one():
+    tr = FaceTracker(max_age_s=1.5)
+    (t,) = tr.update([f(100)], 0.0)
+    tr.update([], 1.0)                                      # nobody for a second
+    (t2,) = tr.update([f(100)], 1.2)
+    assert t2.id == t.id
+    tr.update([], 1.3)
+    tr.update([], 3.5)                                      # gone for over max_age
+    (t3,) = tr.update([f(100)], 3.6)
+    assert t3.id != t.id
+
+
+def test_identity_is_resolved_for_new_tracks_and_rechecked_once_a_second():
+    calls = []
+    tr = FaceTracker(recheck_s=1.0)
+    ident = lambda face: (calls.append(face), ('Kyle', 0.8))[1]
+    for i in range(8):                                      # 4 Hz for two seconds
+        (t,) = tr.update([f(100 + i)], i * 0.25, ident)
+    assert t.name == 'Kyle' and t.similarity == 0.8
+    assert len(calls) == 2                                  # at t=0 and again at t=1.0
+
+
+def test_a_stranger_is_unknown_not_named():
+    tr = FaceTracker()
+    (t,) = tr.update([f(100)], 0.0, lambda face: ('', 0.12))
+    assert t.name == '' and t.similarity == 0.12
+
+
+def test_without_an_identifier_nothing_is_named():
+    (t,) = FaceTracker().update([f(100)], 0.0)
+    assert t.name == '' and t.id == 1