From cf711060d09e82f68941830b4209f1da19fa6866 Mon Sep 17 00:00:00 2001 From: "K. Isom" Date: Sun, 4 Oct 2026 23:13:49 -0700 Subject: [PATCH] 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 Claude-Session: https://claude.ai/code/session_01AFN8nRRCiDiMN7ChBpHJhs --- ros/hermes_vision/hermes_vision/detect.py | 4 +- .../hermes_vision/face_detector_node.py | 15 ++- ros/hermes_vision/hermes_vision/people.py | 126 ++++++++++++++++++ ros/hermes_vision/hermes_vision/recognize.py | 100 ++++++++++++++ ros/hermes_vision/setup.py | 1 + ros/hermes_vision/test/test_recognize.py | 49 +++++++ 6 files changed, 293 insertions(+), 2 deletions(-) create mode 100644 ros/hermes_vision/hermes_vision/people.py create mode 100644 ros/hermes_vision/hermes_vision/recognize.py create mode 100644 ros/hermes_vision/test/test_recognize.py diff --git a/ros/hermes_vision/hermes_vision/detect.py b/ros/hermes_vision/hermes_vision/detect.py index c8db18b..af8c5aa 100644 --- a/ros/hermes_vision/hermes_vision/detect.py +++ b/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]: diff --git a/ros/hermes_vision/hermes_vision/face_detector_node.py b/ros/hermes_vision/hermes_vision/face_detector_node.py index 4572127..644d9a9 100644 --- a/ros/hermes_vision/hermes_vision/face_detector_node.py +++ b/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) diff --git a/ros/hermes_vision/hermes_vision/people.py b/ros/hermes_vision/hermes_vision/people.py new file mode 100644 index 0000000..4fc686c --- /dev/null +++ b/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()) diff --git a/ros/hermes_vision/hermes_vision/recognize.py b/ros/hermes_vision/hermes_vision/recognize.py new file mode 100644 index 0000000..8f9f684 --- /dev/null +++ b/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) diff --git a/ros/hermes_vision/setup.py b/ros/hermes_vision/setup.py index 7ed7d65..ad25a7f 100644 --- a/ros/hermes_vision/setup.py +++ b/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', ], }, ) diff --git a/ros/hermes_vision/test/test_recognize.py b/ros/hermes_vision/test/test_recognize.py new file mode 100644 index 0000000..afa2d63 --- /dev/null +++ b/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')) == {}