Add a camera calibration tool and a checkerboard to show on a screen

hermes_vision.calibrate collects 20 varied checkerboard views from the camera topic
(rejecting repeats of a pose, which constrain nothing), solves with OpenCV and writes
the camera_info YAML camera_ros loads, named with the resolution as it expects.
Tested: the solver recovers a known synthetic camera (focal length within 1%), the
YAML round-trips, and real board detection works on a rendered board. Not yet run on
the real camera: it needs someone holding a board in front of it, with the focus set.

docs/checkerboard-9x6.png is a 10x7-square board that can be shown on a screen.

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

CONTEXT.md

@@ -264,9 +264,14 @@ unrelated photo scored 0.16. **Nobody is enrolled yet**; the owner enrols themse
 
 ## Next
 
-1. **Camera calibration** (needs the owner and a flat checkerboard, printed or on a screen, with the
+1. **Camera calibration: tool built and tested, not yet run.** `python3 -m hermes_vision.calibrate
-   square size measured): collect ~20 views at different poses, solve with OpenCV, write the
+   --cols 9 --rows 6 --square-mm <measured>` (after `source ~/robot-env.sh`, robot running) collects 20
-   camera_info YAML so `camera_ros` stops logging the missing-file error. V2's perception needs it.
+   varied views of a checkerboard (`docs/checkerboard-9x6.png` can be shown on a screen; measure a
+   square), solves with OpenCV and writes the camera_info YAML camera_ros looks for
+   (`~/.ros/camera_info/ov5647__base_soc_i2c0mux_i2c_1_ov5647_36_1296x972.yaml`: the name includes the
+   resolution). The solver recovers a known synthetic camera to within 1%. It needs the owner holding a
+   board in front of the camera, and the lens focus set first (the lens turns if not glued; earlier
+   close-range frames were blurry). V2's perception needs the result.
 2. Waiting on hardware: the servos (`hermes_head`, the pan/tilt URDF, then `head: true`), the speakers
    (playback, echo cancel, the `robot_mic` naming on the host, Hermes talking aloud), the fan (before
    any enclosure), the splitter, the printed enclosure (measure first, see `enclosure/README.md`).

README.md

@@ -130,6 +130,22 @@ give MCP images to the main model: it saves the photo to its image cache and the
 - **Run a node by hand:** `source ~/robot-env.sh` first: it puts the libcamera fork and the right
   OpenCV on the path, and keeps ROS local.
 
+## Calibrating the camera
+
+Needed once, and again if you change the lens focus or the image size. Set the focus first (the lens
+turns if it is not glued: focus at the distance you will use), then show `docs/checkerboard-9x6.png`
+on a screen (or print it) and measure one square with a ruler. With the robot running, on the Pi, after
+`source ~/robot-env.sh`:
+
+```
+python3 -m hermes_vision.calibrate --cols 9 --rows 6 --square-mm <measured>
+```
+
+Hold the camera 25 to 60 cm from the board and vary distance, tilt and position, keeping all of it in
+view. It collects 20 different views, solves with OpenCV and writes
+`~/.ros/camera_info/ov5647__base_soc_i2c0mux_i2c_1_ov5647_36_1296x972.yaml`. A reprojection error under
+about 0.5 px is good. `sudo systemctl restart robot` to load it.
+
 ## Tuning and troubleshooting
 
 - **Recordings are exactly silent:** the Audio switch is off (see above).

ros/hermes_vision/hermes_vision/calibrate.py

@@ -0,0 +1,140 @@
+"""Camera calibration from a checkerboard, writing the camera_info YAML that camera_ros loads.
+
+  python3 -m hermes_vision.calibrate --cols 9 --rows 6 --square-mm 25
+
+COLS x ROWS are the board's *inner corners* (a board of 10 x 7 squares has 9 x 6). With the robot
+running, hold the board in front of the camera at different distances, tilts and positions, keeping the
+whole board in view; it collects views that differ enough from those already taken, solves with OpenCV
+and writes ~/.ros/camera_info/<camera name>_<w>x<h>.yaml (restart the robot to load it). A good result
+has a reprojection error under about 0.5 px. Calibration is only valid for the focus it was taken at,
+so set the lens focus first, and for the image size in use.
+"""
+from __future__ import annotations
+
+import argparse
+import os
+import time
+
+import cv2
+import numpy as np
+
+DEFAULT_NAME = 'ov5647__base_soc_i2c0mux_i2c_1_ov5647_36'      # what camera_ros calls this camera
+
+
+def board_points(cols: int, rows: int, square_m: float) -> np.ndarray:
+    """The board's inner corners in its own frame (z = 0), row by row, in metres."""
+    p = np.zeros((rows * cols, 3), np.float32)
+    p[:, :2] = np.mgrid[0:cols, 0:rows].T.reshape(-1, 2) * square_m
+    return p
+
+
+def view_signature(corners: np.ndarray, size: tuple[int, int]) -> np.ndarray:
+    """A few numbers that describe where and how a board appears: centre, apparent size, and the
+    skew of its first row against its last (a tilt cue), all relative to the image."""
+    c = corners.reshape(-1, 2)
+    w, h = size
+    centre = c.mean(axis=0) / (w, h)
+    span = (c.max(axis=0) - c.min(axis=0)) / (w, h)
+    return np.array([centre[0], centre[1], span[0], span[1], span[0] / max(span[1], 1e-6)])
+
+
+def novel_enough(sig: np.ndarray, taken: list[np.ndarray], min_distance: float = 0.06) -> bool:
+    """True if this view differs enough from every view already taken (a spread of poses matters far
+    more than the number of views: twenty copies of one pose pin down nothing)."""
+    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)
+    return float(rms), k, d.reshape(-1)
+
+
+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
+    d = list(d[:5]) + [0.0] * (5 - len(d[:5]))
+    f = lambda xs: ', '.join(f'{x:.9g}' for x in xs)           # noqa: E731
+    proj = [k[0, 0], 0, k[0, 2], 0, 0, k[1, 1], k[1, 2], 0, 0, 0, 1, 0]
+    return (f'image_width: {w}\nimage_height: {h}\ncamera_name: {name}\n'
+            f'camera_matrix:\n  rows: 3\n  cols: 3\n  data: [{f(k.reshape(-1))}]\n'
+            f'distortion_model: plumb_bob\n'
+            f'distortion_coefficients:\n  rows: 1\n  cols: 5\n  data: [{f(d)}]\n'
+            f'rectification_matrix:\n  rows: 3\n  cols: 3\n  data: [1, 0, 0, 0, 1, 0, 0, 0, 1]\n'
+            f'projection_matrix:\n  rows: 3\n  cols: 4\n  data: [{f(proj)}]\n')
+
+
+def find_board(bgr: np.ndarray, cols: int, rows: int):
+    gray = cv2.cvtColor(bgr, cv2.COLOR_BGR2GRAY)
+    ok, corners = cv2.findChessboardCornersSB(gray, (cols, rows), cv2.CALIB_CB_EXHAUSTIVE | cv2.CALIB_CB_ACCURACY)
+    return (corners.astype(np.float32) if ok else None)
+
+
+def main() -> int:
+    p = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter)
+    p.add_argument('--cols', type=int, required=True, help='inner corners across')
+    p.add_argument('--rows', type=int, required=True, help='inner corners down')
+    p.add_argument('--square-mm', type=float, required=True, help='side of one square, measured')
+    p.add_argument('--views', type=int, default=20)
+    p.add_argument('--timeout', type=float, default=240)
+    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'))
+    a = p.parse_args()
+
+    import rclpy
+    from rclpy.node import Node
+    from sensor_msgs.msg import CompressedImage
+    rclpy.init()
+    node = Node('hermes_calibrate')
+    obj = board_points(a.cols, a.rows, a.square_mm / 1000.0)
+    taken_sigs, obj_pts, img_pts, state = [], [], [], {'size': None, 'last': 0.0, 'seen': 0}
+
+    def on_image(msg):
+        if len(img_pts) >= a.views or time.monotonic() - state['last'] < 0.6:
+            return
+        bgr = cv2.imdecode(np.frombuffer(msg.data, np.uint8), cv2.IMREAD_COLOR)
+        if bgr is None:
+            return
+        state['size'] = (bgr.shape[1], bgr.shape[0])
+        state['last'] = time.monotonic()
+        corners = find_board(bgr, a.cols, a.rows)
+        if corners is None:
+            state['seen'] += 1
+            if state['seen'] % 8 == 1:
+                print('  no board found: show all of it, flat and well lit, not too close', flush=True)
+            return
+        sig = view_signature(corners, state['size'])
+        if not novel_enough(sig, taken_sigs):
+            print('  board found, but too like one already taken: change distance, tilt or position', flush=True)
+            return
+        taken_sigs.append(sig)
+        obj_pts.append(obj)
+        img_pts.append(corners)
+        print(f'  view {len(img_pts)}/{a.views} taken', 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; '
+          f'vary distance, tilt and position. Waiting for {a.views} views...')
+    t0 = time.monotonic()
+    while len(img_pts) < a.views and time.monotonic() - t0 < a.timeout:
+        rclpy.spin_once(node, timeout_sec=0.2)
+    rclpy.shutdown()
+    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)
+    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]}')
+    print(f'wrote {path}\nrestart the robot to load it:  sudo systemctl restart robot')
+    return 0
+
+
+if __name__ == '__main__':
+    raise SystemExit(main())

ros/hermes_vision/setup.py

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

ros/hermes_vision/test/test_calibrate.py

@@ -0,0 +1,74 @@
+import cv2
+import numpy as np
+import yaml
+
+from hermes_vision.calibrate import board_points, camera_info_yaml, novel_enough, solve, view_signature
+
+SIZE = (1296, 972)
+K_TRUE = np.array([[1100.0, 0, 650.0], [0, 1090.0, 480.0], [0, 0, 1]])
+D_TRUE = np.array([-0.18, 0.05, 0.0005, -0.0003, 0.0])
+
+
+def synthetic_views(n=14, seed=3):
+    rng = np.random.default_rng(seed)
+    obj = board_points(9, 6, 0.025)
+    views = []
+    for _ in range(n):
+        rvec = rng.uniform(-0.45, 0.45, 3)
+        tvec = np.array([rng.uniform(-0.06, 0.02), rng.uniform(-0.05, 0.01), rng.uniform(0.28, 0.5)])
+        pts, _ = cv2.projectPoints(obj, rvec, tvec, K_TRUE, D_TRUE)
+        pts = pts.reshape(-1, 2)
+        if (pts[:, 0].min() > 20 and pts[:, 0].max() < SIZE[0] - 20 and pts[:, 1].min() > 20
+                and pts[:, 1].max() < SIZE[1] - 20):
+            views.append(pts.reshape(-1, 1, 2).astype(np.float32))
+    return obj, views
+
+
+def test_solver_recovers_the_true_camera():
+    obj, views = synthetic_views()
+    assert len(views) >= 8
+    rms, k, d = solve([obj] * len(views), views, SIZE)
+    assert rms < 0.05                                          # synthetic points are exact
+    assert abs(k[0, 0] - 1100) / 1100 < 0.01 and abs(k[1, 1] - 1090) / 1090 < 0.01
+    assert abs(k[0, 2] - 650) < 5 and abs(k[1, 2] - 480) < 5
+    assert abs(d[0] - D_TRUE[0]) < 0.03
+
+
+def test_the_yaml_is_the_camera_info_format_and_round_trips():
+    obj, views = synthetic_views()
+    _, k, d = solve([obj] * len(views), views, SIZE)
+    text = camera_info_yaml('cam_1296x972', SIZE, k, d)
+    y = yaml.safe_load(text)
+    assert (y['image_width'], y['image_height'], y['camera_name']) == (1296, 972, 'cam_1296x972')
+    assert y['distortion_model'] == 'plumb_bob' and len(y['distortion_coefficients']['data']) == 5
+    assert np.allclose(np.array(y['camera_matrix']['data']).reshape(3, 3), k, rtol=1e-6)
+    assert y['projection_matrix']['rows'] == 3 and y['projection_matrix']['cols'] == 4
+    assert y['rectification_matrix']['data'] == [1, 0, 0, 0, 1, 0, 0, 0, 1]
+
+
+def test_board_points_layout():
+    p = board_points(9, 6, 0.025)
+    assert p.shape == (54, 3) and np.allclose(p[1] - p[0], [0.025, 0, 0]) and np.allclose(p[9] - p[0], [0, 0.025, 0])
+
+
+def test_novelty_rejects_repeats_and_accepts_a_real_change():
+    _, views = synthetic_views()
+    sigs = [view_signature(v, SIZE) for v in views]
+    assert not novel_enough(sigs[0], [sigs[0]])                # the same pose again
+    far = [s for s in sigs[1:] if np.linalg.norm(s - sigs[0]) > 0.06]
+    assert far and novel_enough(far[0], [sigs[0]])
+    assert novel_enough(sigs[0], [])                            # the first view is always accepted
+
+
+def test_find_board_detects_a_real_rendered_checkerboard_and_not_a_blank_frame():
+    from hermes_vision.calibrate import find_board
+    sq, margin = 80, 80
+    img = np.full((7 * sq + 2 * margin, 10 * sq + 2 * margin), 255, np.uint8)
+    for r in range(7):
+        for c in range(10):
+            if (r + c) % 2 == 0:
+                img[margin + r * sq: margin + (r + 1) * sq, margin + c * sq: margin + (c + 1) * sq] = 0
+    bgr = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR)
+    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