# Auto-calibration at record start — design doc **Author:** yam_calib · **Date:** 2026-07-02 · **Status:** design proposal, wiring not yet applied Goal: bake bimanual scene-camera calibration into base raiden's `recorder.py` on russet so every recording session self-calibrates at startup. Passive, no operator input as long as base boards are visible. Cross-refs: [calibration.md](calibration.md) · [fiducial_exo.md](fiducial_exo.md) · [data_recording.md](data_recording.md). --- ## What the current script does (`exo_calibrate_bimanual_fidex.py`, 1601 lines) Full breakdown of the current interactive script on russet at `/home/robot-lab/raiden_internal/raiden/calibration/exo_calibrate_bimanual_fidex.py`. **High-level flow:** 1. Env: force `MUJOCO_GL=egl` + `PYOPENGL_PLATFORM=egl` (russet has no GLFW display) 2. Import lazy helpers from `exo_calibrate_fidex.py` (single-arm sibling): `_PoseHistory`, `_DetectionState`, `_solve_multiframe_pose`, `_open_zed_native`, `_render_with_intrinsics`, `_compose_overlay`, `_save_calibration`, `_init_rerun`, etc. 3. Load `YAM_BASE_ONLY_CONFIG` (arm 1) + `YAM_BASE_ONLY_CONFIG_ARM2` (arm 2) 4. **Apply russet v2-reverse patch to arm 2** (STL swap + `aruco_offset_pos[0]` sign flip) — this is russet-only, physical-mount-specific 5. Build bimanual MuJoCo XML via `Demos.sim_yam_bimanual.build_bimanual_xml` 6. Position exo meshes for both arms 7. `_discover_role_scene_cams(camera_config_file)` — walks `camera.json` for entries with `role: "scene"` 8. Open ZED at HD1080 for each scene cam 9. Discover per-arm qpos indices for MuJoCo (arm 1 = unprefixed, arm 2 = `arm2_` prefix) 10. Optionally init bimanual RobotController (2 leaders + 2 followers) + `setup_for_teleop_recording()` 11. Init rerun visualizer 12. Spawn ONE daemon thread `_capture_loop_bimanual_multicam` that: - Per frame per cam: `cv2.aruco.detectMarkers` once, `_partition_corners` by ID range (arm 1: 217–225, arm 2: 226–234) - `_solve_one_arm` for each arm: single-frame PnP via `do_est_aruco_pose` (using `corners_est=` to skip re-detect), unwind center-shift, add to per-arm `_PoseHistory` if `accumulating`, pool top-K residuals, run `_solve_multiframe_pose` on the union - Drive both followers into MuJoCo qpos - Re-anchor arm-2 body from live PnP delta (write `model.body_pos[arm2_body_id]` + `body_quat`) - `mj_forward` + re-position arm 2's exo/ArUco mocap meshes - Render MuJoCo overlay with each cam's K + extrinsic, composite over BGR, log to rerun 13. Main thread `input()` loop: `c` toggles `accumulating` + clears histories, `y` saves, `q` quits 14. `_save_bimanual_calibration` writes to `CALIBRATION_FILE`: - `cameras[__arm1]` = arm-1 record - `cameras[__arm2]` = arm-2 record - `cameras[]` = plain (= arm 2 content, world = left arm base per Shun's convention) - `bimanual_transform.right_base_to_left_base` (derived) - `image_size` injected on all three entries ## What's essential vs scaffolding | Component | Keep for auto-cal? | Why | |---|---|---| | ArUco `DICT_6X6_250` + ID partitioning | **YES** | Core physics — can't skip | | `do_est_aruco_pose` (from `exo_utils`) | **YES** | Handles the corner-vs-center-shift math + returns proper `T_aruco_in_cam` | | `_PoseHistory` ring buffer (max=30) | **YES** | Robustness — single-frame PnP is noisy | | Top-K (10) pooled `_solve_multiframe_pose` | **YES** | Same reason | | Russet v2-reverse arm-2 patch | **YES** | Otherwise arm-2 pose is flipped | | `link_to_aruco_transform` (rigid board→link) | **YES** | Chains PnP output into `T_cam_in_arm_base` | | `bimanual_transform.right_base_to_left_base` derivation | **YES** | Downstream visualizer + eval read it | | `_save_calibration` schema | **YES** | LiveVisualizer + deploy hardcode the `__arm1` / `__arm2` / plain convention | | ZED at HD1080 | **YES (must)** | 720p → scene_cam detects 0/18 markers | | MuJoCo model + `build_bimanual_xml` | **NO** | Only needed for the overlay render | | `position_exoskeleton_meshes` | **NO** | Same | | MuJoCo per-frame `body_pos` re-anchor + `mj_forward` | **NO** | Same | | Renderer with `mujoco.GLContext` + EGL | **NO** | Same | | `_render_with_intrinsics` + `_compose_overlay` | **NO** | Same | | Rerun visualizer + JPEG encode + status text | **NO** | Auto-cal has no live UI | | Threading + `_MJ_RENDER_LOCK` + stop_event | **NO** | Single-threaded is fine at record start; no real-time viz to serve | | `_DetectionState` (`stability_str()`, snapshot lock) | **NO** | We don't need per-frame telemetry to decide when to save; auto-cal just runs N frames | | Bimanual teleop init (2 leaders + 2 followers) | **NO** | record.py starts arms separately; auto-cal doesn't drive them | | `_drive_arm_qpos` + follower reads | **NO** | Same | | Yellow-button lock toggle + input() loop | **NO** | No operator during auto-cal | | `_discover_arm_qpos` | **NO** | Only needed for MuJoCo drive | ## Proposed simplified helpers — `raiden/calibration/exo_auto.py` Live in **base raiden**, not raiden_internal. Pure library (no argparse, no rerun, no MuJoCo render, no threading). ```python """Auto-calibration for bimanual scene cameras — passive, headless. Called from `recorder.py` at record start. Pools N frames per camera per arm, runs pooled PnP, writes calibration_results.json. Zero operator input. Simplified from `raiden_internal/raiden/calibration/exo_calibrate_bimanual_fidex.py`: we drop the interactive rerun UI, the MuJoCo overlay render, the bimanual teleop init, and the threading — all that machinery is for verification, not for auto-cal. """ from pathlib import Path from typing import Callable, Optional import copy import json import time import cv2 import numpy as np class CalibrationFailure(RuntimeError): """Auto-cal couldn't recover a required arm's pose after pooling frames.""" # ------------------------------------------------------------ # 1. Config loading + russet patch # ------------------------------------------------------------ def apply_russet_arm2_v2_reverse_patch(exo_cfg_arm2) -> None: """Mutate arm-2 config in-place. Idempotent: safe to call twice. russet's arm-2 base board is mounted mirrored across X (clearance envelope). Two edits: swap the STL path AND flip aruco_offset_pos[0] sign so PnP recovery matches the physical mount orientation. """ from pathlib import Path from ExoConfigs.exoskeleton import _EXO_DIR # or resolve as needed STL = str(Path(_EXO_DIR) / "so100_blender_testings" / "yam_base_board_v2_reverse_clearance_fixed.stl") for link_cfg in exo_cfg_arm2.links.values(): # Guard against double-flip: check if STL is already the reversed one if link_cfg.exo_mesh_path == STL: continue link_cfg.exo_mesh_path = STL old_x = float(link_cfg.aruco_offset_pos[0]) link_cfg.aruco_offset_pos = np.array( [-old_x, link_cfg.aruco_offset_pos[1], link_cfg.aruco_offset_pos[2]], dtype=np.float64) def load_bimanual_configs(patch_russet_v2_reverse: bool = True): """Return (exo_cfg_arm1, exo_cfg_arm2). Deep-copies arm 2 to avoid mutating shared module state. Applies russet patch if requested.""" from ExoConfigs.yam_exo import YAM_BASE_ONLY_CONFIG, YAM_BASE_ONLY_CONFIG_ARM2 exo_arm1 = YAM_BASE_ONLY_CONFIG # arm 1 unchanged exo_arm2 = copy.deepcopy(YAM_BASE_ONLY_CONFIG_ARM2) # deep copy — we mutate if patch_russet_v2_reverse: apply_russet_arm2_v2_reverse_patch(exo_arm2) return exo_arm1, exo_arm2 # ------------------------------------------------------------ # 2. Pooling # ------------------------------------------------------------ class BoardPoseHistory: """Ring buffer of (obj_pts, img_pts, residual_px) tuples for one arm's board on one camera. Sorted by residual on read; top_k_concatenated returns the union of the K lowest-residual entries. """ def __init__(self, max_size: int = 30): self.max_size = max_size self.entries: list[tuple[np.ndarray, np.ndarray, float]] = [] def add(self, obj_pts, img_pts, residual_px: float): self.entries.append((obj_pts, img_pts, float(residual_px))) if len(self.entries) > self.max_size: # drop the WORST entry (highest residual), not the oldest worst_idx = max(range(len(self.entries)), key=lambda i: self.entries[i][2]) self.entries.pop(worst_idx) def top_k_concatenated(self, k: int = 10): if not self.entries: return None, None, 0, float("nan") top = sorted(self.entries, key=lambda e: e[2])[:k] obj_cat = np.concatenate([e[0] for e in top], axis=0) img_cat = np.concatenate([e[1] for e in top], axis=0) mean_resid = float(np.mean([e[2] for e in top])) return obj_cat, img_cat, len(top), mean_resid # ------------------------------------------------------------ # 3. Detection + per-arm pose solve (one frame) # ------------------------------------------------------------ _ARM1_IDS = set(range(217, 217 + 9)) _ARM2_IDS = set(range(226, 226 + 9)) def detect_and_split_boards(bgr): """One `cv2.aruco.detectMarkers` call, split by ID range. Returns (arm1_corners_est, arm2_corners_est) where each is either None or a (corners_subset, ids_subset, []) tuple consumable by `do_est_aruco_pose(..., corners_est=...)`. """ from exo_utils import ARUCO_DICT # DICT_6X6_250 gray = cv2.cvtColor(bgr, cv2.COLOR_BGR2GRAY) corners_all, ids_all, _ = cv2.aruco.detectMarkers( gray, ARUCO_DICT, parameters=cv2.aruco.DetectorParameters()) if ids_all is None: return None, None ids_flat = ids_all.flatten() def _make(id_set): mask = np.array([int(i) in id_set for i in ids_flat]) if not mask.any(): return None sub_corners = [corners_all[i] for i in np.where(mask)[0]] sub_ids = ids_all[mask].reshape(-1, 1) return (sub_corners, sub_ids, []) return _make(_ARM1_IDS), _make(_ARM2_IDS) def solve_arm_frame(bgr, corners_est, aruco_board, board_length, T_aruco_to_link, K, dist, history: BoardPoseHistory, top_k: int = 10): """Per-arm per-frame: single-shot PnP, add to history, pooled solve. Returns dict with keys: T_cam_in_arm_base: (4,4) np.ndarray or None multi_resid_px: float n_used: int # frames in the pool """ from exo_utils import do_est_aruco_pose, ARUCO_DICT if corners_est is None: return None try: result = do_est_aruco_pose( bgr, ARUCO_DICT, aruco_board, board_length, cameraMatrix=K, distCoeffs=dist, corners_est=corners_est) except Exception: return None if result == -1: return None T_aruco_in_cam_single = result["est_aruco_pose"] # Undo center-shift (see exo_calibrate.py for the full explanation). rvec, tvec_shifted = result["rtvec"] R = cv2.Rodrigues(rvec)[0] center_offset = np.array([board_length / 2, board_length / 2, 0]) tvec_corner = tvec_shifted - R @ center_offset obj_cam, img_pts = result["obj_img_pts"] obj_board = (R.T @ (obj_cam.T - tvec_corner.reshape(3, 1))).T proj_chk, _ = cv2.projectPoints( obj_board.astype(np.float32), rvec, tvec_corner, np.asarray(result["cameraMatrix"], np.float64), (np.asarray(result["distCoeffs"], np.float64).reshape(-1) if result.get("distCoeffs") is not None else dist)) residual = float(np.linalg.norm( proj_chk.reshape(-1, 2) - img_pts.reshape(-1, 2), axis=1).mean()) history.add(obj_board, img_pts, residual) # Pooled solve obj_pool, img_pool, n_used, _ = history.top_k_concatenated(k=top_k) T_aruco_in_cam = T_aruco_in_cam_single multi_resid = float("nan") if obj_pool is not None and len(obj_pool) >= 4: from raiden.calibration.exo_calibrate_fidex import _solve_multiframe_pose mf, mf_resid = _solve_multiframe_pose( obj_pool, img_pool, K, dist, board_length) if mf is not None: T_aruco_in_cam = mf multi_resid = mf_resid T_link_in_cam = T_aruco_in_cam @ T_aruco_to_link T_cam_in_arm_base = np.linalg.inv(T_link_in_cam) return {"T_cam_in_arm_base": T_cam_in_arm_base, "multi_resid_px": multi_resid, "n_used": n_used} # ------------------------------------------------------------ # 4. Top-level auto-cal (what record.py calls) # ------------------------------------------------------------ def calibrate_bimanual_scene( cam_handles: list[dict], exo_cfg_arm1, exo_cfg_arm2, n_frames_per_cam: int = 15, history_size: int = 30, top_k: int = 10, require_both_arms: bool = True, require_all_cams: bool = True, max_wait_s: float = 30.0, ) -> dict: """Pool frames from each camera, solve per-arm PnP, return per-cam Ts. cam_handles: list of dicts with keys: name (str) grab_bgr(callable) -> np.ndarray or None K (3,3) np.ndarray dist (5,) or (1,5) np.ndarray Returns: {cam_name: {"T_cam_in_arm1_base": T, "T_cam_in_arm2_base": T, "multi_resid_arm1": r, "multi_resid_arm2": r}} Raises CalibrationFailure if requirements not met. """ from ExoConfigs.exoskeleton import link_to_aruco_transform link_cfg_arm1 = exo_cfg_arm1.links["larger_coarse_board"] link_cfg_arm2 = exo_cfg_arm2.links["larger_coarse_board_arm2"] board_arm1 = exo_cfg_arm1.aruco_board_objects["larger_coarse_board"] board_arm2 = exo_cfg_arm2.aruco_board_objects["larger_coarse_board_arm2"] board_length = link_cfg_arm1.board_length T_link2aruco_arm1 = link_to_aruco_transform(link_cfg_arm1) T_link2aruco_arm2 = link_to_aruco_transform(link_cfg_arm2) T_aruco2link_arm1 = np.linalg.inv(T_link2aruco_arm1) T_aruco2link_arm2 = np.linalg.inv(T_link2aruco_arm2) per_cam = {} for h in cam_handles: per_cam[h["name"]] = { "history_arm1": BoardPoseHistory(history_size), "history_arm2": BoardPoseHistory(history_size), "handle": h, } deadline = time.monotonic() + max_wait_s n_frames_grabbed = {n: 0 for n in per_cam} while any(v < n_frames_per_cam for v in n_frames_grabbed.values()): if time.monotonic() > deadline: break for name, cam_state in per_cam.items(): if n_frames_grabbed[name] >= n_frames_per_cam: continue h = cam_state["handle"] bgr = h["grab_bgr"]() if bgr is None: continue corners_arm1, corners_arm2 = detect_and_split_boards(bgr) solve_arm_frame(bgr, corners_arm1, board_arm1, board_length, T_aruco2link_arm1, h["K"], h["dist"], cam_state["history_arm1"], top_k) solve_arm_frame(bgr, corners_arm2, board_arm2, board_length, T_aruco2link_arm2, h["K"], h["dist"], cam_state["history_arm2"], top_k) n_frames_grabbed[name] += 1 result = {} for name, cam_state in per_cam.items(): h = cam_state["handle"] # Final pooled solve using the best entries. r1 = _final_solve(cam_state["history_arm1"], board_arm1, board_length, T_aruco2link_arm1, h["K"], h["dist"], top_k) r2 = _final_solve(cam_state["history_arm2"], board_arm2, board_length, T_aruco2link_arm2, h["K"], h["dist"], top_k) if require_both_arms and (r1 is None or r2 is None): if require_all_cams: raise CalibrationFailure( f"cam {name!r}: arm1={r1 is not None} arm2={r2 is not None} " f"(need both). Wait for boards to be visible + retry.") continue result[name] = { "T_cam_in_arm1_base": r1["T_cam_in_arm_base"] if r1 else None, "T_cam_in_arm2_base": r2["T_cam_in_arm_base"] if r2 else None, "multi_resid_arm1_px": r1["multi_resid_px"] if r1 else float("nan"), "multi_resid_arm2_px": r2["multi_resid_px"] if r2 else float("nan"), "n_used_arm1": r1["n_used"] if r1 else 0, "n_used_arm2": r2["n_used"] if r2 else 0, } if require_all_cams and set(result.keys()) != {h["name"] for h in cam_handles}: missing = {h["name"] for h in cam_handles} - set(result.keys()) raise CalibrationFailure(f"cams missing valid calibration: {missing}") return result def _final_solve(history: BoardPoseHistory, aruco_board, board_length, T_aruco_to_link, K, dist, top_k): """Terminal pooled solve after all frames grabbed.""" obj_pool, img_pool, n_used, mean_resid = history.top_k_concatenated(k=top_k) if obj_pool is None or len(obj_pool) < 4: return None from raiden.calibration.exo_calibrate_fidex import _solve_multiframe_pose T_aruco_in_cam, resid = _solve_multiframe_pose( obj_pool, img_pool, K, dist, board_length) if T_aruco_in_cam is None: return None T_link_in_cam = T_aruco_in_cam @ T_aruco_to_link return { "T_cam_in_arm_base": np.linalg.inv(T_link_in_cam), "multi_resid_px": float(resid), "n_used": int(n_used), } # ------------------------------------------------------------ # 5. Save (same schema as _save_bimanual_calibration) # ------------------------------------------------------------ _RES_PX = {"HD720": [1280, 720], "HD1080": [1920, 1080], "HD2K": [2208, 1242]} def save_bimanual_calibration_json( result: dict, cam_handles: list[dict], resolution: str, out_path: Path, ) -> None: """Write the calibration_results.json schema that LiveVisualizer + deploy consume. Per cam we write THREE `cameras[]` entries: __arm1 — arm 1 (right) reference __arm2 — arm 2 (left, = world) reference — plain key, matches __arm2 (Shun's convention) PLUS a top-level `bimanual_transform.right_base_to_left_base` (== T_left_in_right) when both arm solves are present for cam 0. """ out_path = Path(out_path) cal = json.loads(out_path.read_text()) if out_path.exists() else {} cams = cal.setdefault("cameras", {}) img_size = _RES_PX.get(resolution, [1920, 1080]) def _mk_entry(K, dist, T_cam_in_base, arm_label): return { "intrinsics": { "K": np.asarray(K).tolist(), "distortion": np.asarray(dist).reshape(-1).tolist(), "image_size": img_size, }, "extrinsics": { "T_cam_in_base": np.asarray(T_cam_in_base).tolist(), }, "arm": arm_label, "resolution": resolution, } h_by_name = {h["name"]: h for h in cam_handles} T1_ref = T2_ref = None for name, r in result.items(): h = h_by_name[name] if r["T_cam_in_arm1_base"] is not None: cams[f"{name}__arm1"] = _mk_entry( h["K"], h["dist"], r["T_cam_in_arm1_base"], "right") if r["T_cam_in_arm2_base"] is not None: cams[f"{name}__arm2"] = _mk_entry( h["K"], h["dist"], r["T_cam_in_arm2_base"], "left") cams[name] = _mk_entry( h["K"], h["dist"], r["T_cam_in_arm2_base"], "left") if T1_ref is None and r["T_cam_in_arm1_base"] is not None: T1_ref = r["T_cam_in_arm1_base"] if T2_ref is None and r["T_cam_in_arm2_base"] is not None: T2_ref = r["T_cam_in_arm2_base"] # bimanual_transform.right_base_to_left_base = T_left_in_right (inverse of T_right_in_left) if T1_ref is not None and T2_ref is not None: T_right_in_left = T2_ref @ np.linalg.inv(T1_ref) T_left_in_right = np.linalg.inv(T_right_in_left) cal["bimanual_transform"] = { "right_base_to_left_base": T_left_in_right.tolist(), "method": "auto-cal at record start", "translation_m_right_base_in_left_base": T_right_in_left[:3, 3].tolist(), } out_path.write_text(json.dumps(cal, indent=2)) ``` ## Wiring hook — proposed `raiden/recorder.py` diff Add right after `load_cameras_from_config` returns (or wherever scene cams are available), before the operator begins the demo: ```python from raiden.calibration.exo_auto import ( CalibrationFailure, load_bimanual_configs, calibrate_bimanual_scene, save_bimanual_calibration_json) def _run_auto_cal(scene_cams, recording_dir, resolution="HD1080"): """Called once at record start. If cameras aren't at HD1080, we do a temporary re-open at HD1080 for the calibration window, then re-open at the recording resolution. Failure is non-fatal — we log + fall back to whatever calibration_results.json already exists.""" try: exo_a1, exo_a2 = load_bimanual_configs(patch_russet_v2_reverse=True) cam_handles = [{ "name": name, "grab_bgr": lambda cam=cam: cam.grab_bgr(), "K": cam.K, "dist": cam.dist, "W": cam.W, "H": cam.H, } for name, cam in scene_cams.items()] result = calibrate_bimanual_scene( cam_handles, exo_a1, exo_a2, n_frames_per_cam=15) save_bimanual_calibration_json( result, cam_handles, resolution=resolution, out_path=recording_dir / "calibration_results.json") print(f" ✓ auto-cal: {list(result.keys())} — " f"{sum(v['n_used_arm1'] for v in result.values())} arm1 frames " f"+ {sum(v['n_used_arm2'] for v in result.values())} arm2 frames pooled") except CalibrationFailure as e: print(f" ⚠ auto-cal failed ({e}); using prior calibration_results.json") ``` ## Blocker: HD1080 for scene cams record.py currently opens scene cams at **HD720@30**. At 720p the scene_cam detects **0/18** markers (empirical on `flip_pink_cup_2026-06-30`). Auto-cal REQUIRES HD1080. Options: 1. **Simple:** bump scene cams to HD1080 for the whole recording. File size ~2× 2. **Bimodal:** open scene cams at HD1080 for calibration (~2s window), close + reopen at HD720 for main recording. More code but bandwidth-friendly Ego + wrist cams stay at HD720 (they detect fine at close range — ego_cam @ 720p gets 16/18 markers). ## Deliverables checklist - [x] Understand current `_fidex` script — 1601 lines of bimanual scene cal with rerun+MuJoCo+teleop - [x] Identify essential vs scaffolding — table above - [x] Design simplified helper API (`raiden/calibration/exo_auto.py`) — ~300 lines pure library - [x] Design record.py wiring hook — ~20 lines - [x] Save goal to persistent memory — `~/.claude/projects/-data-cameron/memory/yam-record-autocal-goal.md` - [x] Save design doc to shared vault — this file - [ ] Write the actual `exo_auto.py` on russet — **BLOCKED, awaiting review** - [ ] Patch record.py — **BLOCKED, awaiting review** - [ ] Decide HD1080 tradeoff (options 1 vs 2) — **needs your call** - [ ] Run one live test on russet — **after wiring** ## Open questions for the user 1. **HD1080 tradeoff** — Option 1 (bump all recording to HD1080) or Option 2 (bimodal open at record start)? 2. **`n_frames_per_cam`** — 15 is a guess; empirically the pooled solve converges by ~10 frames. Fine to make configurable via `record.py` CLI arg? 3. **On failure — hard fail or silent continue?** Currently proposed: warn + continue with prior cal. Alternative: hard-fail record startup so the operator can't accidentally record without cal. 4. **Ego cam auto-cal too?** It's `role: "scene"` in `camera.json` at russet, so `_discover_role_scene_cams` picks it up. Ego cam sees the boards close-range so it should calibrate at HD720. Include or exclude from auto-cal loop? ## References - Source of truth (current script): `/home/robot-lab/raiden_internal/raiden/calibration/exo_calibrate_bimanual_fidex.py` on russet - Local mirror I read from: `/data/cameron/agents_stuff/agents/yam_calib/scratch/exo_calibrate_bimanual_fidex.py` - Base raiden's `recorder.py` (wire-in target): `/home/robot-lab/raiden/raiden/recorder.py` on russet - Public URL for the (older) fiducial-exo geometry doc: https://omidlab.net/browse/para/robot/yam/docs/fiducial_exoskeleton_board.md?raw