"""Auto-calibration for bimanual scene cameras — passive, headless. PROPOSED for `raiden/calibration/exo_auto.py` in base raiden on russet. 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` (1601 lines) — dropped: rerun UI, MuJoCo overlay render, bimanual teleop init, threading, interactive input() loop, per-frame status telemetry. Kept: the PnP math (ArUco detection, pooling, multi-frame solve), russet v2-reverse patch, and the `calibration_results.json` schema (LiveVisualizer + deploy depend on it). Usage (in recorder.py): from raiden.calibration.exo_auto import ( CalibrationFailure, load_bimanual_configs, calibrate_bimanual_scene, save_bimanual_calibration_json) 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} 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="HD1080", out_path=recording_dir / "calibration_results.json") except CalibrationFailure as e: print(f"auto-cal failed: {e}; using prior calibration if any") Status: **DRAFT, not yet on russet.** Awaiting user review before wiring. """ from __future__ import annotations import copy import json import time from pathlib import Path from typing import Callable, Optional import cv2 import numpy as np class CalibrationFailure(RuntimeError): """Auto-cal couldn't recover a required arm's pose after pooling frames.""" # ---------------------------------------------------------------------- # _solve_multiframe_pose — inlined verbatim from # raiden_internal/raiden/calibration/exo_calibrate_fidex.py:371 so this # module is self-contained (doesn't require raiden_internal on sys.path). # ---------------------------------------------------------------------- def _solve_multiframe_pose(obj_pts: np.ndarray, img_pts: np.ndarray, K: np.ndarray, dist: np.ndarray, board_length: float): """Same post-processing as ``do_est_aruco_pose`` (ambiguity resolution, center-offset shift, Y/Z column flip), but applied to a pre-pooled correspondence set across multiple frames. Returns (T_aruco_in_cam_4x4, residual_px) or (None, None) on failure. """ ok, rvecs, tvecs, _ = cv2.solvePnPGeneric( obj_pts.astype(np.float32), img_pts.astype(np.float32), K, dist, flags=cv2.SOLVEPNP_ITERATIVE, ) if not ok or not rvecs: return None, None # Ambiguity disambiguation — same heuristic as do_est_aruco_pose. best_idx = 0 for i, (rv, tv) in enumerate(zip(rvecs, tvecs)): R_mat, _ = cv2.Rodrigues(rv) normal = R_mat @ np.array([0., 0., 1.]) if tv[2] > 0 and normal[2] < 0: best_idx = i break rvec = rvecs[best_idx][:, 0] tvec = tvecs[best_idx][:, 0] R_mat = cv2.Rodrigues(rvec)[0] center_offset_board = np.array([board_length / 2, board_length / 2, 0], dtype=np.float64) tvec_shifted = tvec + R_mat.dot(center_offset_board) est = np.eye(4) est[:3, 3] = tvec_shifted est[:3, :3] = R_mat est[:, 1:-1] *= -1 # same Y/Z column flip do_est_aruco_pose applies proj, _ = cv2.projectPoints(obj_pts.astype(np.float32), rvec, tvec, K, dist) residual = float(np.linalg.norm( proj.reshape(-1, 2) - img_pts.reshape(-1, 2), axis=1).mean()) return est, residual # ---------------------------------------------------------------------- # 1. Config loading + russet patch # ---------------------------------------------------------------------- def apply_russet_arm2_v2_reverse_patch(exo_cfg_arm2, *, verbose: bool = True) -> 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. Only call on russet. On the upstream single-arm rig, do NOT apply. """ # Locate exo_redo via ExoConfigs' own module path — robust regardless # of how sys.path was set up. import ExoConfigs exo_configs_dir = Path(ExoConfigs.__file__).resolve().parent # .../exo_redo/ExoConfigs exo_redo_dir = exo_configs_dir.parent # .../exo_redo STL = str(exo_redo_dir / "so100_blender_testings" / "yam_base_board_v2_reverse_clearance_fixed.stl") for link_cfg in exo_cfg_arm2.links.values(): if link_cfg.exo_mesh_path == STL: continue # already patched link_cfg.exo_mesh_path = STL old = np.asarray(link_cfg.aruco_offset_pos, dtype=np.float64).copy() link_cfg.aruco_offset_pos = np.array( [-old[0], old[1], old[2]], dtype=np.float64) if verbose: print(f" [exo_auto] arm2 v2-reverse patch: STL={Path(STL).name}, " f"aruco_offset_pos.x {old[0]:+.2f} → {-old[0]:+.2f}") 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 exo_arm2 = copy.deepcopy(YAM_BASE_ONLY_CONFIG_ARM2) 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)` for one arm on one cam. top_k_concatenated returns the union of the K lowest-residual entries for a single pooled PnP solve. """ def __init__(self, max_size: int = 30): self.max_size = int(max_size) self.entries: list[tuple[np.ndarray, np.ndarray, float]] = [] def add(self, obj_pts: np.ndarray, img_pts: np.ndarray, residual_px: float) -> None: self.entries.append((obj_pts, img_pts, float(residual_px))) if len(self.entries) > self.max_size: # drop the WORST entry, not the oldest — high-res frames are more # valuable than recency worst = max(range(len(self.entries)), key=lambda i: self.entries[i][2]) self.entries.pop(worst) 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) return obj_cat, img_cat, len(top), float(np.mean([e[2] for e in top])) # ---------------------------------------------------------------------- # 3. Detection + per-frame per-arm solve # ---------------------------------------------------------------------- _ARM1_IDS = frozenset(range(217, 217 + 9)) # larger_coarse_board _ARM2_IDS = frozenset(range(226, 226 + 9)) # larger_coarse_board_arm2 def detect_and_split_boards(bgr: np.ndarray): """One `cv2.aruco.detectMarkers` call, split by ID range. Returns `(arm1_corners_est, arm2_corners_est)`; each is either None or a `(corners_subset, ids_subset, [])` tuple ready to pass into `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 _sub(id_set): mask = np.array([int(i) in id_set for i in ids_flat]) if not mask.any(): return None return ([corners_all[i] for i in np.where(mask)[0]], ids_all[mask].reshape(-1, 1), []) return _sub(_ARM1_IDS), _sub(_ARM2_IDS) def solve_arm_frame( bgr: np.ndarray, corners_est, aruco_board, board_length: float, T_aruco_to_link: np.ndarray, K: np.ndarray, dist: np.ndarray, history: BoardPoseHistory, top_k: int = 10, ) -> Optional[dict]: """Per-frame per-arm: single-shot PnP, add to history, pooled solve. Returns dict with keys `T_cam_in_arm_base` (4,4), `multi_resid_px`, `n_used` — or None if no detection this frame. """ 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 do_est_aruco_pose's board-center shift so the (obj, img) pairs # we cache use the standard corner-origin convention. 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 using the K lowest-residual entries so far. 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: 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 return { "T_cam_in_arm_base": np.linalg.inv(T_link_in_cam), "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 pooled PnP, return per-cam Ts. `cam_handles`: list of dicts with keys `name` (str), `grab_bgr` (callable that returns a BGR ndarray or None on timeout), `K` (3,3), `dist` (5,). Returns: {cam_name: {"T_cam_in_arm1_base": T, "T_cam_in_arm2_base": T, "multi_resid_arm1_px": float, "multi_resid_arm2_px": float, "n_used_arm1": int, "n_used_arm2": int}} Raises CalibrationFailure if `require_both_arms` / `require_all_cams` aren't satisfied within `max_wait_s`. """ 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_arm1 = link_cfg_arm1.board_length board_length_arm2 = link_cfg_arm2.board_length T_aruco2link_arm1 = np.linalg.inv(link_to_aruco_transform(link_cfg_arm1)) T_aruco2link_arm2 = np.linalg.inv(link_to_aruco_transform(link_cfg_arm2)) per_cam = {h["name"]: { "history_arm1": BoardPoseHistory(history_size), "history_arm2": BoardPoseHistory(history_size), "handle": h, # Best-of-pool frame + its detected corners/ids — the markers-panel # viz consumes this so we always show a frame at CAL RESOLUTION # (HD1080 for scene cam), not the post-downshift HD720 where scene # cam detects 0 markers. Set to (bgr, corners_all, ids_all, n_total) # each time we see a frame with more detected markers than before. "best_bgr": None, "best_corners": None, "best_ids": None, "best_n_total": 0, } for h in cam_handles} n_frames = {h["name"]: 0 for h in cam_handles} print(f" [exo_auto] pooling {n_frames_per_cam} frame(s)/cam for " f"{len(cam_handles)} cam(s): {list(per_cam.keys())}") deadline = time.monotonic() + max_wait_s while any(v < n_frames_per_cam for v in n_frames.values()): if time.monotonic() > deadline: print(f" [exo_auto] max_wait_s ({max_wait_s}s) hit — moving on with " f"partial pools: {n_frames}") break for name, cs in per_cam.items(): if n_frames[name] >= n_frames_per_cam: continue bgr = cs["handle"]["grab_bgr"]() if bgr is None: continue # Detect once, keep the raw ArUco output so we can also # remember the best-detection frame for the markers panel. from exo_utils import ARUCO_DICT _gray = cv2.cvtColor(bgr, cv2.COLOR_BGR2GRAY) _corners_all, _ids_all, _ = cv2.aruco.detectMarkers( _gray, ARUCO_DICT, parameters=cv2.aruco.DetectorParameters()) _n_total = 0 if _ids_all is None else int(len(_ids_all.flatten())) if _n_total > cs["best_n_total"]: cs["best_bgr"] = bgr.copy() cs["best_corners"] = _corners_all cs["best_ids"] = _ids_all cs["best_n_total"] = _n_total # Partition + per-arm solve (as before). c1, c2 = detect_and_split_boards(bgr) solve_arm_frame(bgr, c1, board_arm1, board_length_arm1, T_aruco2link_arm1, cs["handle"]["K"], cs["handle"]["dist"], cs["history_arm1"], top_k) solve_arm_frame(bgr, c2, board_arm2, board_length_arm2, T_aruco2link_arm2, cs["handle"]["K"], cs["handle"]["dist"], cs["history_arm2"], top_k) n_frames[name] += 1 result = {} for name, cs in per_cam.items(): h = cs["handle"] r1 = _final_solve(cs["history_arm1"], board_arm1, board_length_arm1, T_aruco2link_arm1, h["K"], h["dist"], top_k) r2 = _final_solve(cs["history_arm2"], board_arm2, board_length_arm2, 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_ok={r1 is not None} " f"arm2_ok={r2 is not None} (both required)") 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, # Best-detected frame captured DURING pooling (at cal resolution # — HD1080 for scene cam, HD720 for others). markers-panel viz # renders from this, not a post-downshift retrieval. "best_bgr": cs["best_bgr"], "best_corners": cs["best_corners"], "best_ids": cs["best_ids"], "best_n_total": cs["best_n_total"], } # Print per-arm pooled residuals so we can compare arm1 vs arm2 # detection quality after each auto-cal run. Sub-pixel = trustworthy, # 1–2 px = usable, >2 px = probably misaligned (retry). _r1 = result[name]["multi_resid_arm1_px"] _r2 = result[name]["multi_resid_arm2_px"] _n1 = result[name]["n_used_arm1"] _n2 = result[name]["n_used_arm2"] print(f" [exo_auto] {name} pooled residuals: " f"arm1={_r1:.3f}px (n={_n1}) " f"arm2={_r2:.3f}px (n={_n2})") # Save the best-detection frame (raw + annotated) to /tmp so the # markers-panel viz can render it at CAL RESOLUTION (HD1080 for # scene cam), not the post-downshift HD720 where scene cam # detects 0 markers. if cs["best_bgr"] is not None: try: _raw_path = Path(f"/tmp/cal_bestframe_{name}.png") cv2.imwrite(str(_raw_path), cs["best_bgr"]) _ann = cs["best_bgr"].copy() if cs["best_ids"] is not None: cv2.aruco.drawDetectedMarkers( _ann, cs["best_corners"], cs["best_ids"]) _ids_flat = cs["best_ids"].flatten().tolist() _a1 = sum(1 for i in _ids_flat if 217 <= int(i) <= 225) _a2 = sum(1 for i in _ids_flat if 226 <= int(i) <= 234) else: _a1 = _a2 = 0 _hdr = (f"best-of-pool markers: arm1={_a1}/9 arm2={_a2}/9 " f"(res={_ann.shape[1]}x{_ann.shape[0]}, " f"pool={cs['best_n_total']}/18 max)") cv2.rectangle(_ann, (10, 10), (10 + 9 * len(_hdr), 46), (0, 0, 0), -1) cv2.putText(_ann, _hdr, (14, 38), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2, cv2.LINE_AA) _ann_path = Path(f"/tmp/cal_bestmarkers_{name}.png") cv2.imwrite(str(_ann_path), _ann) print(f" [exo_auto] {name} best-of-pool frame: " f"{cs['best_n_total']}/18 markers, saved " f"{_ann_path}") except Exception as _saveErr: print(f" [exo_auto] failed to save bestframe for " f"{name}: {_saveErr}") if require_all_cams: missing = {h["name"] for h in cam_handles} - set(result.keys()) if missing: raise CalibrationFailure( f"missing valid cal for cam(s): {missing}") return result def _final_solve(history: BoardPoseHistory, aruco_board, board_length, T_aruco_to_link, K, dist, top_k): obj_pool, img_pool, n_used, _ = history.top_k_concatenated(k=top_k) if obj_pool is None or len(obj_pool) < 4: return None 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 + downstream 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 top-level `bimanual_transform.right_base_to_left_base` (== T_left_in_right) derived from the first camera that yielded both arm solves. """ 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(K, dist, T, 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).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(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(h["K"], h["dist"], r["T_cam_in_arm2_base"], "left") cams[name] = _mk(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"] 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)) print(f" [exo_auto] wrote {out_path} — " f"{len(result)} cam(s), bimanual_transform=" f"{'yes' if 'bimanual_transform' in cal else 'no'}")