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:
+26
-2
@@ -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
|
||||
`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` /
|
||||
`head_position` only when a head is fitted. With no servos only `look` is registered, by design
|
||||
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 those two are not registered, by design
|
||||
(`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,
|
||||
~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
|
||||
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)
|
||||
|
||||
`hermes_vision.recognize` / `people`: SFace embeddings on YuNet's landmarks. Nothing is stored until
|
||||
|
||||
@@ -100,10 +100,17 @@ door.
|
||||
|
||||
```
|
||||
ping 10.55.0.2
|
||||
hermes mcp test robot # connects, finds the look tool
|
||||
hermes chat -q "Use the look tool and tell me what you see."
|
||||
hermes mcp test robot # connects, lists the tools
|
||||
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
|
||||
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
|
||||
|
||||
@@ -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]
|
||||
@@ -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' \
|
||||
&& 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"
|
||||
before="$(cfg_sum)"
|
||||
bash "$PI_DIR/install-usb-gadget.sh"
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
|
||||
@@ -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'))
|
||||
|
||||
|
||||
@@ -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>
|
||||
|
||||
|
||||
@@ -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']
|
||||
|
||||
@@ -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()
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -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:
|
||||
|
||||
@@ -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
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
Reference in New Issue
Block a user