diff --git a/scripts/calibrate-camera-room.py b/scripts/calibrate-camera-room.py new file mode 100644 index 00000000..df7e8610 --- /dev/null +++ b/scripts/calibrate-camera-room.py @@ -0,0 +1,300 @@ +#!/usr/bin/env python3 +"""Two-checkerboard camera-room calibration for WiFi pose training (ADR-152 S2.1.3). + +Aligns the ADR-079 ground-truth camera and the ESP32 WiFi transceivers in +one shared 3D room frame -- the PerceptAlign (arXiv 2601.12252) defense +against "coordinate overfitting", where CSI-to-camera-coordinate regression +memorizes the deployment layout and collapses cross-layout. + +Procedure (<5 minutes): + 1. Print a checkerboard (default 9x6 inner corners, 25 mm squares). + 2. Tape one board flat on the ORIGIN WALL, tape-measure its top-left inner + corner position in room coordinates (+x along wall, +y into room, +z up). + 3. Lay the second board flat on the FLOOR, measure its near-left inner corner. + 4. With the collection camera in its final position, photograph each board. + 5. Run this script; tape-measure each ESP32 node position when prompted + (or pass --geometry nodes.json). + +Output: a calibration bundle JSON consumed by + scripts/collect-ground-truth.py --calibration + +Usage: + python scripts/calibrate-camera-room.py \\ + --wall-image photos/wall.jpg --wall-origin 0.50,0.0,1.60 \\ + --floor-image photos/floor.jpg --floor-origin 1.00,1.00,0.0 \\ + --calib-images "photos/intrinsics/*.jpg" \\ + --geometry config/transceivers.json \\ + --output data/calibration/camera-room.json +""" + +from __future__ import annotations + +import argparse +import glob +import json +import sys +from datetime import datetime +from pathlib import Path + +import cv2 +import numpy as np + +sys.path.insert(0, str(Path(__file__).resolve().parent)) +import calibration_lib as cal # noqa: E402 + +INTRINSICS_CACHE = Path("data") / ".cache" / "camera_intrinsics.json" + + +def parse_vec3(text: str) -> np.ndarray: + parts = [float(p) for p in text.replace(",", " ").split()] + if len(parts) != 3: + raise argparse.ArgumentTypeError(f"Expected 3 comma-separated numbers, got {text!r}") + return np.array(parts, dtype=np.float64) + + +def detect_corners(image_path: Path, cols: int, rows: int) -> tuple[np.ndarray, tuple[int, int]]: + image = cv2.imread(str(image_path)) + if image is None: + print(f"ERROR: Cannot read image {image_path}", file=sys.stderr) + sys.exit(1) + corners = cal.find_board_corners(image, cols, rows) + if corners is None: + print( + f"ERROR: No {cols}x{rows} checkerboard found in {image_path}. " + "Check lighting, focus, and the --board-cols/--board-rows flags.", + file=sys.stderr, + ) + sys.exit(1) + h, w = image.shape[:2] + return corners, (w, h) + + +def resolve_intrinsics(args, repo_root: Path, board_args: tuple[int, int, float]) -> dict: + """Pre-computed file > cached > computed from --calib-images > + last-resort 2-view estimate from the wall+floor photos themselves.""" + cols, rows, square_m = board_args + + if args.intrinsics: + print(f"Intrinsics: loading {args.intrinsics}") + return cal.load_intrinsics(Path(args.intrinsics)) + + cache_path = repo_root / INTRINSICS_CACHE + if cache_path.exists() and not args.recalibrate_intrinsics: + print(f"Intrinsics: using cached {cache_path} (pass --recalibrate-intrinsics to redo)") + intr = cal.load_intrinsics(cache_path) + intr["source"] = "cached" + return intr + + if args.calib_images: + paths = sorted(glob.glob(args.calib_images)) + if len(paths) < 3: + print( + f"ERROR: --calib-images matched only {len(paths)} file(s); " + "need >= 3 checkerboard views for stable intrinsics.", + file=sys.stderr, + ) + sys.exit(1) + corner_sets, image_size = [], None + for p in paths: + corners, size = detect_corners(Path(p), cols, rows) + if image_size is None: + image_size = size + elif size != image_size: + print(f"ERROR: {p} has size {size}, expected {image_size}.", file=sys.stderr) + sys.exit(1) + corner_sets.append(corners) + print(f" corners found: {p}") + intr = cal.compute_intrinsics(corner_sets, image_size, cols, rows, square_m) + print(f"Intrinsics: computed from {len(paths)} views, " + f"reprojection RMS {intr['reprojection_error_px']:.3f} px") + cal.save_bundle(intr, cache_path) # plain JSON write; reused on next run + print(f" cached to {cache_path}") + return intr + + # Last resort: 2-view calibration from the extrinsic photos. Workable but + # weak -- warn loudly and recommend a proper multi-view pass. + print( + "WARNING: no --intrinsics / cache / --calib-images; estimating intrinsics " + "from the wall+floor photos alone (2 views, low quality). Prefer " + "--calib-images with 5-10 varied board views.", + file=sys.stderr, + ) + corner_sets, image_size = [], None + for p in (args.wall_image, args.floor_image): + corners, size = detect_corners(Path(p), cols, rows) + image_size = image_size or size + corner_sets.append(corners) + intr = cal.compute_intrinsics(corner_sets, image_size, cols, rows, square_m) + intr["source"] = "two-view-fallback" + return intr + + +def prompt_transceiver_geometry() -> dict: + """Tape-measure entry of ESP32 node positions in room coordinates.""" + print() + print("Transceiver geometry -- enter one node per line:") + print(" [yaw_deg] (meters, room frame; blank line to finish)") + print(" example: esp32-s3-a 0.10 2.40 1.10 180") + nodes = [] + while True: + try: + line = input("node> ").strip() + except EOFError: + break + if not line: + break + parts = line.split() + if len(parts) not in (4, 5): + print(" expected: [yaw_deg]", file=sys.stderr) + continue + try: + node = {"id": parts[0], "position_m": [float(parts[1]), float(parts[2]), float(parts[3])]} + if len(parts) == 5: + node["antenna_yaw_deg"] = float(parts[4]) + except ValueError: + print(" positions must be numeric", file=sys.stderr) + continue + nodes.append(node) + if not nodes: + print("WARNING: no transceiver nodes entered; bundle will carry empty geometry.", + file=sys.stderr) + return {"nodes": nodes, "units": "meters", "source": "tape-measure-prompt"} + + +def load_geometry_file(path: Path) -> dict: + with open(path, "r", encoding="utf-8") as f: + data = json.load(f) + nodes = data.get("nodes", data if isinstance(data, list) else None) + if nodes is None: + raise ValueError(f"{path}: expected {{'nodes': [...]}} or a top-level list") + for node in nodes: + if "id" not in node or "position_m" not in node: + raise ValueError(f"{path}: each node needs 'id' and 'position_m' [x,y,z]") + return {"nodes": nodes, "units": "meters", "source": "file"} + + +def main(): + parser = argparse.ArgumentParser( + description="Two-checkerboard camera-room calibration (ADR-152 S2.1.3 / ADR-079)." + ) + parser.add_argument("--wall-image", required=True, + help="Photo of the checkerboard on the origin wall") + parser.add_argument("--floor-image", required=True, + help="Photo of the checkerboard on the floor (camera NOT moved)") + parser.add_argument("--wall-origin", type=parse_vec3, default="0.5,0.0,1.6", + help="Room xyz (m) of the wall board's first inner corner " + "(default: 0.5,0.0,1.6)") + parser.add_argument("--floor-origin", type=parse_vec3, default="1.0,1.0,0.0", + help="Room xyz (m) of the floor board's first inner corner " + "(default: 1.0,1.0,0.0)") + parser.add_argument("--wall-axes", default="+x,-z", + help="Wall board column,row directions in room frame (default: +x,-z)") + parser.add_argument("--floor-axes", default="+x,+y", + help="Floor board column,row directions in room frame (default: +x,+y)") + parser.add_argument("--board-cols", type=int, default=cal.DEFAULT_BOARD_COLS, + help=f"Inner corners per row (default: {cal.DEFAULT_BOARD_COLS})") + parser.add_argument("--board-rows", type=int, default=cal.DEFAULT_BOARD_ROWS, + help=f"Inner corners per column (default: {cal.DEFAULT_BOARD_ROWS})") + parser.add_argument("--square-size-mm", type=float, default=cal.DEFAULT_SQUARE_SIZE_MM, + help=f"Checkerboard square size in mm (default: {cal.DEFAULT_SQUARE_SIZE_MM})") + parser.add_argument("--intrinsics", help="Pre-computed intrinsics JSON (skips computation)") + parser.add_argument("--calib-images", + help="Glob of >=3 checkerboard photos for intrinsics computation") + parser.add_argument("--recalibrate-intrinsics", action="store_true", + help="Ignore the cached intrinsics and recompute") + parser.add_argument("--geometry", + help="Transceiver geometry JSON ({nodes:[{id,position_m,[antenna_yaw_deg]}]}); " + "omit to be prompted for tape-measure entry") + parser.add_argument("--output", default=None, + help="Bundle output path (default: data/calibration/camera-room-.json)") + args = parser.parse_args() + + if isinstance(args.wall_origin, str): + args.wall_origin = parse_vec3(args.wall_origin) + if isinstance(args.floor_origin, str): + args.floor_origin = parse_vec3(args.floor_origin) + + repo_root = Path(__file__).resolve().parent.parent + cols, rows = args.board_cols, args.board_rows + square_m = args.square_size_mm / 1000.0 + + # --- Intrinsics --- + intrinsics = resolve_intrinsics(args, repo_root, (cols, rows, square_m)) + camera_matrix = np.asarray(intrinsics["camera_matrix"], dtype=np.float64) + dist_coeffs = np.asarray(intrinsics["dist_coeffs"], dtype=np.float64) + + # --- Corner detection on the two placed boards --- + wall_corners, wall_size = detect_corners(Path(args.wall_image), cols, rows) + floor_corners, floor_size = detect_corners(Path(args.floor_image), cols, rows) + if wall_size != floor_size: + print(f"ERROR: wall image {wall_size} and floor image {floor_size} differ in size; " + "both must come from the fixed collection camera.", file=sys.stderr) + sys.exit(1) + print(f"Corners detected: wall + floor boards ({cols}x{rows}, {args.square_size_mm} mm)") + + # Re-scale intrinsics if they were computed at a different resolution + # than the extrinsic photos (the bundle always stores K at wall_size). + intr_size = tuple(intrinsics["image_size"]) + if intr_size != wall_size: + sx, sy = wall_size[0] / intr_size[0], wall_size[1] / intr_size[1] + camera_matrix[0, 0] *= sx + camera_matrix[0, 2] *= sx + camera_matrix[1, 1] *= sy + camera_matrix[1, 2] *= sy + print(f" intrinsics scaled {intr_size} -> {wall_size}") + intrinsics = {**intrinsics, "camera_matrix": camera_matrix.tolist(), + "image_size": list(wall_size)} + + # --- Room-frame corner positions from the measured placements --- + wall_u, wall_v = (cal.parse_axis(t) for t in args.wall_axes.split(",")) + floor_u, floor_v = (cal.parse_axis(t) for t in args.floor_axes.split(",")) + wall_room = cal.board_room_points(cols, rows, square_m, args.wall_origin, wall_u, wall_v) + floor_room = cal.board_room_points(cols, rows, square_m, args.floor_origin, floor_u, floor_v) + + # --- Extrinsics: joint two-board solve (resolves per-board corner-order + # ambiguity -- a single planar board is centrosymmetric; the pair is not) --- + extrinsics = cal.solve_two_board_extrinsics( + wall_room, wall_corners, floor_room, floor_corners, camera_matrix, dist_coeffs + ) + wall_rmse = extrinsics["per_board"]["wall"]["rmse_px"] + floor_rmse = extrinsics["per_board"]["floor"]["rmse_px"] + print(f" joint solve: RMSE {extrinsics['rmse_px']:.3f} px " + f"(wall {wall_rmse:.3f} / floor {floor_rmse:.3f})") + print(f" camera at room {np.round(extrinsics['translation_m'], 3).tolist()} m") + if max(wall_rmse, floor_rmse) > 3.0: + print( + "WARNING: high per-board reprojection error -- re-check the measured " + "board origins/axes and that the camera did not move between photos.", + file=sys.stderr, + ) + + # --- Transceiver geometry --- + if args.geometry: + geometry = load_geometry_file(Path(args.geometry)) + print(f"Transceiver geometry: {len(geometry['nodes'])} node(s) from {args.geometry}") + else: + geometry = prompt_transceiver_geometry() + + # --- Bundle --- + bundle = cal.make_bundle( + camera_intrinsics=intrinsics, + camera_to_room_extrinsics=extrinsics, + checkerboard_spec={"cols": cols, "rows": rows, "square_size_mm": args.square_size_mm}, + transceiver_geometry=geometry, + ) + if args.output: + out_path = Path(args.output) + else: + ts = datetime.now().strftime("%Y%m%d_%H%M%S") + out_path = repo_root / "data" / "calibration" / f"camera-room-{ts}.json" + cal.save_bundle(bundle, out_path) + + print() + print("=== Calibration bundle written ===") + print(f" path: {out_path}") + print(f" calibration_id: {cal.calibration_id(bundle)}") + print(f" next: python scripts/collect-ground-truth.py --calibration {out_path}") + + +if __name__ == "__main__": + main() diff --git a/scripts/calibration_lib.py b/scripts/calibration_lib.py new file mode 100644 index 00000000..02b0c50c --- /dev/null +++ b/scripts/calibration_lib.py @@ -0,0 +1,416 @@ +#!/usr/bin/env python3 +"""Camera-room calibration library for WiFi pose ground truth (ADR-152 S2.1.3). + +Implements the PerceptAlign-style two-checkerboard alignment adopted in +ADR-152 S2.1.3 to defend the ADR-079 camera-supervised pipeline against +"coordinate overfitting" (arXiv 2601.12252, MobiCom'26): models regressing +CSI to raw camera-frame coordinates memorize the deployment layout and +collapse cross-layout. The fix is to express camera AND WiFi transceivers +in one shared 3D room frame, and stamp every training label with the +calibration + transceiver geometry that produced it. + +Used by: + scripts/calibrate-camera-room.py (produces the calibration bundle) + scripts/collect-ground-truth.py (consumes it via --calibration) + +Room frame convention (right-handed, meters): + origin = a designated wall/floor corner of the room + +x = along the origin wall + +y = into the room (away from the origin wall) + +z = up + +No-depth limitation (IMPORTANT): a single 2D camera keypoint constrains +only a *ray* in the room frame, not a 3D point. The transform helpers here +therefore return unit bearing rays from the camera center -- a projective +alignment. Consumers that need metric 3D points must supply a depth +assumption downstream (floor-plane intersection, known subject height, +multi-view triangulation, ...). Raw image coordinates are always preserved +alongside the room-frame rays so training can choose either representation. +""" + +from __future__ import annotations + +import hashlib +import json +from datetime import datetime, timezone +from pathlib import Path + +import cv2 +import numpy as np + +BUNDLE_SCHEMA_VERSION = 1 +BUNDLE_METHOD = "two-checkerboard" + +# Default checkerboard: 9x6 inner corners, 25 mm squares (a common print). +DEFAULT_BOARD_COLS = 9 +DEFAULT_BOARD_ROWS = 6 +DEFAULT_SQUARE_SIZE_MM = 25.0 + +_AXIS_TOKENS = { + "+x": (1.0, 0.0, 0.0), "-x": (-1.0, 0.0, 0.0), + "+y": (0.0, 1.0, 0.0), "-y": (0.0, -1.0, 0.0), + "+z": (0.0, 0.0, 1.0), "-z": (0.0, 0.0, -1.0), +} + + +def parse_axis(token: str) -> np.ndarray: + """Parse an axis token like '+x' or '-z' into a room-frame unit vector.""" + key = token.strip().lower() + if key in _AXIS_TOKENS: + return np.array(_AXIS_TOKENS[key], dtype=np.float64) + raise ValueError(f"Invalid axis token {token!r}; expected one of {sorted(_AXIS_TOKENS)}") + + +# --------------------------------------------------------------------------- +# Checkerboard geometry +# --------------------------------------------------------------------------- + +def board_object_points(cols: int, rows: int, square_size_m: float) -> np.ndarray: + """Inner-corner positions in the board's own frame (z=0 plane), row-major. + + Matches the corner ordering of cv2.findChessboardCorners for a + (cols, rows) pattern: cols varies fastest. + """ + pts = np.zeros((rows * cols, 3), dtype=np.float64) + grid = np.mgrid[0:cols, 0:rows].T.reshape(-1, 2) # (rows*cols, 2), cols fastest + pts[:, :2] = grid * square_size_m + return pts + + +def board_room_points( + cols: int, + rows: int, + square_size_m: float, + origin: np.ndarray, + u_axis: np.ndarray, + v_axis: np.ndarray, +) -> np.ndarray: + """Inner-corner positions in ROOM coordinates for a board placed at a + known position: first corner at `origin`, columns stepping along + `u_axis`, rows stepping along `v_axis` (both room-frame unit vectors). + """ + local = board_object_points(cols, rows, square_size_m) + origin = np.asarray(origin, dtype=np.float64) + u = np.asarray(u_axis, dtype=np.float64) + v = np.asarray(v_axis, dtype=np.float64) + return origin[None, :] + local[:, 0:1] * u[None, :] + local[:, 1:2] * v[None, :] + + +def find_board_corners(image: np.ndarray, cols: int, rows: int) -> np.ndarray | None: + """Detect and sub-pixel-refine checkerboard inner corners. + + Returns (cols*rows, 2) float64 pixel coordinates, or None if not found. + """ + gray = image if image.ndim == 2 else cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) + flags = cv2.CALIB_CB_ADAPTIVE_THRESH | cv2.CALIB_CB_NORMALIZE_IMAGE + found, corners = cv2.findChessboardCorners(gray, (cols, rows), flags=flags) + if not found: + return None + criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 1e-3) + corners = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria) + return corners.reshape(-1, 2).astype(np.float64) + + +# --------------------------------------------------------------------------- +# Intrinsics +# --------------------------------------------------------------------------- + +def compute_intrinsics( + corner_sets: list[np.ndarray], + image_size: tuple[int, int], + cols: int, + rows: int, + square_size_m: float, +) -> dict: + """Camera intrinsics from N checkerboard views via cv2.calibrateCamera. + + corner_sets: list of (cols*rows, 2) pixel corner arrays. + image_size: (width, height) of the calibration images. + """ + obj = board_object_points(cols, rows, square_size_m).astype(np.float32) + obj_pts = [obj for _ in corner_sets] + img_pts = [c.reshape(-1, 1, 2).astype(np.float32) for c in corner_sets] + rms, camera_matrix, dist_coeffs, _, _ = cv2.calibrateCamera( + obj_pts, img_pts, tuple(image_size), None, None + ) + return { + "image_size": [int(image_size[0]), int(image_size[1])], + "camera_matrix": camera_matrix.tolist(), + "dist_coeffs": dist_coeffs.ravel().tolist(), + "reprojection_error_px": float(rms), + "source": "computed", + } + + +def load_intrinsics(path: Path) -> dict: + """Load a pre-computed intrinsics JSON ({camera_matrix, dist_coeffs, image_size}).""" + with open(path, "r", encoding="utf-8") as f: + data = json.load(f) + # Accept either a bare intrinsics dict or a full calibration bundle. + intr = data.get("camera_intrinsics", data) + for key in ("camera_matrix", "dist_coeffs", "image_size"): + if key not in intr: + raise ValueError(f"Intrinsics file {path} missing key {key!r}") + intr = dict(intr) + intr["source"] = "file" + return intr + + +# --------------------------------------------------------------------------- +# Extrinsics (camera -> room rigid transform) +# --------------------------------------------------------------------------- + +def reprojection_rmse( + room_points: np.ndarray, + image_points: np.ndarray, + rvec: np.ndarray, + tvec: np.ndarray, + camera_matrix: np.ndarray, + dist_coeffs: np.ndarray, +) -> float: + proj, _ = cv2.projectPoints(room_points, rvec, tvec, camera_matrix, dist_coeffs) + err = proj.reshape(-1, 2) - image_points.reshape(-1, 2) + return float(np.sqrt(np.mean(np.sum(err**2, axis=1)))) + + +def _solve_pnp( + room_points: np.ndarray, + image_points: np.ndarray, + camera_matrix: np.ndarray, + dist_coeffs: np.ndarray, +) -> dict | None: + """One solvePnP run (room->camera), inverted to camera->room. Returns + {rotation (3x3 camera->room), translation_m (camera center in room + frame), rmse_px} or None on failure. + """ + ok, rvec, tvec = cv2.solvePnP( + room_points.reshape(-1, 1, 3), + image_points.reshape(-1, 1, 2), + camera_matrix, + dist_coeffs, + flags=cv2.SOLVEPNP_ITERATIVE, + ) + if not ok: + return None + rmse = reprojection_rmse(room_points, image_points, rvec, tvec, camera_matrix, dist_coeffs) + r_room_to_cam, _ = cv2.Rodrigues(rvec) + r_cam_to_room = r_room_to_cam.T + camera_center_room = (-r_cam_to_room @ tvec).ravel() + return { + "rotation": r_cam_to_room.tolist(), + "translation_m": camera_center_room.tolist(), + "rmse_px": rmse, + } + + +def solve_extrinsics( + room_points: np.ndarray, + image_points: np.ndarray, + camera_matrix: np.ndarray, + dist_coeffs: np.ndarray, +) -> dict: + """Solve the camera->room rigid transform from 3D room-frame points and + their 2D pixel observations. + + NOTE: the corner grid of a single planar checkerboard is centrosymmetric, + so the corner ordering returned by findChessboardCorners (which may + enumerate from either board end) cannot be disambiguated from one board + alone -- the reversed ordering fits a ghost pose with identical + reprojection error. Use solve_two_board_extrinsics for the full + two-checkerboard procedure, where the joint point set breaks the symmetry. + """ + ext = _solve_pnp(room_points, image_points, camera_matrix, dist_coeffs) + if ext is None: + raise RuntimeError("solvePnP failed") + return ext + + +def solve_two_board_extrinsics( + wall_room: np.ndarray, + wall_image: np.ndarray, + floor_room: np.ndarray, + floor_image: np.ndarray, + camera_matrix: np.ndarray, + dist_coeffs: np.ndarray, +) -> dict: + """Joint camera->room solve over both checkerboards (the ADR-152 S2.1.3 + two-checkerboard method). + + Tries all 4 per-board corner-ordering combinations: each board's ordering + is individually ambiguous (centrosymmetric grid), but the combined + wall+floor point set is not, so exactly one combination reaches minimal + reprojection error. Returns the solve_extrinsics dict plus + {wall_flipped, floor_flipped, per_board: {wall|floor: {rmse_px}}}. + """ + best = None + for wall_flipped in (False, True): + for floor_flipped in (False, True): + wi = wall_image[::-1].copy() if wall_flipped else wall_image + fi = floor_image[::-1].copy() if floor_flipped else floor_image + room = np.concatenate([wall_room, floor_room], axis=0) + img = np.concatenate([wi, fi], axis=0) + ext = _solve_pnp(room, img, camera_matrix, dist_coeffs) + if ext is None: + continue + if best is None or ext["rmse_px"] < best[0]["rmse_px"]: + ext["wall_flipped"] = wall_flipped + ext["floor_flipped"] = floor_flipped + rvec, _ = cv2.Rodrigues(np.asarray(ext["rotation"]).T) + tvec = -np.asarray(ext["rotation"]).T @ np.asarray(ext["translation_m"]) + ext["per_board"] = { + "wall": {"rmse_px": reprojection_rmse( + wall_room, wi, rvec, tvec, camera_matrix, dist_coeffs)}, + "floor": {"rmse_px": reprojection_rmse( + floor_room, fi, rvec, tvec, camera_matrix, dist_coeffs)}, + } + best = (ext,) + if best is None: + raise RuntimeError("solvePnP failed for all corner-ordering combinations") + return best[0] + + +def extrinsics_consistency(ext_a: dict, ext_b: dict) -> dict: + """Angular + translational disagreement between two extrinsic solutions + (the two single-board solves). Large values mean a mis-entered board + placement or a bad corner detection. + """ + ra = np.asarray(ext_a["rotation"]) + rb = np.asarray(ext_b["rotation"]) + r_delta = ra.T @ rb + angle = float(np.degrees(np.arccos(np.clip((np.trace(r_delta) - 1.0) / 2.0, -1.0, 1.0)))) + t_delta = float( + np.linalg.norm(np.asarray(ext_a["translation_m"]) - np.asarray(ext_b["translation_m"])) + ) + return {"rotation_deg": angle, "translation_m": t_delta} + + +# --------------------------------------------------------------------------- +# Calibration bundle (the artifact written to disk) +# --------------------------------------------------------------------------- + +def make_bundle( + camera_intrinsics: dict, + camera_to_room_extrinsics: dict, + checkerboard_spec: dict, + transceiver_geometry: dict, +) -> dict: + return { + "schema_version": BUNDLE_SCHEMA_VERSION, + "method": BUNDLE_METHOD, + "calibrated_at": datetime.now(timezone.utc).isoformat(), + "room_frame": { + "description": "right-handed; origin at wall/floor corner; " + "+x along origin wall, +y into room, +z up", + "units": "meters", + }, + "checkerboard_spec": checkerboard_spec, + "camera_intrinsics": camera_intrinsics, + "camera_to_room_extrinsics": camera_to_room_extrinsics, + "transceiver_geometry": transceiver_geometry, + } + + +def calibration_id(bundle: dict) -> str: + """Stable content hash of a bundle -- stamped onto every emitted sample + so a label can always be traced to the exact calibration that framed it. + """ + canonical = json.dumps(bundle, sort_keys=True, separators=(",", ":")) + return "sha256:" + hashlib.sha256(canonical.encode("utf-8")).hexdigest() + + +def save_bundle(bundle: dict, path: Path) -> None: + path = Path(path) + path.parent.mkdir(parents=True, exist_ok=True) + with open(path, "w", encoding="utf-8") as f: + json.dump(bundle, f, indent=2) + f.write("\n") + + +def load_bundle(path: Path) -> dict: + with open(path, "r", encoding="utf-8") as f: + bundle = json.load(f) + for key in ("camera_intrinsics", "camera_to_room_extrinsics", "transceiver_geometry"): + if key not in bundle: + raise ValueError(f"Calibration bundle {path} missing key {key!r}") + return bundle + + +# --------------------------------------------------------------------------- +# Keypoint transform (image -> room-frame bearing rays) +# --------------------------------------------------------------------------- + +class CalibrationContext: + """Pre-computed transform state for a collection session. + + Scales the bundle's intrinsics to the live capture resolution (MediaPipe + keypoints are normalized [0,1], so we need the actual frame size to get + back to pixels before undistorting). + """ + + def __init__(self, bundle: dict, frame_w: int, frame_h: int): + self.bundle = bundle + self.calibration_id = calibration_id(bundle) + self.transceiver_geometry = bundle["transceiver_geometry"] + self.frame_w = int(frame_w) + self.frame_h = int(frame_h) + + intr = bundle["camera_intrinsics"] + k = np.asarray(intr["camera_matrix"], dtype=np.float64) + cal_w, cal_h = intr["image_size"] + sx = self.frame_w / float(cal_w) + sy = self.frame_h / float(cal_h) + k = k.copy() + k[0, 0] *= sx + k[0, 2] *= sx + k[1, 1] *= sy + k[1, 2] *= sy + self.camera_matrix = k + self.dist_coeffs = np.asarray(intr["dist_coeffs"], dtype=np.float64) + + ext = bundle["camera_to_room_extrinsics"] + self.r_cam_to_room = np.asarray(ext["rotation"], dtype=np.float64) + self.origin_room = np.asarray(ext["translation_m"], dtype=np.float64) + + def transform_keypoints(self, keypoints_norm: list[list[float]]) -> tuple[np.ndarray, np.ndarray]: + """Normalized [0,1] image keypoints -> unit bearing rays in the room + frame, anchored at the camera center. + + Projective alignment ONLY (no depth): each returned ray is the locus + of room positions consistent with the 2D observation. Returns + (camera_origin_room (3,), ray_dirs (N, 3) unit vectors). + """ + pts = np.asarray(keypoints_norm, dtype=np.float64) + pts_px = pts * np.array([self.frame_w, self.frame_h], dtype=np.float64) + undist = cv2.undistortPoints( + pts_px.reshape(-1, 1, 2), self.camera_matrix, self.dist_coeffs + ).reshape(-1, 2) + rays_cam = np.concatenate([undist, np.ones((len(undist), 1))], axis=1) + rays_cam /= np.linalg.norm(rays_cam, axis=1, keepdims=True) + rays_room = (self.r_cam_to_room @ rays_cam.T).T + return self.origin_room, rays_room + + +def load_calibration_context(path: Path, frame_w: int, frame_h: int) -> CalibrationContext: + return CalibrationContext(load_bundle(path), frame_w, frame_h) + + +def augment_record(record: dict, ctx: CalibrationContext | None) -> dict: + """Stamp a ground-truth record with room-frame rays + calibration metadata. + + With ctx=None this is the identity -- the record (and hence the emitted + JSONL line) is byte-identical to the pre-calibration ADR-079 format. + Raw image-coordinate keypoints are kept untouched in both cases; the + room-frame representation is ADDED, never substituted, so training can + choose either (ADR-152 S2.1.3). + """ + if ctx is None: + return record + if record.get("keypoints"): + _, rays = ctx.transform_keypoints(record["keypoints"]) + record["keypoints_room"] = [[round(float(v), 5) for v in ray] for ray in rays] + else: + record["keypoints_room"] = [] + record["camera_origin_room"] = [round(float(v), 5) for v in ctx.origin_room] + record["calibration_id"] = ctx.calibration_id + record["transceiver_geometry"] = ctx.transceiver_geometry + return record diff --git a/scripts/collect-ground-truth.py b/scripts/collect-ground-truth.py index 65fafe6d..fc9808d1 100644 --- a/scripts/collect-ground-truth.py +++ b/scripts/collect-ground-truth.py @@ -6,9 +6,19 @@ synchronizes with ESP32 CSI recording from the sensing server. Output: JSONL file in data/ground-truth/ with per-frame 17-keypoint COCO poses. +With --calibration (produced by scripts/calibrate-camera-room.py, +ADR-152 S2.1.3), every record is additionally stamped with room-frame bearing +rays for each keypoint, the calibration_id, and the transceiver geometry -- +the PerceptAlign-style defense against coordinate overfitting. Raw image +coordinates are always kept; without depth the room-frame representation is +a projective alignment (rays, not 3D points) -- see scripts/calibration_lib.py. +Without --calibration the output is byte-identical to the original ADR-079 +format. + Usage: python scripts/collect-ground-truth.py --preview --duration 60 python scripts/collect-ground-truth.py --server http://192.168.1.10:3000 + python scripts/collect-ground-truth.py --calibration data/calibration/camera-room.json """ from __future__ import annotations @@ -168,8 +178,23 @@ def main(): default="data/ground-truth", help="Output directory (default: data/ground-truth)", ) + parser.add_argument( + "--calibration", + default=None, + help="Camera-room calibration bundle JSON from scripts/calibrate-camera-room.py " + "(ADR-152 S2.1.3); adds room-frame keypoint rays + transceiver geometry " + "to every record", + ) args = parser.parse_args() + if not args.calibration: + print( + "WARNING: no --calibration bundle; labels stay in raw camera coordinates " + "and are layout-brittle (coordinate overfitting, ADR-152 S2.1.3) -- run " + "scripts/calibrate-camera-room.py first.", + file=sys.stderr, + ) + # --- Resolve paths relative to repo root --- repo_root = Path(__file__).resolve().parent.parent output_dir = repo_root / args.output @@ -193,6 +218,25 @@ def main(): frame_h = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT)) print(f"Camera opened: {frame_w}x{frame_h}") + # --- Load calibration bundle (ADR-152 S2.1.3) --- + calib_ctx = None + if args.calibration: + # Lazy import keeps the no-calibration path identical to the original. + sys.path.insert(0, str(Path(__file__).resolve().parent)) + import calibration_lib + + try: + calib_ctx = calibration_lib.load_calibration_context( + Path(args.calibration), frame_w, frame_h + ) + except (OSError, ValueError, json.JSONDecodeError) as exc: + print(f"ERROR: Cannot load calibration bundle {args.calibration}: {exc}", + file=sys.stderr) + sys.exit(1) + n_nodes = len(calib_ctx.transceiver_geometry.get("nodes", [])) + print(f"Calibration: {calib_ctx.calibration_id[:23]}... " + f"({n_nodes} transceiver node(s)); emitting room-frame keypoint rays") + # --- Create PoseLandmarker --- options = PoseLandmarkerOptions( base_options=BaseOptions(model_asset_path=str(model_path)), @@ -287,6 +331,10 @@ def main(): "n_visible": n_visible, "n_persons": n_persons, } + if calib_ctx is not None: + # Adds keypoints_room (bearing rays), camera_origin_room, + # calibration_id, transceiver_geometry (ADR-152 S2.1.3). + record = calibration_lib.augment_record(record, calib_ctx) out_file.write(json.dumps(record) + "\n") frame_count += 1 total_confidence += confidence diff --git a/scripts/tests/conftest.py b/scripts/tests/conftest.py new file mode 100644 index 00000000..7a9ef663 --- /dev/null +++ b/scripts/tests/conftest.py @@ -0,0 +1,8 @@ +"""Make scripts/ importable for the calibration tests (ADR-152 S2.1.3).""" + +import sys +from pathlib import Path + +SCRIPTS_DIR = Path(__file__).resolve().parents[1] +if str(SCRIPTS_DIR) not in sys.path: + sys.path.insert(0, str(SCRIPTS_DIR)) diff --git a/scripts/tests/test_calibration.py b/scripts/tests/test_calibration.py new file mode 100644 index 00000000..070f8995 --- /dev/null +++ b/scripts/tests/test_calibration.py @@ -0,0 +1,326 @@ +#!/usr/bin/env python3 +"""Headless tests for the camera-room calibration pipeline (ADR-152 S2.1.3). + +Covers calibration_lib.py end to end on synthetic data -- no camera, no +display, no MediaPipe: + * known extrinsics recovered from synthetic two-checkerboard corners + * calibration bundle JSON round-trip + stable content hash + * image->room keypoint transform correctness (rays pass through the + original 3D points -- the projective, no-depth alignment of ADR-079 + labels into the shared room frame) + * collect-ground-truth's no-calibration record path is byte-identical + (augment_record with ctx=None is the identity) + +Run: python -m pytest scripts/tests/ -q +""" + +from __future__ import annotations + +import json + +import cv2 +import numpy as np +import pytest + +import calibration_lib as cal + +# --------------------------------------------------------------------------- +# Synthetic scene fixtures +# --------------------------------------------------------------------------- + +IMG_W, IMG_H = 1280, 720 +K_GT = np.array( + [[800.0, 0.0, 640.0], + [0.0, 800.0, 360.0], + [0.0, 0.0, 1.0]] +) +DIST_ZERO = np.zeros(5) +DIST_MILD = np.array([-0.10, 0.02, 0.001, -0.001, 0.0]) + +BOARD_COLS, BOARD_ROWS = 9, 6 +SQUARE_M = 0.025 + + +def look_at_pose(camera_pos, target): + """Ground-truth camera pose: returns (R_cam_to_room, camera_center_room). + + Camera convention: +z forward (optical axis), +x right, +y down. + """ + c = np.asarray(camera_pos, dtype=np.float64) + fwd = np.asarray(target, dtype=np.float64) - c + fwd /= np.linalg.norm(fwd) + up_room = np.array([0.0, 0.0, 1.0]) + x_cam = np.cross(fwd, -up_room) + x_cam /= np.linalg.norm(x_cam) + y_cam = np.cross(fwd, x_cam) + r_cam_to_room = np.stack([x_cam, y_cam, fwd], axis=1) # columns = camera axes in room + return r_cam_to_room, c + + +def room_to_cam(r_cam_to_room, center): + """Invert to the solvePnP (room->camera) convention: rvec, tvec.""" + r_room_to_cam = r_cam_to_room.T + tvec = -r_room_to_cam @ center + rvec, _ = cv2.Rodrigues(r_room_to_cam) + return rvec, tvec.reshape(3, 1) + + +def project_room_points(points_room, r_cam_to_room, center, k=K_GT, dist=DIST_ZERO): + rvec, tvec = room_to_cam(r_cam_to_room, center) + proj, _ = cv2.projectPoints(np.asarray(points_room, dtype=np.float64), rvec, tvec, k, dist) + return proj.reshape(-1, 2) + + +@pytest.fixture +def scene(): + """A camera in the room looking at the wall + floor checkerboards.""" + r_gt, c_gt = look_at_pose(camera_pos=[1.5, 3.0, 1.3], target=[1.0, 0.5, 0.8]) + wall_room = cal.board_room_points( + BOARD_COLS, BOARD_ROWS, SQUARE_M, + origin=[0.5, 0.0, 1.6], u_axis=cal.parse_axis("+x"), v_axis=cal.parse_axis("-z"), + ) + floor_room = cal.board_room_points( + BOARD_COLS, BOARD_ROWS, SQUARE_M, + origin=[1.0, 1.0, 0.0], u_axis=cal.parse_axis("+x"), v_axis=cal.parse_axis("+y"), + ) + return r_gt, c_gt, wall_room, floor_room + + +def make_bundle(r_gt, c_gt, dist=DIST_ZERO): + return cal.make_bundle( + camera_intrinsics={ + "image_size": [IMG_W, IMG_H], + "camera_matrix": K_GT.tolist(), + "dist_coeffs": dist.tolist(), + "reprojection_error_px": 0.0, + "source": "synthetic", + }, + camera_to_room_extrinsics={ + "rotation": r_gt.tolist(), + "translation_m": c_gt.tolist(), + "rmse_px": 0.0, + }, + checkerboard_spec={"cols": BOARD_COLS, "rows": BOARD_ROWS, "square_size_mm": 25.0}, + transceiver_geometry={ + "nodes": [ + {"id": "esp32-s3-a", "position_m": [0.1, 2.4, 1.1], "antenna_yaw_deg": 180.0}, + {"id": "esp32-c6-b", "position_m": [3.2, 0.3, 0.9]}, + ], + "units": "meters", + "source": "file", + }, + ) + + +# --------------------------------------------------------------------------- +# Extrinsics recovery from synthetic checkerboard corners +# --------------------------------------------------------------------------- + +class TestExtrinsicsRecovery: + def test_two_board_combined_recovers_known_pose(self, scene): + r_gt, c_gt, wall_room, floor_room = scene + room_pts = np.concatenate([wall_room, floor_room], axis=0) + img_pts = project_room_points(room_pts, r_gt, c_gt) + + ext = cal.solve_extrinsics(room_pts, img_pts, K_GT, DIST_ZERO) + + assert ext["rmse_px"] < 1e-3 + np.testing.assert_allclose(np.asarray(ext["translation_m"]), c_gt, atol=1e-4) + r_delta = np.asarray(ext["rotation"]).T @ r_gt + angle_deg = np.degrees(np.arccos(np.clip((np.trace(r_delta) - 1) / 2, -1, 1))) + assert angle_deg < 0.01 + + def test_single_board_solves_agree(self, scene): + # With correct corner ordering, each board alone recovers the same pose. + r_gt, c_gt, wall_room, floor_room = scene + ext_wall = cal.solve_extrinsics( + wall_room, project_room_points(wall_room, r_gt, c_gt), K_GT, DIST_ZERO) + ext_floor = cal.solve_extrinsics( + floor_room, project_room_points(floor_room, r_gt, c_gt), K_GT, DIST_ZERO) + consistency = cal.extrinsics_consistency(ext_wall, ext_floor) + assert consistency["rotation_deg"] < 0.1 + assert consistency["translation_m"] < 1e-3 + + def test_reversed_corner_order_auto_recovered(self, scene): + # findChessboardCorners may enumerate from either board end. A single + # board cannot disambiguate that flip (centrosymmetric grid), but the + # joint two-board solve can -- feed it a reversed wall ordering and + # require the true pose back. + r_gt, c_gt, wall_room, floor_room = scene + wall_img = project_room_points(wall_room, r_gt, c_gt) + floor_img = project_room_points(floor_room, r_gt, c_gt) + ext = cal.solve_two_board_extrinsics( + wall_room, wall_img[::-1].copy(), floor_room, floor_img, + K_GT, DIST_ZERO) + assert ext["wall_flipped"] is True + assert ext["floor_flipped"] is False + assert ext["rmse_px"] < 1e-3 + np.testing.assert_allclose(np.asarray(ext["translation_m"]), c_gt, atol=1e-3) + + def test_joint_solver_matches_unflipped(self, scene): + r_gt, c_gt, wall_room, floor_room = scene + ext = cal.solve_two_board_extrinsics( + wall_room, project_room_points(wall_room, r_gt, c_gt), + floor_room, project_room_points(floor_room, r_gt, c_gt), + K_GT, DIST_ZERO) + assert ext["wall_flipped"] is False and ext["floor_flipped"] is False + assert ext["per_board"]["wall"]["rmse_px"] < 1e-3 + assert ext["per_board"]["floor"]["rmse_px"] < 1e-3 + + def test_intrinsics_recovered_from_synthetic_views(self): + # Several board views from different poses -> calibrateCamera should + # get focal length / principal point close to ground truth. + obj = cal.board_object_points(BOARD_COLS, BOARD_ROWS, SQUARE_M) + poses = [ + ([0.05, 1.2, 0.05], [0.10, 0.0, 0.06]), + ([-0.25, 1.0, 0.20], [0.10, 0.0, 0.06]), + ([0.45, 0.9, -0.15], [0.10, 0.0, 0.06]), + ([0.10, 1.4, 0.30], [0.10, 0.0, 0.06]), + ([-0.15, 0.8, -0.20], [0.10, 0.0, 0.06]), + ] + corner_sets = [] + for cam_pos, target in poses: + r, c = look_at_pose(cam_pos, target) + # Embed the board rigidly in the y=0 plane (u=+x, v=+z) and view it. + board_in_room = np.column_stack([obj[:, 0], obj[:, 2], obj[:, 1]]) + corner_sets.append(project_room_points(board_in_room, r, c)) + intr = cal.compute_intrinsics(corner_sets, (IMG_W, IMG_H), + BOARD_COLS, BOARD_ROWS, SQUARE_M) + k = np.asarray(intr["camera_matrix"]) + assert abs(k[0, 0] - K_GT[0, 0]) / K_GT[0, 0] < 0.05 + assert abs(k[1, 1] - K_GT[1, 1]) / K_GT[1, 1] < 0.05 + assert intr["reprojection_error_px"] < 1.0 + + +# --------------------------------------------------------------------------- +# Bundle round-trip + content hash +# --------------------------------------------------------------------------- + +class TestBundle: + def test_save_load_roundtrip(self, scene, tmp_path): + r_gt, c_gt, _, _ = scene + bundle = make_bundle(r_gt, c_gt) + path = tmp_path / "camera-room.json" + cal.save_bundle(bundle, path) + loaded = cal.load_bundle(path) + assert loaded == bundle + assert cal.calibration_id(loaded) == cal.calibration_id(bundle) + + def test_bundle_schema_fields(self, scene): + r_gt, c_gt, _, _ = scene + bundle = make_bundle(r_gt, c_gt) + for key in ("schema_version", "method", "calibrated_at", "room_frame", + "checkerboard_spec", "camera_intrinsics", + "camera_to_room_extrinsics", "transceiver_geometry"): + assert key in bundle + assert bundle["method"] == "two-checkerboard" + + def test_calibration_id_changes_with_content(self, scene): + r_gt, c_gt, _, _ = scene + bundle_a = make_bundle(r_gt, c_gt) + bundle_b = json.loads(json.dumps(bundle_a)) + bundle_b["transceiver_geometry"]["nodes"][0]["position_m"] = [0.2, 2.4, 1.1] + assert cal.calibration_id(bundle_a) != cal.calibration_id(bundle_b) + assert cal.calibration_id(bundle_a).startswith("sha256:") + + def test_load_bundle_rejects_missing_keys(self, tmp_path): + path = tmp_path / "bad.json" + path.write_text('{"camera_intrinsics": {}}', encoding="utf-8") + with pytest.raises(ValueError, match="missing key"): + cal.load_bundle(path) + + +# --------------------------------------------------------------------------- +# Keypoint transform: image -> room-frame bearing rays (projective alignment) +# --------------------------------------------------------------------------- + +class TestKeypointTransform: + PERSON_POINTS = np.array([ + [1.2, 1.5, 1.7], # head height + [1.1, 1.5, 1.4], # shoulder + [1.3, 1.6, 0.9], # hip + [1.2, 1.5, 0.1], # ankle + ]) + + @pytest.mark.parametrize("dist", [DIST_ZERO, DIST_MILD], ids=["no-distortion", "mild-distortion"]) + def test_rays_pass_through_original_points(self, scene, dist): + r_gt, c_gt, _, _ = scene + img = project_room_points(self.PERSON_POINTS, r_gt, c_gt, dist=dist) + kps_norm = (img / np.array([IMG_W, IMG_H])).tolist() + + ctx = cal.CalibrationContext(make_bundle(r_gt, c_gt, dist=dist), IMG_W, IMG_H) + origin, rays = ctx.transform_keypoints(kps_norm) + + np.testing.assert_allclose(origin, c_gt, atol=1e-9) + np.testing.assert_allclose(np.linalg.norm(rays, axis=1), 1.0, atol=1e-9) + for point, ray in zip(self.PERSON_POINTS, rays): + v = point - origin + # Distance from the true 3D point to the recovered ray ~ 0, and + # the point sits in FRONT of the camera along the ray. + dist_to_ray = np.linalg.norm(v - np.dot(v, ray) * ray) + assert dist_to_ray < 1e-4 + assert np.dot(v, ray) > 0 + + def test_resolution_scaling(self, scene): + # Collection camera runs 640x360 while the bundle was made at + # 1280x720 -- normalized keypoints must land on the same rays. + r_gt, c_gt, _, _ = scene + img = project_room_points(self.PERSON_POINTS, r_gt, c_gt) + kps_norm = (img / np.array([IMG_W, IMG_H])).tolist() + + ctx = cal.CalibrationContext(make_bundle(r_gt, c_gt), 640, 360) + origin, rays = ctx.transform_keypoints(kps_norm) + for point, ray in zip(self.PERSON_POINTS, rays): + v = point - origin + assert np.linalg.norm(v - np.dot(v, ray) * ray) < 1e-4 + + +# --------------------------------------------------------------------------- +# collect-ground-truth record path (import-level; no camera loop) +# --------------------------------------------------------------------------- + +class TestRecordAugmentation: + LEGACY_RECORD = { + "ts_ns": 1775300000000000000, + "keypoints": [[0.45, 0.12]] * 17, + "confidence": 0.92, + "n_visible": 14, + "n_persons": 1, + } + + def test_no_calibration_is_byte_identical(self): + # The collector's no---calibration path must emit exactly the + # original ADR-079 JSONL line (back-compat guarantee). + record = json.loads(json.dumps(self.LEGACY_RECORD)) + before = json.dumps(record) + out = cal.augment_record(record, None) + assert out is record + assert json.dumps(out) == before + assert set(out.keys()) == {"ts_ns", "keypoints", "confidence", + "n_visible", "n_persons"} + + def test_calibrated_record_gains_room_fields(self, scene): + r_gt, c_gt, _, _ = scene + bundle = make_bundle(r_gt, c_gt) + ctx = cal.CalibrationContext(bundle, IMG_W, IMG_H) + + record = json.loads(json.dumps(self.LEGACY_RECORD)) + out = cal.augment_record(record, ctx) + + # Raw image coords preserved untouched; room representation added. + assert out["keypoints"] == self.LEGACY_RECORD["keypoints"] + assert len(out["keypoints_room"]) == 17 + assert all(len(ray) == 3 for ray in out["keypoints_room"]) + assert out["calibration_id"] == cal.calibration_id(bundle) + assert out["transceiver_geometry"] == bundle["transceiver_geometry"] + assert len(out["camera_origin_room"]) == 3 + json.dumps(out) # remains JSONL-serializable + + def test_empty_keypoints_record(self, scene): + r_gt, c_gt, _, _ = scene + ctx = cal.CalibrationContext(make_bundle(r_gt, c_gt), IMG_W, IMG_H) + record = {"ts_ns": 1, "keypoints": [], "confidence": 0.0, + "n_visible": 0, "n_persons": 0} + out = cal.augment_record(record, ctx) + assert out["keypoints_room"] == [] + assert "calibration_id" in out