kyleandClaude Sonnet 5.5
cf711060d0
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
94 lines
4.2 KiB
Python
94 lines
4.2 KiB
Python
"""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.
|
|
/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
|
|
import numpy as np
|
|
import rclpy
|
|
from rclpy.node import Node
|
|
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):
|
|
def __init__(self):
|
|
super().__init__('hermes_face_detector')
|
|
param = lambda name, default: self.declare_parameter(name, default).value # noqa: E731
|
|
topic = param('image_topic', '/camera/image_raw/compressed')
|
|
self.period = 1.0 / min(30.0, max(0.5, float(param('rate_hz', 4.0))))
|
|
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
|
|
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()
|
|
if now < self._next:
|
|
return
|
|
self._next = now + self.period
|
|
img = cv2.imdecode(np.frombuffer(msg.data, np.uint8), cv2.IMREAD_COLOR)
|
|
if img is None:
|
|
self.get_logger().warn('could not decode image', throttle_duration_sec=10.0)
|
|
return
|
|
faces = self.detector.detect(img)
|
|
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)
|
|
hyp = ObjectHypothesisWithPose()
|
|
hyp.hypothesis.class_id, hyp.hypothesis.score = 'face', f.score
|
|
d.results.append(hyp)
|
|
out.detections.append(d)
|
|
self.pub.publish(out)
|
|
if self.debug_pub:
|
|
for f in faces:
|
|
cv2.rectangle(img, (f.x, f.y), (f.x + f.w, f.y + f.h), (0, 255, 0), 2)
|
|
ok, jpg = cv2.imencode('.jpg', img, [cv2.IMWRITE_JPEG_QUALITY, 80])
|
|
if ok:
|
|
self.debug_pub.publish(CompressedImage(header=msg.header, format='jpeg', data=jpg.tobytes()))
|
|
|
|
|
|
def main(args=None):
|
|
rclpy.init(args=args)
|
|
node = FaceDetectorNode()
|
|
try:
|
|
rclpy.spin(node)
|
|
except KeyboardInterrupt:
|
|
pass
|
|
finally:
|
|
node.destroy_node()
|
|
if rclpy.ok():
|
|
rclpy.shutdown()
|
|
|
|
|
|
if __name__ == '__main__':
|
|
main()
|