Add opt-in face recognition to hermes_vision

SFace embeddings on top of YuNet's landmarks. Nothing is stored until someone is
enrolled with `python -m hermes_vision.people enroll NAME`; only 128-number
embeddings are kept, never images, in a 0600 JSON file under a 0700 directory,
written atomically. `list` and `forget` manage it. The detector node puts a
recognised name in Detection2D.id (empty if unknown) and picks up database
changes without a restart.

Checked with a throwaway database on the burst frames: eight of nine unenrolled
frames were recognised, down to side profiles (similarity 0.52-0.66), the miss
being a head tipped back at 0.34 against the 0.363 threshold, and an unrelated
photo scored 0.16 and stayed unknown.

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

ros/hermes_vision/hermes_vision/detect.py

@@ -22,6 +22,7 @@ class Face:
     w: int
     h: int
     score: float = 1.0
+    landmarks: tuple = ()   # YuNet's five points (eyes, nose, mouth corners) as x0,y0,...,x4,y4; empty for Haar
 
 
 def yunet_input_size(w: int, h: int, max_side: int = 640) -> tuple[int, int]:
@@ -81,7 +82,8 @@ class FaceDetector:
         if found is None:
             return []
         fx, fy = w0 / tw, h0 / th
-        return [Face(int(f[0] * fx), int(f[1] * fy), int(f[2] * fx), int(f[3] * fy), float(f[14]))
+        return [Face(int(f[0] * fx), int(f[1] * fy), int(f[2] * fx), int(f[3] * fy), float(f[14]),
+                     tuple(float(v) * (fx if i % 2 == 0 else fy) for i, v in enumerate(f[4:14])))
                 for f in found if f[2] * fx >= self.min_size]
 
     def _detect_haar(self, bgr) -> list[Face]:

ros/hermes_vision/hermes_vision/face_detector_node.py

@@ -3,9 +3,12 @@
 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.
              /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.
 """
+import os
 import time
 
 import cv2
@@ -16,6 +19,7 @@ from sensor_msgs.msg import CompressedImage
 from vision_msgs.msg import Detection2D, Detection2DArray, ObjectHypothesisWithPose
 
 from .detect import FaceDetector
+from .recognize import COSINE_THRESHOLD, DEFAULT_DB, DEFAULT_MODEL, FaceRecognizer
 
 
 class FaceDetectorNode(Node):
@@ -27,12 +31,19 @@ class FaceDetectorNode(Node):
         self.debug = bool(param('publish_debug', False))
         self.detector = FaceDetector(param('model_path', ''), float(param('score_threshold', 0.6)),
                                      int(param('min_size', 40)))
+        self.recognizer = None
+        if bool(param('recognize', True)) and self.detector.backend == 'yunet' \
+                and os.path.exists(os.path.expanduser(param('recognition_model', DEFAULT_MODEL))):
+            self.recognizer = FaceRecognizer(param('recognition_model', DEFAULT_MODEL), param('faces_db', DEFAULT_DB),
+                                             float(param('recognition_threshold', COSINE_THRESHOLD)))
         self.pub = self.create_publisher(Detection2DArray, '/vision/faces', 10)
         self.debug_pub = (self.create_publisher(CompressedImage, '/vision/faces/debug/compressed', 1)
                           if self.debug else None)
         self.create_subscription(CompressedImage, topic, self._on_image, 1)
         self._next = 0.0
-        self.get_logger().info(f'face detector up: {self.detector.backend}, {1 / self.period:g} Hz, {topic}')
+        who = ', '.join(self.recognizer.enrolled) if self.recognizer else 'off'
+        self.get_logger().info(f'face detector up: {self.detector.backend}, {1 / self.period:g} Hz, {topic}; '
+                               f'recognising: {who or "nobody enrolled yet"}')
 
     def _on_image(self, msg: CompressedImage) -> None:
         now = time.monotonic()
@@ -47,6 +58,8 @@ class FaceDetectorNode(Node):
         out = Detection2DArray(header=msg.header)
         for f in faces:
             d = Detection2D(header=msg.header)
+            if self.recognizer:
+                d.id = self.recognizer.identify(img, f)[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.size_x, d.bbox.size_y = float(f.w), float(f.h)

ros/hermes_vision/hermes_vision/people.py

@@ -0,0 +1,126 @@
+"""Manage who the robot recognises. Enrolling is a deliberate step; forgetting is one command.
+
+  python -m hermes_vision.people list
+  python -m hermes_vision.people enroll NAME [--samples 10]        # live, from the camera topic
+  python -m hermes_vision.people enroll-images NAME IMAGE [IMAGE ...]
+  python -m hermes_vision.people forget NAME
+  python -m hermes_vision.people check IMAGE [IMAGE ...]           # who is in these images
+
+Only embeddings are stored (see hermes_vision.recognize), never photos. --db picks another file.
+"""
+import argparse
+import sys
+import time
+
+import cv2
+import numpy as np
+
+from .detect import FaceDetector
+from .recognize import DEFAULT_DB, DEFAULT_MODEL, FaceRecognizer, load_db, save_db
+
+
+def _sample(detector, recognizer, bgr, min_score=0.85, min_size=120):
+    """One embedding if the frame has exactly one good, large face; else None."""
+    faces = detector.detect(bgr)
+    if len(faces) != 1 or faces[0].score < min_score or faces[0].w < min_size:
+        return None
+    return recognizer.embed(bgr, faces[0])
+
+
+def _add(db_path, name, embeddings):
+    db = load_db(db_path)
+    db.setdefault(name, []).extend(embeddings)
+    save_db(db, db_path)
+    print(f'{name}: {len(db[name])} sample(s) stored in {db_path}')
+
+
+def enroll_live(args, detector, recognizer):
+    import rclpy
+    from rclpy.node import Node
+    from sensor_msgs.msg import CompressedImage
+    rclpy.init()
+    node = Node('hermes_enroll')
+    got, last = [], [0.0]
+
+    def on_image(msg):
+        if len(got) >= args.samples or time.monotonic() - last[0] < 0.8:
+            return
+        bgr = cv2.imdecode(np.frombuffer(msg.data, np.uint8), cv2.IMREAD_COLOR)
+        emb = None if bgr is None else _sample(detector, recognizer, bgr)
+        if emb is not None:
+            got.append(emb)
+            last[0] = time.monotonic()
+            print(f'  sample {len(got)}/{args.samples}: turn your head a little between samples', flush=True)
+
+    node.create_subscription(CompressedImage, args.topic, on_image, 1)
+    print(f'Enrolling {args.name!r}: look at the camera, alone in frame. Waiting for {args.samples} samples...')
+    t0 = time.monotonic()
+    while len(got) < args.samples and time.monotonic() - t0 < args.timeout:
+        rclpy.spin_once(node, timeout_sec=0.2)
+    rclpy.shutdown()
+    if len(got) < args.samples:
+        print(f'only {len(got)} usable sample(s) in {args.timeout:.0f} s; nothing stored '
+              '(need exactly one large, clear face in frame)')
+        return 1
+    _add(args.db, args.name, got)
+    return 0
+
+
+def main():
+    p = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter)
+    p.add_argument('--db', default=DEFAULT_DB)
+    p.add_argument('--model', default=DEFAULT_MODEL)
+    sub = p.add_subparsers(dest='cmd', required=True)
+    sub.add_parser('list')
+    e = sub.add_parser('enroll')
+    e.add_argument('name')
+    e.add_argument('--samples', type=int, default=10)
+    e.add_argument('--timeout', type=float, default=60)
+    e.add_argument('--topic', default='/camera/image_raw/compressed')
+    ei = sub.add_parser('enroll-images')
+    ei.add_argument('name')
+    ei.add_argument('images', nargs='+')
+    f = sub.add_parser('forget')
+    f.add_argument('name')
+    c = sub.add_parser('check')
+    c.add_argument('images', nargs='+')
+    args = p.parse_args()
+
+    if args.cmd == 'list':
+        db = load_db(args.db)
+        print('\n'.join(f'{n}: {len(s)} sample(s)' for n, s in sorted(db.items())) or 'nobody enrolled')
+        return 0
+    if args.cmd == 'forget':
+        db = load_db(args.db)
+        if args.name not in db:
+            print(f'{args.name!r} is not enrolled')
+            return 1
+        del db[args.name]
+        save_db(db, args.db)
+        print(f'forgot {args.name!r}')
+        return 0
+
+    detector, recognizer = FaceDetector(), FaceRecognizer(args.model, args.db)
+    if detector.backend != 'yunet':
+        print('recognition needs the YuNet detector (landmarks); is OpenCV >= 4.8 on the path?')
+        return 2
+    if args.cmd == 'enroll':
+        return enroll_live(args, detector, recognizer)
+    if args.cmd == 'enroll-images':
+        embs = [e for e in (_sample(detector, recognizer, cv2.imread(i)) for i in args.images) if e is not None]
+        print(f'{len(embs)} of {len(args.images)} image(s) had exactly one good face')
+        if embs:
+            _add(args.db, args.name, embs)
+        return 0 if embs else 1
+    for path in args.images:                                     # check
+        bgr = cv2.imread(path)
+        for face in detector.detect(bgr):
+            name, sim = recognizer.identify(bgr, face)
+            print(f'{path}: face at ({face.x},{face.y}) {face.w}x{face.h} -> {name or "unknown"} (similarity {sim:.2f})')
+        else:
+            pass
+    return 0
+
+
+if __name__ == '__main__':
+    sys.exit(main())

ros/hermes_vision/hermes_vision/recognize.py

@@ -0,0 +1,100 @@
+"""Recognise enrolled people from faces. Opt-in: nothing is stored until someone is enrolled.
+
+Uses OpenCV's SFace on top of YuNet's landmarks (they align the face crop). What is stored is a
+set of 128-number embeddings per person, never images, in a JSON file readable only by its owner
+(default ~/.local/share/hermes/faces.json). No ROS imports.
+"""
+from __future__ import annotations
+
+import json
+import os
+import tempfile
+from typing import Optional
+
+import cv2
+import numpy as np
+
+from .detect import Face
+
+DEFAULT_MODEL = os.path.expanduser('~/models/face_recognition_sface_2021dec.onnx')
+DEFAULT_DB = os.path.expanduser('~/.local/share/hermes/faces.json')
+COSINE_THRESHOLD = 0.363     # SFace's recommended cosine-similarity threshold for "same person"
+
+
+def load_db(path: str = DEFAULT_DB) -> dict[str, list[np.ndarray]]:
+    try:
+        with open(path) as f:
+            raw = json.load(f)
+    except FileNotFoundError:
+        return {}
+    return {name: [np.asarray(e, np.float32) for e in embs] for name, embs in raw.items()}
+
+
+def save_db(db: dict[str, list[np.ndarray]], path: str = DEFAULT_DB) -> None:
+    """Write atomically, mode 0600, so a crash never leaves a half-written or world-readable file."""
+    os.makedirs(os.path.dirname(path), mode=0o700, exist_ok=True)
+    fd, tmp = tempfile.mkstemp(dir=os.path.dirname(path), prefix='.faces-')
+    try:
+        with os.fdopen(fd, 'w') as f:
+            json.dump({n: [e.tolist() for e in embs] for n, embs in db.items()}, f)
+        os.chmod(tmp, 0o600)
+        os.replace(tmp, path)
+    except BaseException:
+        os.unlink(tmp)
+        raise
+
+
+def cosine(a: np.ndarray, b: np.ndarray) -> float:
+    return float(np.dot(a, b) / (np.linalg.norm(a) * np.linalg.norm(b) + 1e-12))
+
+
+def best_match(emb: np.ndarray, db: dict[str, list[np.ndarray]],
+               threshold: float = COSINE_THRESHOLD) -> tuple[str, float]:
+    """The enrolled person whose closest sample is most similar, or ('', score) below the threshold."""
+    best_name, best = '', -1.0
+    for name, samples in db.items():
+        for s in samples:
+            c = cosine(emb, s)
+            if c > best:
+                best_name, best = name, c
+    return (best_name, best) if best >= threshold else ('', best)
+
+
+class FaceRecognizer:
+    def __init__(self, model_path: str = DEFAULT_MODEL, db_path: str = DEFAULT_DB,
+                 threshold: float = COSINE_THRESHOLD):
+        self.threshold = threshold
+        self.db_path = db_path
+        self._net = cv2.FaceRecognizerSF.create(os.path.expanduser(model_path), '')
+        self._db: dict[str, list[np.ndarray]] = {}
+        self._mtime = -1.0
+        self.reload()
+
+    def reload(self) -> None:
+        """Re-read the database if it changed on disk (so enrolling does not need a restart)."""
+        try:
+            m = os.stat(self.db_path).st_mtime
+        except FileNotFoundError:
+            self._db, self._mtime = {}, -1.0
+            return
+        if m != self._mtime:
+            self._db, self._mtime = load_db(self.db_path), m
+
+    @property
+    def enrolled(self) -> list[str]:
+        return sorted(self._db)
+
+    def embed(self, bgr, face: Face) -> Optional[np.ndarray]:
+        if len(face.landmarks) != 10:        # Haar faces have no landmarks to align with
+            return None
+        row = np.array([face.x, face.y, face.w, face.h, *face.landmarks, face.score], np.float32)
+        aligned = self._net.alignCrop(bgr, row)
+        return self._net.feature(aligned).reshape(-1).astype(np.float32)
+
+    def identify(self, bgr, face: Face) -> tuple[str, float]:
+        """('name', similarity), or ('', similarity) if nobody enrolled matches."""
+        self.reload()
+        if not self._db:
+            return '', 0.0
+        emb = self.embed(bgr, face)
+        return ('', 0.0) if emb is None else best_match(emb, self._db, self.threshold)

ros/hermes_vision/setup.py

@@ -20,6 +20,7 @@ setup(
         'console_scripts': [
             'face_detector = hermes_vision.face_detector_node:main',
             'detect_faces = hermes_vision.detect_cli:main',
+            'people = hermes_vision.people:main',
         ],
     },
 )

ros/hermes_vision/test/test_recognize.py

@@ -0,0 +1,49 @@
+import json
+import os
+import stat
+
+import numpy as np
+import pytest
+
+from hermes_vision.recognize import best_match, cosine, load_db, save_db
+
+
+def vec(seed, noise=0.0, base=None):
+    r = np.random.default_rng(seed)
+    v = r.normal(size=128).astype(np.float32) if base is None else base
+    return v + noise * r.normal(size=128).astype(np.float32)
+
+
+def test_cosine_basics():
+    a = vec(1)
+    assert cosine(a, a) == pytest.approx(1.0)
+    assert cosine(a, -a) == pytest.approx(-1.0)
+
+
+def test_best_match_picks_the_closest_person_and_rejects_strangers():
+    alice, bob = vec(1), vec(2)
+    db = {'alice': [alice, vec(1, 0.3, alice)], 'bob': [bob]}
+    assert best_match(vec(9, 0.2, alice), db)[0] == 'alice'
+    assert best_match(vec(9, 0.2, bob), db)[0] == 'bob'
+    name, sim = best_match(vec(77), db)            # an unrelated face
+    assert name == '' and sim < 0.363
+
+
+def test_empty_db_matches_nobody():
+    assert best_match(vec(1), {}) == ('', -1.0)
+
+
+def test_db_roundtrip_is_private_and_atomic(tmp_path):
+    path = str(tmp_path / 'sub' / 'faces.json')
+    save_db({'alice': [vec(1), vec(2)]}, path)
+    assert stat.S_IMODE(os.stat(path).st_mode) == 0o600
+    assert stat.S_IMODE(os.stat(os.path.dirname(path)).st_mode) == 0o700
+    back = load_db(path)
+    assert list(back) == ['alice'] and len(back['alice']) == 2
+    assert np.allclose(back['alice'][0], vec(1))
+    assert [f for f in os.listdir(os.path.dirname(path)) if f.startswith('.faces-')] == []   # no temp litter
+    assert json.load(open(path)).keys() == {'alice'}
+
+
+def test_load_missing_db_is_empty(tmp_path):
+    assert load_db(str(tmp_path / 'nope.json')) == {}