diff --git a/CONTEXT.md b/CONTEXT.md index 5bb5a85..b09c8b6 100644 --- a/CONTEXT.md +++ b/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 - square size measured): collect ~20 views at different poses, solve with OpenCV, write the - camera_info YAML so `camera_ros` stops logging the missing-file error. V2's perception needs it. +1. **Camera calibration: tool built and tested, not yet run.** `python3 -m hermes_vision.calibrate + --cols 9 --rows 6 --square-mm ` (after `source ~/robot-env.sh`, robot running) collects 20 + 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`). diff --git a/README.md b/README.md index 28e7075..bb91052 100644 --- a/README.md +++ b/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 +``` + +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). diff --git a/docs/checkerboard-9x6.png b/docs/checkerboard-9x6.png new file mode 100644 index 0000000..56d3984 Binary files /dev/null and b/docs/checkerboard-9x6.png differ diff --git a/ros/hermes_vision/hermes_vision/calibrate.py b/ros/hermes_vision/hermes_vision/calibrate.py new file mode 100644 index 0000000..2d7420e --- /dev/null +++ b/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/_x.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()) diff --git a/ros/hermes_vision/setup.py b/ros/hermes_vision/setup.py index ad25a7f..7d03744 100644 --- a/ros/hermes_vision/setup.py +++ b/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', ], }, ) diff --git a/ros/hermes_vision/test/test_calibrate.py b/ros/hermes_vision/test/test_calibrate.py new file mode 100644 index 0000000..df0af9b --- /dev/null +++ b/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