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
This commit is contained in:
2026-10-05 01:21:32 -07:00
co-authored by Claude Sonnet 5.5
parent 497f026dc9
commit 3b65697f1e
16 changed files with 599 additions and 44 deletions
+26 -2
View File
@@ -211,8 +211,8 @@ hotter, so fit the fan before enclosing it. Playback and the servos are untested
The only way in or out of the ROS graph. Serves MCP (SDK 2.3, streamable HTTP) on The only way in or out of the ROS graph. Serves MCP (SDK 2.3, streamable HTTP) on
`http://10.55.0.2:8765/mcp`, the USB address only (retrying the bind if the link is not up yet), with `http://10.55.0.2:8765/mcp`, the USB address only (retrying the bind if the link is not up yet), with
a bearer token. **Tool names are the stable contract with Hermes**: `look()`, and `turn_head` / a bearer token. **Tool names are the stable contract with Hermes**: `look()`, `faces()`, `list_people()`, and `turn_head` /
`head_position` only when a head is fitted. With no servos only `look` is registered, by design `head_position` only when a head is fitted. With no servos those two are not registered, by design
(`head: false`; the node refuses `head: true` until `hermes_head` exists). `look()` waits for the next (`head: false`; the node refuses `head: true` until `hermes_head` exists). `look()` waits for the next
camera frame taken after the call (never a stale one) and passes the camera's JPEG through (1296x972, camera frame taken after the call (never a stale one) and passes the camera's JPEG through (1296x972,
~155 KB); it also publishes `looking` on `/hermes/state` so the face squints and flashes. ~155 KB); it also publishes `looking` on `/hermes/state` so the face squints and flashes.
@@ -250,6 +250,30 @@ Hermes does it itself, so `look()` needs no change. Two consequences: each look(
room in talos's Hermes image cache (Hermes's retention, not the robot's), and a look-and-describe turn room in talos's Hermes image cache (Hermes's retention, not the robot's), and a look-and-describe turn
takes ~70 s on the local models, mostly the vision call. takes ~70 s on the local models, mostly the vision call.
### Hermes's feedback on the bridge (2026-10-05) and what was done
Hermes (on talos, from `~/.hermes/cache/scratch/hermes-bot-v0-face-identity.md`) asked for recognitions
instead of raw photos. **Built:** `faces()` returns the faces in view as structured JSON with no photo
(`track_id`, pixel and normalised `bbox`, detection `confidence`, `seen_for_s`, and `identity`
`{name, similarity}` or `null`); `look(send_image="always"|"never")` now returns that same JSON as a
text block plus the photo (default kept as before: the image, so scene description still works);
`list_people()` gives names and sample counts; a `robot://status` resource (hardware present or
absent, what is detected, enrolled count, tools) and prompts `who_is_here` and `look_around` fill the
previously empty `list_resources` / `list_prompts`; `/vision/faces` carries a persistent track id
(`hermes_vision.tracking.FaceTracker`, matching by box overlap, identity re-checked once a second;
`Detection2D.id` is the track id, `results[0]` the face, `results[1]` the identity if recognised).
Verified with a real Hermes turn on the local `ornith` model: it called `faces()` (0.03 s, 593
characters, no image, no vision call) and answered that Kyle was there at 0.78 similarity.
**Declined, deliberately:** embeddings in responses (biometric templates, meaningless to an LLM,
and the registry stays on the robot); age/gender/hat attributes (no model for them here, and guessing
age and gender from faces is unreliable and sensitive); auto-updating a person's face data from new
captures (it lets a stranger slowly become "Kyle" with nobody deciding that); a remote `enroll` tool
(a prompt could make the robot call anyone "Kyle": enrolment stays the owner's step on the robot,
`python3 -m hermes_vision.people enroll NAME`; a gated enrol tool, off by default, is possible if the
owner wants it); detecting people, objects or motion (only faces exist, and `robot://status` says so).
`look()`'s default was not flipped to JSON-only, because the image is what lets Hermes describe a
scene; that default is the owner's call.
## Face recognition (opt-in) ## Face recognition (opt-in)
`hermes_vision.recognize` / `people`: SFace embeddings on YuNet's landmarks. Nothing is stored until `hermes_vision.recognize` / `people`: SFace embeddings on YuNet's landmarks. Nothing is stored until
+9 -2
View File
@@ -100,10 +100,17 @@ door.
``` ```
ping 10.55.0.2 ping 10.55.0.2
hermes mcp test robot # connects, finds the look tool hermes mcp test robot # connects, lists the tools
hermes chat -q "Use the look tool and tell me what you see." hermes chat -q "Who is at the robot's desk right now?" # calls faces(): instant, no photo
hermes chat -q "Use the look tool and tell me what you see." # sends a photo
``` ```
What Hermes can use: `faces()` (who is in view: names, positions, tracking ids; instant, no photo),
`look()` (the same faces summary plus a current photo), `list_people()`, and, once a head is fitted,
`turn_head` and `head_position`. `robot://status` says what hardware this robot actually has. Only faces
are detected. Face data never leaves the robot: Hermes gets names and similarity scores, and an
unrecognised face is `null`, never a guess.
The robot's face wakes when Hermes connects and falls asleep about 40 seconds after it leaves. A The robot's face wakes when Hermes connects and falls asleep about 40 seconds after it leaves. A
look-and-describe turn takes a minute or so on local models: `look()` itself is instant. Hermes does not look-and-describe turn takes a minute or so on local models: `look()` itself is instant. Hermes does not
give MCP images to the main model: it saves the photo to its image cache and the model calls give MCP images to the main model: it saves the photo to its image cache and the model calls
@@ -0,0 +1,20 @@
image_width: 1296
image_height: 972
camera_name: ov5647__base_soc_i2c0mux_i2c_1_ov5647_36_1296x972
camera_matrix:
rows: 3
cols: 3
data: [1152.96331, 0, 652.952458, 0, 1151.52195, 463.351689, 0, 0, 1]
distortion_model: plumb_bob
distortion_coefficients:
rows: 1
cols: 5
data: [0.0727701258, -0.232756953, 0.00468014288, -0.00488550855, 0]
rectification_matrix:
rows: 3
cols: 3
data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
projection_matrix:
rows: 3
cols: 4
data: [1152.96331, 0, 652.952458, 0, 0, 1151.52195, 463.351689, 0, 0, 0, 1, 0]
+7
View File
@@ -116,6 +116,13 @@ chown -R "$ROBOT_USER:$ROBOT_USER" "$HOME_DIR/ros_ws/src"
as_user bash -c "source /opt/ros/jazzy/setup.bash && source '$HOME_DIR/camera_ws/install/setup.bash' \ as_user bash -c "source /opt/ros/jazzy/setup.bash && source '$HOME_DIR/camera_ws/install/setup.bash' \
&& cd '$HOME_DIR/ros_ws' && colcon build --symlink-install" && cd '$HOME_DIR/ros_ws' && colcon build --symlink-install"
step "Camera calibration (this camera's, from pi/calibration; an existing file is never overwritten)"
as_user mkdir -p "$HOME_DIR/.ros/camera_info"
for f in "$PI_DIR"/calibration/*.yaml; do
[ -e "$f" ] || continue
[ -e "$HOME_DIR/.ros/camera_info/$(basename "$f")" ] || install -o "$ROBOT_USER" -g "$ROBOT_USER" -m 644 "$f" "$HOME_DIR/.ros/camera_info/"
done
step "USB gadget (NCM Ethernet + ACM serial) and the audio codec overlay" step "USB gadget (NCM Ethernet + ACM serial) and the audio codec overlay"
before="$(cfg_sum)" before="$(cfg_sum)"
bash "$PI_DIR/install-usb-gadget.sh" bash "$PI_DIR/install-usb-gadget.sh"
+1
View File
@@ -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 bind: 10.55.0.2 # the USB link; the firewall accepts 8765 only from 10.55.0.1 on usb0
port: 8765 port: 8765
head: false # no servos fitted yet: turn_head and head_position stay unregistered 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 connected_grace_s: 30.0 # an authenticated client heard within this long keeps the heartbeat going
look_timeout_s: 3.0 look_timeout_s: 3.0
+4 -2
View File
@@ -3,7 +3,8 @@
Subscribes: /hermes/state (std_msgs/String): sleeping idle listening thinking looking speaking 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 /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. 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 /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/activity (std_msgs/String): speaking | listening | quiet, from hermes_audio
/audio/scope/{mic,speaker} (std_msgs/Float32MultiArray): the real waveform for the mouth /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: def _on_faces(self, msg: Detection2DArray) -> None:
target = pick_target([ target = pick_target([
Target(d.bbox.center.position.x, d.bbox.center.position.y, d.bbox.size_x, d.bbox.size_y, 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]) for d in msg.detections])
if target: if target:
gx, gy = gaze_from_pixel(target.cx, target.cy, *self.image_size) gx, gy = gaze_from_pixel(target.cx, target.cy, *self.image_size)
+145 -10
View File
@@ -2,16 +2,25 @@
so it is testable with a stub camera. 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: 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) turn_head(pan, tilt) -> only registered if a head backend is given (servos fitted)
head_position() -> likewise 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 from __future__ import annotations
import hmac import hmac
import json
import os
import threading import threading
import time 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.mcpserver import Image, MCPServer
from mcp.server.transport_security import TransportSecuritySettings from mcp.server.transport_security import TransportSecuritySettings
@@ -43,32 +52,131 @@ class LatestFrame:
return self._jpeg 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): class Head(Protocol):
def move(self, pan: float, tilt: float) -> tuple[float, float]: ... def move(self, pan: float, tilt: float) -> tuple[float, float]: ...
def position(self) -> tuple[float, float]: ... def position(self) -> tuple[float, float]: ...
def build_mcp(frames: LatestFrame, announce_look: Callable[[], None] = lambda: None, 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( mcp = MCPServer(
"robot", "robot",
instructions=( instructions=(
"This server is a small physical robot on the user's desk. " "This server is a small physical robot on the user's desk, with a camera and a face display. "
"Call look() whenever you need to see what is in front of it; the image is current. " "To know WHO or HOW MANY people are there, call faces(): it is instant and returns names, "
"If turn_head is available, aim first and then look." "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() @mcp.tool()
def look() -> Image: def faces() -> dict[str, Any]:
"""Take a photograph with the robot's camera and return it. Use this to see the room, """Report the faces the robot's camera sees right now, with no photo (instant and cheap).
the desk, the person, or anything the user asks you to look at.""" 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() t0 = time.monotonic()
announce_look() # the face squints and flashes 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) jpeg = frames.wait_newer(t0, look_timeout_s)
if jpeg is None: if jpeg is None:
raise RuntimeError("the robot's camera is not producing images") 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: if head is not None:
@mcp.tool() @mcp.tool()
@@ -86,6 +194,33 @@ def build_mcp(frames: LatestFrame, announce_look: Callable[[], None] = lambda: N
p, t = head.position() p, t = head.position()
return f"pan {p:+.0f} degrees, tilt {t:+.0f} degrees" 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 return mcp
+30 -3
View File
@@ -2,6 +2,7 @@
Serves MCP (streamable HTTP, bearer token) to Hermes on the USB link and talks to the graph locally. 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) 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): Publishes: /hermes/state (std_msgs/String):
`idle` once a second while an authenticated MCP client is connected: the agent heartbeat `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"); the face uses to wake and, when it stops, to fall asleep ("no agent connected");
@@ -16,10 +17,11 @@ import time
import rclpy import rclpy
import uvicorn import uvicorn
from rclpy.node import Node 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 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): class McpNode(Node):
@@ -36,6 +38,11 @@ class McpNode(Node):
self.state_pub = self.create_publisher(String, '/hermes/state', 10) self.state_pub = self.create_publisher(String, '/hermes/state', 10)
self.frames = LatestFrame() self.frames = LatestFrame()
self.create_subscription(CompressedImage, topic, lambda m: self.frames.put(bytes(m.data)), 1) 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() token = os.environ.get('ROBOT_MCP_TOKEN', '').strip()
self.activity = None 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): ' self.get_logger().error('ROBOT_MCP_TOKEN is missing or too short (openssl rand -hex 32): '
'not serving MCP, and no heartbeat') 'not serving MCP, and no heartbeat')
return 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) self.app, self.activity = build_app(mcp, token, self.host)
threading.Thread(target=self._serve, name='mcp-http', daemon=True).start() threading.Thread(target=self._serve, name='mcp-http', daemon=True).start()
self.create_timer(1.0, self._heartbeat) 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: def _announce_look(self) -> None:
self.state_pub.publish(String(data='looking')) self.state_pub.publish(String(data='looking'))
+1
View File
@@ -10,6 +10,7 @@
<depend>rclpy</depend> <depend>rclpy</depend>
<depend>sensor_msgs</depend> <depend>sensor_msgs</depend>
<depend>std_msgs</depend> <depend>std_msgs</depend>
<depend>vision_msgs</depend>
<test_depend>python3-pytest</test_depend> <test_depend>python3-pytest</test_depend>
+80 -5
View File
@@ -1,5 +1,6 @@
import asyncio import asyncio
import base64 import base64
import json
import threading import threading
import time import time
@@ -8,7 +9,8 @@ import pytest
pytest.importorskip('mcp') pytest.importorskip('mcp')
from starlette.testclient import TestClient # noqa: E402 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 TOKEN = 'x' * 32
@@ -25,9 +27,11 @@ def tool_names(mcp):
return {t.name for t in asyncio.run(mcp.list_tools())} 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(): def test_tools_have_no_head_tools_without_a_head_and_keep_their_names_with_one():
assert tool_names(build_mcp(LatestFrame())) == {'look'} base = {'look', 'faces', 'list_people'}
assert tool_names(build_mcp(LatestFrame(), head=FakeHead())) == {'look', 'turn_head', 'head_position'} 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(): 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) 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() threading.Timer(0.3, lambda: frames.put(b'\xff\xd8fresh')).start()
result = asyncio.run(mcp.call_tool('look', {})) result = asyncio.run(mcp.call_tool('look', {}))
image = result.content[0] text, image = result.content
assert announced == [1] 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 image.type == 'image' and image.mime_type == 'image/jpeg'
assert base64.b64decode(image.data) == b'\xff\xd8fresh' # the frame that arrived after the call 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)) asyncio.run(BearerAuth(inner, TOKEN)({'type': 'lifespan'}, None, None))
assert seen == ['lifespan'] 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']
+16
View File
@@ -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()
+80 -8
View File
@@ -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) 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]): 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).""" """OpenCV calibration. Returns (rms reprojection error in px, camera matrix, distortion).
rms, k, d, _, _ = cv2.calibrateCamera(obj_pts, img_pts, size, None, None)
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) 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: 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).""" """The ROS camera_info YAML (the format camera_calibration_parsers reads)."""
w, h = size w, h = size
@@ -80,8 +131,14 @@ def main() -> int:
p.add_argument('--topic', default='/camera/image_raw/compressed') p.add_argument('--topic', default='/camera/image_raw/compressed')
p.add_argument('--name', default=DEFAULT_NAME) p.add_argument('--name', default=DEFAULT_NAME)
p.add_argument('--out-dir', default=os.path.expanduser('~/.ros/camera_info')) 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() 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 import rclpy
from rclpy.node import Node from rclpy.node import Node
from sensor_msgs.msg import CompressedImage from sensor_msgs.msg import CompressedImage
@@ -111,7 +168,9 @@ def main() -> int:
taken_sigs.append(sig) taken_sigs.append(sig)
obj_pts.append(obj) obj_pts.append(obj)
img_pts.append(corners) 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) 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; ' 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: if len(img_pts) < 10:
print(f'only {len(img_pts)} usable views; nothing written (need at least 10, ideally {a.views})') print(f'only {len(img_pts)} usable views; nothing written (need at least 10, ideally {a.views})')
return 1 return 1
size = state['size'] return finish(a, obj_pts, img_pts, state['size'], resave=True)
rms, k, d = solve(obj_pts, img_pts, size)
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]}' name = f'{a.name}_{size[0]}x{size[1]}'
os.makedirs(a.out_dir, exist_ok=True) os.makedirs(a.out_dir, exist_ok=True)
path = os.path.join(a.out_dir, name + '.yaml') path = os.path.join(a.out_dir, name + '.yaml')
with open(path, 'w') as f: with open(path, 'w') as f:
f.write(camera_info_yaml(name, size, k, d)) 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"})') if resave:
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]}') 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') print(f'wrote {path}\nrestart the robot to load it: sudo systemctl restart robot')
return 0 return 0
@@ -1,10 +1,13 @@
"""ROS node: faces in the camera image. """ROS node: faces in the camera image.
Subscribes: image_topic (sensor_msgs/CompressedImage, default /camera/image_raw/compressed) Subscribes: image_topic (sensor_msgs/CompressedImage, default /camera/image_raw/compressed)
Publishes: /vision/faces (vision_msgs/Detection2DArray): one detection per face, bbox in pixels Publishes: /vision/faces (vision_msgs/Detection2DArray): one detection per face, bbox in full-size
(center and size), class_id 'face', score = detector confidence; header from the image. camera pixels (center and size), header from the image. Detection2D.id is the track id
Detection2D.id is the recognised person's name, or empty if unknown or recognition is (a number as a string, stable while the person stays in view). results[0] is the face
off. Recognition only runs for people enrolled with hermes_vision.people. 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. /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. 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 vision_msgs.msg import Detection2D, Detection2DArray, ObjectHypothesisWithPose
from .detect import Face, FaceDetector 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): 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._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)), 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))) 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 recognition_model = os.path.expanduser(param('recognition_model', DEFAULT_MODEL)) # declare once
faces_db = param('faces_db', DEFAULT_DB) faces_db = param('faces_db', DEFAULT_DB)
threshold = float(param('recognition_threshold', COSINE_THRESHOLD)) 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) self.get_logger().warn('could not decode image', throttle_duration_sec=10.0)
return return
faces = self.detector.detect(img) # in the (possibly half-size) decoded image 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]) tracks = self.tracker.update(faces, now, (lambda f: self.recognizer.identify(img, f)) if self.recognizer else None)
if self.recognizer else [''] * len(faces))
k = self.scale # published boxes are in full-size camera pixels k = self.scale # published boxes are in full-size camera pixels
out = Detection2DArray(header=msg.header) out = Detection2DArray(header=msg.header)
for f, name in zip(faces, names): for t in tracks:
f = Face(f.x * k, f.y * k, f.w * k, f.h * k, f.score) 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 = 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.x = f.x + f.w / 2.0
d.bbox.center.position.y = f.y + f.h / 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) d.bbox.size_x, d.bbox.size_y = float(f.w), float(f.h)
hyp = ObjectHypothesisWithPose() hyp = ObjectHypothesisWithPose()
hyp.hypothesis.class_id, hyp.hypothesis.score = 'face', f.score hyp.hypothesis.class_id, hyp.hypothesis.score = 'face', f.score
d.results.append(hyp) 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) out.detections.append(d)
self.pub.publish(out) self.pub.publish(out)
if self.debug_pub: if self.debug_pub:
@@ -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
+42 -1
View File
@@ -2,7 +2,8 @@ import cv2
import numpy as np import numpy as np
import yaml 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) SIZE = (1296, 972)
K_TRUE = np.array([[1100.0, 0, 650.0], [0, 1090.0, 480.0], [0, 0, 1]]) 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) corners = find_board(bgr, 9, 6)
assert corners is not None and corners.reshape(-1, 2).shape == (54, 2) 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 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
+55
View File
@@ -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