Get the camera working with the Raspberry Pi libcamera fork; add face detection

Stock libcamera 0.2.0 (Ubuntu) and ROS Jazzy's 0.7.2 both detect the OV5647 but
crash in the Pi IPA when capturing, for every stream format and size. The
Raspberry Pi fork, built into /opt/libcamera-rpi, captures 1296x972 frames.
capture_still.py takes a still with it, and detect_faces.py runs OpenCV YuNet
(Haar fallback) on images. YuNet needs OpenCV 4.8+ and input sides in multiples
of 32, so the venv carries opencv-python-headless 4.10 pinned with numpy<2.

camera_ros still has to be built against the fork.

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

CONTEXT.md

@@ -38,7 +38,21 @@ printed enclosure, speakers. Work until those arrive is limited to what the Pi,
   rotates in software and leaves the scan direction fixed). Mounted pose (microSD end up, USB-C edge
   on the left) needs content turned 90 degrees clockwise: `rotation=270`. Full-frame push is
   ~23 fps through the driver; the face pushes raw RGB565 with `_block` and reaches ~27-28 fps.
-- Not done: audio (`wm8960-soundcard` overlay + mixer), camera (`cam -l`, `camera_ros`), PWM
+- **Camera: works with the Raspberry Pi libcamera fork, not with the stock stacks.** The OV5647 is
+  detected (`ov5647 10-0036`) and Ubuntu's libcamera 0.2.0 lists it once `libcamera-ipa` is
+  installed, but capture dies in the Pi IPA (`assertion "it != buffers_.end()" in prepareIsp()`),
+  and ROS Jazzy's libcamera 0.7.2 dies with `FATAL … A list of V4L2 controls requires a
+  ControlInfoMap` right after `configure`. Both are deterministic whatever the stream format or
+  size. Fix: build the fork (`github.com/raspberrypi/libcamera`, commit `6c1dd9d`, 2026-08-17) with
+  `-Dpipelines=rpi/vc4 -Dipas=rpi/vc4 -Dcam=enabled -Dv4l2=false …` into `/opt/libcamera-rpi`; its
+  `cam` then captures 1296x972 RGB888 at ~16 fps (dim room). `pi/capture_still.py` takes a still
+  with it. **`camera_ros` still has to be built against this fork** (the apt one uses ROS's broken
+  libcamera). Frames are padded (stride 3936) and B,G,R in memory.
+- Face detection: `pi/detect_faces.py` (OpenCV YuNet, Haar fallback). YuNet 2023mar needs OpenCV
+  4.8+, so `~/venv` has `opencv-python-headless==4.10.0.84` with `numpy<2` (the system NumPy stays
+  1.26.4 for ROS); YuNet also needs input sides in multiples of 32. Verified on a sample image.
+  First camera shot was nearly black (mean 5/255): the lens was facing something close and dim.
+- Not done: audio (`wm8960-soundcard` overlay + mixer; `alsa-utils` is installed, no card yet), PWM
   (waits for servos). Ask before rebooting the Pi.
 
 ## Package 0b (USB link): Pi side and talos side working

pi/capture_still.py

@@ -0,0 +1,43 @@
+#!/usr/bin/env python3
+"""Capture one still from the camera with the Raspberry Pi libcamera fork, as a JPEG.
+
+  capture_still.py OUT.jpg [--frames N] [--width W --height H]
+
+Runs `cam` from /opt/libcamera-rpi for N frames (so auto-exposure settles) and keeps the last.
+Run it with the venv's python so OpenCV is the newer one: ~/venv/bin/python capture_still.py ...
+"""
+import argparse
+import glob
+import os
+import subprocess
+import tempfile
+
+import cv2
+import numpy as np
+
+PREFIX = '/opt/libcamera-rpi'
+
+
+def main():
+    p = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter)
+    p.add_argument('out')
+    p.add_argument('--frames', type=int, default=45)
+    p.add_argument('--width', type=int, default=1296)
+    p.add_argument('--height', type=int, default=972)
+    a = p.parse_args()
+    env = dict(os.environ, LD_LIBRARY_PATH=f'{PREFIX}/lib/aarch64-linux-gnu')
+    with tempfile.TemporaryDirectory() as tmp:
+        subprocess.run([f'{PREFIX}/bin/cam', '-c', '1', '--stream',
+                        f'pixelformat=RGB888,width={a.width},height={a.height}',
+                        f'--capture={a.frames}', f'--file={tmp}/f-#.bin'],
+                       env=env, check=True, stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL, timeout=90)
+        last = sorted(glob.glob(f'{tmp}/f-*.bin'))[-1]
+        raw = np.fromfile(last, np.uint8)
+    stride = raw.size // a.height              # rows are padded
+    img = raw.reshape(a.height, stride)[:, :a.width * 3].reshape(a.height, a.width, 3)   # B,G,R in memory
+    cv2.imwrite(a.out, img)
+    print(f'{a.out}: {a.width}x{a.height}, mean brightness {img.mean():.1f}/255')
+
+
+if __name__ == '__main__':
+    main()

pi/detect_faces.py

@@ -0,0 +1,101 @@
+#!/usr/bin/env python3
+"""Detect faces in images and write annotated copies.
+
+  detect_faces.py IMAGE [IMAGE ...] [--out DIR] [--min-size PX]
+
+Uses OpenCV's YuNet detector when its model loads (~/models/face_detection_yunet_2023mar.onnx),
+otherwise the Haar cascades shipped with opencv-data (frontal, plus profile in both directions).
+Prints one line per image: the number of faces, then x,y,w,h boxes in pixels.
+"""
+import argparse
+import os
+
+import cv2
+import numpy as np
+
+HAAR = '/usr/share/opencv4/haarcascades'
+YUNET = os.path.expanduser('~/models/face_detection_yunet_2023mar.onnx')
+
+
+def yunet_detector(w, h):
+    if not os.path.exists(YUNET):
+        return None
+    try:
+        cv2.FaceDetectorYN.create(YUNET, '', (640, 480), 0.6, 0.3, 5000)
+    except cv2.error:
+        return None
+    by_size = {}
+
+    def run(img):
+        # This OpenCV's YuNet needs input sides that are multiples of 32: work at about 640 wide.
+        h0, w0 = img.shape[:2]
+        scale = 640 / max(w0, h0) if max(w0, h0) > 640 else 1.0
+        tw, th = max(32, round(w0 * scale / 32) * 32), max(32, round(h0 * scale / 32) * 32)
+        small = cv2.resize(img, (tw, th), interpolation=cv2.INTER_AREA)
+        if (tw, th) not in by_size:   # OpenCV 4.6 breaks if the input size changes after create()
+            by_size[(tw, th)] = cv2.FaceDetectorYN.create(YUNET, '', (tw, th), 0.6, 0.3, 5000)
+        _, faces = by_size[(tw, th)].detect(small)
+        if faces is None:
+            return []
+        fx, fy = w0 / tw, h0 / th
+        return [(int(f[0] * fx), int(f[1] * fy), int(f[2] * fx), int(f[3] * fy)) for f in faces]
+    return run
+
+
+def haar_detector(min_size):
+    frontal = cv2.CascadeClassifier(os.path.join(HAAR, 'haarcascade_frontalface_alt2.xml'))
+    profile = cv2.CascadeClassifier(os.path.join(HAAR, 'haarcascade_profileface.xml'))
+
+    def run(img):
+        gray = cv2.equalizeHist(cv2.cvtColor(img, cv2.COLOR_BGR2GRAY))
+        ms = (min_size, min_size)
+        boxes = [tuple(b) for b in frontal.detectMultiScale(gray, 1.1, 5, minSize=ms)]
+        for flip in (False, True):          # the profile cascade only sees one side
+            g = cv2.flip(gray, 1) if flip else gray
+            for x, y, w, h in profile.detectMultiScale(g, 1.1, 5, minSize=ms):
+                boxes.append((gray.shape[1] - x - w if flip else x, y, w, h))
+        return merge(boxes)
+    return run
+
+
+def merge(boxes, overlap=0.4):
+    """Drop boxes that mostly overlap an earlier one (frontal and profile can both fire)."""
+    kept = []
+    for x, y, w, h in boxes:
+        for kx, ky, kw, kh in kept:
+            iw = min(x + w, kx + kw) - max(x, kx)
+            ih = min(y + h, ky + kh) - max(y, ky)
+            if iw > 0 and ih > 0 and iw * ih > overlap * min(w * h, kw * kh):
+                break
+        else:
+            kept.append((int(x), int(y), int(w), int(h)))
+    return kept
+
+
+def main():
+    p = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter)
+    p.add_argument('images', nargs='+')
+    p.add_argument('--out', default='.')
+    p.add_argument('--min-size', type=int, default=40)
+    args = p.parse_args()
+    os.makedirs(args.out, exist_ok=True)
+    detector, name = None, None
+    for path in args.images:
+        img = cv2.imread(path)
+        if img is None:
+            print(f'{path}: unreadable')
+            continue
+        if detector is None:
+            detector = yunet_detector(img.shape[1], img.shape[0])
+            name = 'yunet' if detector else 'haar'
+            detector = detector or haar_detector(args.min_size)
+        boxes = detector(img)
+        for x, y, w, h in boxes:
+            cv2.rectangle(img, (x, y), (x + w, y + h), (0, 255, 0), 2)
+        dest = os.path.join(args.out, 'faces-' + os.path.basename(path).rsplit('.', 1)[0] + '.jpg')
+        cv2.imwrite(dest, img)
+        print(f'{path}: {len(boxes)} face(s) [{name}] {boxes} -> {dest}')
+
+
+if __name__ == '__main__':
+    main()