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_bringup/config/robot.yaml

@@ -36,5 +36,6 @@ hermes_mcp:
     bind: 10.55.0.2                    # the USB link; the firewall accepts 8765 only from 10.55.0.1 on usb0
     port: 8765
     head: false                        # no servos fitted yet: turn_head and head_position stay unregistered
+    speakers: false                    # no speakers fitted yet: reported honestly in robot://status
     connected_grace_s: 30.0            # an authenticated client heard within this long keeps the heartbeat going
     look_timeout_s: 3.0

ros/hermes_face/hermes_face/face_node.py

@@ -3,7 +3,8 @@
 Subscribes:  /hermes/state (std_msgs/String): sleeping idle listening thinking looking speaking
              /joint_states (sensor_msgs/JointState): pan_joint, tilt_joint in radians. The sign
                  convention is the MCP tool's: positive pan = the robot's left, positive tilt = up.
-             /vision/faces (vision_msgs/Detection2DArray): the eyes glance toward the best face
+             /vision/faces (vision_msgs/Detection2DArray): the eyes glance toward the best face (a named one
+                 first: the name is results[1].class_id)
              /camera/camera_info (sensor_msgs/CameraInfo): the image size for that, if published
              /audio/activity (std_msgs/String): speaking | listening | quiet, from hermes_audio
              /audio/scope/{mic,speaker} (std_msgs/Float32MultiArray): the real waveform for the mouth
@@ -115,7 +116,8 @@ class FaceNode(Node):
     def _on_faces(self, msg: Detection2DArray) -> None:
         target = pick_target([
             Target(d.bbox.center.position.x, d.bbox.center.position.y, d.bbox.size_x, d.bbox.size_y,
-                   d.results[0].hypothesis.score if d.results else 1.0, d.id)
+                   d.results[0].hypothesis.score if d.results else 1.0,
+                   d.results[1].hypothesis.class_id if len(d.results) > 1 else '')   # the name, if recognised
             for d in msg.detections])
         if target:
             gx, gy = gaze_from_pixel(target.cx, target.cy, *self.image_size)

ros/hermes_mcp/hermes_mcp/bridge.py

@@ -2,16 +2,25 @@
 so it is testable with a stub camera.
 
 MCP tool names are the stable contract with Hermes and must not change when the internals do:
-  look()                  -> the next fresh JPEG from the camera
+  look(send_image)        -> the faces in view as JSON text, and (by default) the next fresh JPEG
+  faces()                 -> the same faces summary with no photo: cheap, the right call for "who is here"
+  list_people()           -> the names enrolled on the robot (never their face data)
   turn_head(pan, tilt)    -> only registered if a head backend is given (servos fitted)
   head_position()         -> likewise
+Also an `robot://status` resource and prompts, so a client can discover what this robot can do.
+
+Faces never come with embeddings: they are biometric templates, meaningless to a language model, and
+the registry stays on the robot. A face that is not recognised has identity null; a name is never
+made up. Enrolling people is deliberately not a tool: it is a step the owner takes on the robot.
 """
 from __future__ import annotations
 
 import hmac
+import json
+import os
 import threading
 import time
-from typing import Callable, Optional, Protocol
+from typing import Any, Callable, Literal, Optional, Protocol
 
 from mcp.server.mcpserver import Image, MCPServer
 from mcp.server.transport_security import TransportSecuritySettings
@@ -43,32 +52,131 @@ class LatestFrame:
             return self._jpeg
 
 
+class FacesBuffer:
+    """The newest face detections, with how long each tracked person has been in view."""
+
+    def __init__(self) -> None:
+        self._cv = threading.Condition()
+        self._faces: Optional[list[dict]] = None
+        self._size = (1296, 972)
+        self._t = -1.0
+        self._first: dict[int, tuple[float, float]] = {}         # track id -> (first seen, last seen)
+
+    def set_size(self, width: int, height: int) -> None:
+        with self._cv:
+            self._size = (int(width), int(height))
+
+    def put(self, faces: list[dict], t: Optional[float] = None) -> None:
+        """faces: dicts with track_id, bbox {x, y, w, h}, confidence, name ('' if unknown), similarity."""
+        now = time.monotonic() if t is None else t
+        with self._cv:
+            for f in faces:
+                first, _ = self._first.get(f['track_id'], (now, now))
+                self._first[f['track_id']] = (first, now)
+            self._first = {i: v for i, v in self._first.items() if now - v[1] < 5.0}
+            self._faces, self._t = faces, now
+            self._cv.notify_all()
+
+    def _snapshot(self, now: float) -> dict:
+        w, h = self._size
+        out = []
+        for f in self._faces or []:
+            b = f['bbox']
+            first = self._first.get(f['track_id'], (now, now))[0]
+            out.append({
+                'track_id': f['track_id'],
+                'bbox': {k: round(float(b[k])) for k in ('x', 'y', 'w', 'h')},
+                'bbox_norm': {'cx': round((b['x'] + b['w'] / 2) / w, 3), 'cy': round((b['y'] + b['h'] / 2) / h, 3),
+                              'w': round(b['w'] / w, 3), 'h': round(b['h'] / h, 3)},
+                'confidence': round(float(f['confidence']), 2),
+                'identity': ({'name': f['name'], 'similarity': round(float(f['similarity']), 2)}
+                             if f.get('name') else None),
+                'seen_for_s': round(max(0.0, now - first), 1),
+            })
+        return {'available': True, 'count': len(out), 'image_size': {'w': w, 'h': h},
+                'data_age_s': round(max(0.0, now - self._t), 2), 'faces': out}
+
+    def summary(self, max_age_s: float = 1.5, wait_s: float = 0.0) -> dict:
+        """The faces in view now. If the newest data is older than max_age_s, wait up to wait_s for fresh
+        data; if there is none, say so rather than reporting a stale room."""
+        deadline = time.monotonic() + wait_s
+        with self._cv:
+            while self._faces is None or time.monotonic() - self._t > max_age_s:
+                remaining = deadline - time.monotonic()
+                if remaining <= 0:
+                    return {'available': False, 'reason': 'face detection is not producing results right now'}
+                self._cv.wait(remaining)
+            return self._snapshot(time.monotonic())
+
+
+def people_summary(path: str) -> dict:
+    """Who is enrolled on this robot: names and how many samples, read straight from the registry file
+    (JSON), never the face data itself."""
+    try:
+        with open(path) as f:
+            db = json.load(f)
+    except FileNotFoundError:
+        db = {}
+    return {'count': len(db), 'people': [{'name': n, 'samples': len(v)} for n, v in sorted(db.items())]}
+
+
 class Head(Protocol):
     def move(self, pan: float, tilt: float) -> tuple[float, float]: ...
     def position(self) -> tuple[float, float]: ...
 
 
 def build_mcp(frames: LatestFrame, announce_look: Callable[[], None] = lambda: None,
-              head: Optional[Head] = None, look_timeout_s: float = 3.0) -> MCPServer:
+              head: Optional[Head] = None, look_timeout_s: float = 3.0,
+              faces_buf: Optional[FacesBuffer] = None, people_path: str = '',
+              hardware: Optional[dict] = None) -> MCPServer:
+    faces_buf = faces_buf or FacesBuffer()
+    hw = {'camera': True, 'head': head is not None, 'speakers': False, **(hardware or {})}
     mcp = MCPServer(
         "robot",
         instructions=(
-            "This server is a small physical robot on the user's desk. "
-            "Call look() whenever you need to see what is in front of it; the image is current. "
-            "If turn_head is available, aim first and then look."
+            "This server is a small physical robot on the user's desk, with a camera and a face display. "
+            "To know WHO or HOW MANY people are there, call faces(): it is instant and returns names, "
+            "positions and tracking ids with no photo; identity null means unknown, so say so rather "
+            "than guessing. Call look() when you need to see the scene itself: it returns the faces "
+            "summary and a current photo. list_people() says who the robot knows. "
+            + ("turn_head aims the camera: aim first, then look. " if head is not None else
+               "The camera is fixed: there is no head to turn. ")
+            + "Only faces are detected: not other people-shaped things, objects or motion. "
+              "Read robot://status for what hardware this robot has."
         ),
     )
 
     @mcp.tool()
-    def look() -> Image:
-        """Take a photograph with the robot's camera and return it. Use this to see the room,
-        the desk, the person, or anything the user asks you to look at."""
+    def faces() -> dict[str, Any]:
+        """Report the faces the robot's camera sees right now, with no photo (instant and cheap).
+        Each face has a track_id (the same person keeps the same id while they stay in view),
+        a bounding box in camera pixels and normalised, a detection confidence, seen_for_s (how long
+        they have been in view), and identity: {name, similarity} if the robot recognises them, or
+        null if not. null means unknown: never invent a name for it. similarity is a cosine score, not a
+        probability. Use this to answer who or how many."""
+        return faces_buf.summary(max_age_s=1.5, wait_s=1.5)
+
+    @mcp.tool()
+    def look(send_image: Literal["always", "never"] = "always") -> list:
+        """Look through the robot's camera. Returns the faces summary (JSON text, same as faces()) and,
+        unless send_image is "never", a current photograph of the scene. Use it to see the room, the
+        desk, a document, or anything the user asks you to look at; use faces() alone for who/how many."""
         t0 = time.monotonic()
         announce_look()                      # the face squints and flashes
+        summary = faces_buf.summary(max_age_s=1.5, wait_s=0.3)
+        text = json.dumps({'faces': summary})
+        if send_image == "never":
+            return [text]
         jpeg = frames.wait_newer(t0, look_timeout_s)
         if jpeg is None:
             raise RuntimeError("the robot's camera is not producing images")
-        return Image(data=jpeg, format="jpeg")
+        return [text, Image(data=jpeg, format="jpeg")]
+
+    @mcp.tool()
+    def list_people() -> dict[str, Any]:
+        """Who the robot has been taught to recognise: names and how many samples each, never their
+        face data. People are enrolled by the owner on the robot, not through this server."""
+        return people_summary(people_path) if people_path else {'count': 0, 'people': []}
 
     if head is not None:
         @mcp.tool()
@@ -86,6 +194,33 @@ def build_mcp(frames: LatestFrame, announce_look: Callable[[], None] = lambda: N
             p, t = head.position()
             return f"pan {p:+.0f} degrees, tilt {t:+.0f} degrees"
 
+    @mcp.resource("robot://status")
+    def status() -> str:
+        """What this robot is and what it can do right now: hardware present, what is detected, who is known."""
+        live = faces_buf.summary(max_age_s=3.0)
+        return json.dumps({
+            'robot': 'hermes-bot', 'hardware': hw,
+            'camera': {'image_size': live.get('image_size') if live['available'] else None},
+            'detects': ['faces'], 'not_detected': ['people', 'objects', 'motion'],
+            'face_detection_running': live['available'],
+            'recognition': {'enrolled': people_summary(people_path)['count'] if people_path else 0,
+                            'registry': 'on the robot; enrolled by the owner, not through MCP; embeddings never leave it'},
+            'tools': sorted(['faces', 'look', 'list_people'] + (['turn_head', 'head_position'] if head else [])),
+        }, indent=2)
+
+    @mcp.prompt()
+    def who_is_here() -> str:
+        """Find out who is at the robot's desk."""
+        return ("Call faces() and tell me who is there. Name the people the robot recognises, say how many "
+                "faces it does not recognise (identity null: do not guess who they are), and mention anyone "
+                "who has only just appeared. If face detection is unavailable, say so.")
+
+    @mcp.prompt()
+    def look_around() -> str:
+        """See the scene and describe it."""
+        return ("Call look() and describe what you see in the photo, then say who the faces summary says is "
+                "there. Do not name anyone the summary lists as identity null.")
+
     return mcp
 
 

ros/hermes_mcp/hermes_mcp/mcp_node.py

@@ -2,6 +2,7 @@
 
 Serves MCP (streamable HTTP, bearer token) to Hermes on the USB link and talks to the graph locally.
 Subscribes:  image_topic (sensor_msgs/CompressedImage, default /camera/image_raw/compressed)
+             /vision/faces (vision_msgs/Detection2DArray) and /camera/camera_info, for faces() and look()
 Publishes:   /hermes/state (std_msgs/String):
                `idle` once a second while an authenticated MCP client is connected: the agent heartbeat
                  the face uses to wake and, when it stops, to fall asleep ("no agent connected");
@@ -16,10 +17,11 @@ import time
 import rclpy
 import uvicorn
 from rclpy.node import Node
-from sensor_msgs.msg import CompressedImage
+from sensor_msgs.msg import CameraInfo, CompressedImage
 from std_msgs.msg import String
+from vision_msgs.msg import Detection2DArray
 
-from .bridge import LatestFrame, build_app, build_mcp
+from .bridge import FacesBuffer, LatestFrame, build_app, build_mcp
 
 
 class McpNode(Node):
@@ -36,6 +38,11 @@ class McpNode(Node):
         self.state_pub = self.create_publisher(String, '/hermes/state', 10)
         self.frames = LatestFrame()
         self.create_subscription(CompressedImage, topic, lambda m: self.frames.put(bytes(m.data)), 1)
+        self.faces = FacesBuffer()
+        self.create_subscription(Detection2DArray, '/vision/faces', self._on_faces, 5)
+        self.create_subscription(CameraInfo, '/camera/camera_info', self._on_camera_info, 1)
+        people_path = os.path.expanduser(str(param('faces_db', '~/.local/share/hermes/faces.json')))
+        hardware = {'speakers': bool(param('speakers', False))}
 
         token = os.environ.get('ROBOT_MCP_TOKEN', '').strip()
         self.activity = None
@@ -45,11 +52,31 @@ class McpNode(Node):
             self.get_logger().error('ROBOT_MCP_TOKEN is missing or too short (openssl rand -hex 32): '
                                     'not serving MCP, and no heartbeat')
             return
-        mcp = build_mcp(self.frames, self._announce_look, head=None, look_timeout_s=look_timeout)
+        mcp = build_mcp(self.frames, self._announce_look, head=None, look_timeout_s=look_timeout,
+                        faces_buf=self.faces, people_path=people_path, hardware=hardware)
         self.app, self.activity = build_app(mcp, token, self.host)
         threading.Thread(target=self._serve, name='mcp-http', daemon=True).start()
         self.create_timer(1.0, self._heartbeat)
 
+    def _on_camera_info(self, msg: CameraInfo) -> None:
+        if msg.width and msg.height:
+            self.faces.set_size(msg.width, msg.height)
+
+    def _on_faces(self, msg: Detection2DArray) -> None:
+        faces = []
+        for d in msg.detections:
+            if not d.results:
+                continue
+            c = d.bbox.center.position
+            faces.append({
+                'track_id': int(d.id) if d.id.isdigit() else hash(d.id) & 0xFFFF,
+                'bbox': {'x': c.x - d.bbox.size_x / 2, 'y': c.y - d.bbox.size_y / 2, 'w': d.bbox.size_x, 'h': d.bbox.size_y},
+                'confidence': d.results[0].hypothesis.score,
+                'name': d.results[1].hypothesis.class_id if len(d.results) > 1 else '',
+                'similarity': d.results[1].hypothesis.score if len(d.results) > 1 else 0.0,
+            })
+        self.faces.put(faces)
+
     def _announce_look(self) -> None:
         self.state_pub.publish(String(data='looking'))
 

ros/hermes_mcp/package.xml

@@ -10,6 +10,7 @@
   <depend>rclpy</depend>
   <depend>sensor_msgs</depend>
   <depend>std_msgs</depend>
+  <depend>vision_msgs</depend>
 
   <test_depend>python3-pytest</test_depend>
 

ros/hermes_mcp/test/test_bridge.py

@@ -1,5 +1,6 @@
 import asyncio
 import base64
+import json
 import threading
 import time
 
@@ -8,7 +9,8 @@ import pytest
 pytest.importorskip('mcp')
 from starlette.testclient import TestClient  # noqa: E402
 
-from hermes_mcp.bridge import Activity, BearerAuth, LatestFrame, build_app, build_mcp  # noqa: E402
+from hermes_mcp.bridge import (Activity, BearerAuth, FacesBuffer, LatestFrame, build_app, build_mcp,  # noqa: E402
+                               people_summary)
 
 TOKEN = 'x' * 32
 
@@ -25,9 +27,11 @@ def tool_names(mcp):
     return {t.name for t in asyncio.run(mcp.list_tools())}
 
 
-def test_tools_are_look_only_without_a_head_and_keep_their_names_with_one():
-    assert tool_names(build_mcp(LatestFrame())) == {'look'}
-    assert tool_names(build_mcp(LatestFrame(), head=FakeHead())) == {'look', 'turn_head', 'head_position'}
+def test_tools_have_no_head_tools_without_a_head_and_keep_their_names_with_one():
+    base = {'look', 'faces', 'list_people'}
+    assert tool_names(build_mcp(LatestFrame())) == base
+    assert tool_names(build_mcp(LatestFrame(), head=FakeHead())) == base | {'turn_head', 'head_position'}
+    assert 'enroll' not in tool_names(build_mcp(LatestFrame(), head=FakeHead()))   # enrolment is the owner's step
 
 
 def test_look_returns_the_next_frame_not_a_stale_one():
@@ -37,8 +41,9 @@ def test_look_returns_the_next_frame_not_a_stale_one():
     mcp = build_mcp(frames, announce_look=lambda: announced.append(1), look_timeout_s=2.0)
     threading.Timer(0.3, lambda: frames.put(b'\xff\xd8fresh')).start()
     result = asyncio.run(mcp.call_tool('look', {}))
-    image = result.content[0]
+    text, image = result.content
     assert announced == [1]
+    assert text.type == 'text' and json.loads(text.text)['faces']['available'] is False   # no detector running here
     assert image.type == 'image' and image.mime_type == 'image/jpeg'
     assert base64.b64decode(image.data) == b'\xff\xd8fresh'       # the frame that arrived after the call
 
@@ -97,3 +102,73 @@ def test_bearer_auth_passes_lifespan_through():
 
     asyncio.run(BearerAuth(inner, TOKEN)({'type': 'lifespan'}, None, None))
     assert seen == ['lifespan']
+
+
+def face(track_id, x, name='', sim=0.0, conf=0.9, w=300, h=400):
+    return {'track_id': track_id, 'bbox': {'x': x, 'y': 100, 'w': w, 'h': h}, 'confidence': conf, 'name': name, 'similarity': sim}
+
+
+def call(mcp, name, args=None):
+    return asyncio.run(mcp.call_tool(name, args or {}))
+
+
+def test_faces_reports_identity_null_for_a_stranger_and_never_makes_up_a_name():
+    buf = FacesBuffer()
+    buf.put([face(1, 100, 'Kyle', 0.82), face(2, 600)])
+    out = call(build_mcp(LatestFrame(), faces_buf=buf), 'faces').structured_content
+    assert out['available'] and out['count'] == 2 and out['image_size'] == {'w': 1296, 'h': 972}
+    kyle, stranger = out['faces']
+    assert kyle['identity'] == {'name': 'Kyle', 'similarity': 0.82}
+    assert stranger['identity'] is None                       # unknown, not guessed
+    assert set(kyle) == {'track_id', 'bbox', 'bbox_norm', 'confidence', 'identity', 'seen_for_s'}
+    assert 'embedding' not in json.dumps(out)
+
+
+def test_track_ids_persist_and_seen_for_grows():
+    buf = FacesBuffer()
+    buf.put([face(7, 100)], t=time.monotonic() - 3.0)
+    buf.put([face(7, 105)])
+    (f,) = buf.summary()['faces']
+    assert f['track_id'] == 7 and 2.5 < f['seen_for_s'] < 3.6
+
+
+def test_faces_says_when_detection_is_not_running_instead_of_reporting_an_empty_room():
+    out = call(build_mcp(LatestFrame(), faces_buf=FacesBuffer()), 'faces').structured_content
+    assert out['available'] is False and 'reason' in out
+    buf = FacesBuffer()
+    buf.put([face(1, 100)], t=time.monotonic() - 30)           # stale data is not "now"
+    assert buf.summary(max_age_s=1.5)['available'] is False
+
+
+def test_an_empty_room_is_available_with_zero_faces():
+    buf = FacesBuffer()
+    buf.put([])
+    assert buf.summary()['available'] and buf.summary()['count'] == 0
+
+
+def test_look_can_skip_the_photo():
+    frames, buf = LatestFrame(), FacesBuffer()
+    buf.put([face(1, 100, 'Kyle', 0.8)])
+    frames.put(b'\xff\xd8x')
+    result = call(build_mcp(frames, faces_buf=buf), 'look', {'send_image': 'never'})
+    assert len(result.content) == 1 and result.content[0].type == 'text'
+    assert json.loads(result.content[0].text)['faces']['faces'][0]['identity']['name'] == 'Kyle'
+
+
+def test_list_people_gives_names_and_counts_never_face_data(tmp_path):
+    path = tmp_path / 'faces.json'
+    path.write_text(json.dumps({'Kyle': [[0.1] * 128] * 10, 'Alex': [[0.2] * 128] * 3}))
+    out = call(build_mcp(LatestFrame(), people_path=str(path)), 'list_people').structured_content
+    assert out == {'count': 2, 'people': [{'name': 'Alex', 'samples': 3}, {'name': 'Kyle', 'samples': 10}]}
+    assert people_summary(str(tmp_path / 'missing.json')) == {'count': 0, 'people': []}
+
+
+def test_the_server_can_be_discovered_resources_and_prompts_are_not_empty():
+    mcp = build_mcp(LatestFrame(), hardware={'speakers': False})
+    assert [str(r.uri) for r in asyncio.run(mcp.list_resources())] == ['robot://status']
+    assert {p.name for p in asyncio.run(mcp.list_prompts())} == {'who_is_here', 'look_around'}
+    status = json.loads(asyncio.run(mcp.read_resource('robot://status'))[0].content)
+    assert status['hardware'] == {'camera': True, 'head': False, 'speakers': False}
+    assert status['detects'] == ['faces'] and 'people' in status['not_detected']
+    assert 'embeddings never leave it' in status['recognition']['registry']
+    assert 'turn_head' not in status['tools']

ros/hermes_mcp/test/test_node.py

@@ -0,0 +1,16 @@
+import pytest
+
+rclpy = pytest.importorskip('rclpy')
+
+
+def test_the_node_constructs_without_a_token_and_does_not_serve(monkeypatch):
+    """Declaring a parameter twice raises at construction, and nothing else exercises the node."""
+    monkeypatch.delenv('ROBOT_MCP_TOKEN', raising=False)
+    from hermes_mcp.mcp_node import McpNode
+    rclpy.init()
+    try:
+        node = McpNode()
+        assert node.activity is None            # no token: not serving, no heartbeat
+        node.destroy_node()
+    finally:
+        rclpy.shutdown()

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