"""Sanity check for the auto-cal pipeline — grab 10 frames from each scene-role camera at HD1080, run ArUco detect + per-arm pooled PnP, report detection counts + residuals. Does NOT touch teleop / CAN / robot control — just cameras + detection. Runs on russet. Uses base raiden's Camera API + a monkey-patched ZED resolution (HD1080). Uses raiden_internal for the shared PnP helpers (``_solve_multiframe_pose``, ``exo_utils.do_est_aruco_pose``) plus the russet v2-reverse patch. Usage (on russet, via ssh): timeout 60 python3 /tmp/sanity_cal_check.py Kill via ^C or wait for the timeout; the script exits cleanly and closes every camera. """ from __future__ import annotations # MUJOCO_GL / PYOPENGL_PLATFORM — set BEFORE any module that imports mujoco. # fidex helpers indirectly touch mujoco; setting these here is defensive. import os os.environ.setdefault("MUJOCO_GL", "egl") os.environ.setdefault("PYOPENGL_PLATFORM", "egl") import copy import sys import time import traceback from pathlib import Path import cv2 import numpy as np # --------------------------------------------------------------------- # Wire raiden_internal on the path so we can import the fidex helpers. # --------------------------------------------------------------------- RAIDEN_INTERNAL = Path("/home/robot-lab/raiden_internal") RAIDEN_BASE = Path("/home/robot-lab/raiden") EXO_REDO = RAIDEN_INTERNAL / "third_party" / "exo_redo" for p in (str(RAIDEN_INTERNAL), str(RAIDEN_BASE), str(EXO_REDO)): if p not in sys.path: sys.path.insert(0, p) # --------------------------------------------------------------------- # Monkey-patch ZED default resolution to HD1080 before base raiden loads. # --------------------------------------------------------------------- def _patch_zed_hd1080(): """Base raiden's ZED camera hardcodes HD720. Patch the class's ``open`` to inject HD1080. Only affects THIS process.""" import pyzed.sl as sl from raiden.cameras.zed import ZedCamera _original_open = ZedCamera.open def _hd1080_open(self, enable_depth: bool = False) -> None: # Monkey-patch: temporarily patch sl.RESOLUTION.HD720 replacement is # too invasive; simplest is to re-implement open() with HD1080. init_params = sl.InitParameters() init_params.set_from_serial_number(self._serial) init_params.camera_resolution = sl.RESOLUTION.HD1080 # HD1080 caps at 30 fps; ZED SDK downshifts silently otherwise. init_params.camera_fps = 15 init_params.depth_mode = ( sl.DEPTH_MODE.NEURAL_LIGHT if enable_depth else sl.DEPTH_MODE.NONE) init_params.coordinate_units = sl.UNIT.METER if enable_depth: init_params.depth_minimum_distance = 0.1 status = self._camera.open(init_params) if status != sl.ERROR_CODE.SUCCESS: raise RuntimeError( f"Failed to open ZED '{self._name}' (serial={self._serial}): " f"{status}") self._is_open = True self._has_depth = enable_depth # Force the image buffer to HD1080 dims — the base __init__ sized it # for HD720; if we leave it, retrieve_image dumps into a smaller # buffer and get_data() gives 720p color. self._image = sl.Mat() ZedCamera.open = _hd1080_open return _original_open # --------------------------------------------------------------------- # Apply russet v2-reverse arm-2 patch (idempotent). # --------------------------------------------------------------------- def _apply_russet_patch(): from ExoConfigs.yam_exo import ( YAM_BASE_ONLY_CONFIG, YAM_BASE_ONLY_CONFIG_ARM2) STL = str(EXO_REDO / "so100_blender_testings" / "yam_base_board_v2_reverse_clearance_fixed.stl") exo_a1 = YAM_BASE_ONLY_CONFIG exo_a2 = copy.deepcopy(YAM_BASE_ONLY_CONFIG_ARM2) for link_cfg in exo_a2.links.values(): if link_cfg.exo_mesh_path == STL: continue 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) print(f" ✓ russet v2-reverse patch applied to arm-2 config") return exo_a1, exo_a2 # --------------------------------------------------------------------- # ArUco detect + per-arm partition + per-arm pooled solve. # --------------------------------------------------------------------- _ARM1_IDS = frozenset(range(217, 217 + 9)) _ARM2_IDS = frozenset(range(226, 226 + 9)) def _detect_and_split(bgr): 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()) if ids_all is None: return None, None, 0 ids_flat = ids_all.flatten() total = len(ids_flat) 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), total class _History: def __init__(self, max_size=30): self.max_size = max_size self.entries = [] def add(self, obj_pts, img_pts, resid): self.entries.append((obj_pts, img_pts, float(resid))) if len(self.entries) > self.max_size: worst = max(range(len(self.entries)), key=lambda i: self.entries[i][2]) self.entries.pop(worst) def top_k(self, k=10): if not self.entries: return None, None, 0, float("nan") top = sorted(self.entries, key=lambda e: e[2])[:k] obj = np.concatenate([e[0] for e in top], axis=0) img = np.concatenate([e[1] for e in top], axis=0) return obj, img, len(top), float(np.mean([e[2] for e in top])) def _solve_arm(bgr, corners_est, board, board_length, T_aruco_to_link, K, dist, hist: _History, top_k=10): from exo_utils import do_est_aruco_pose, ARUCO_DICT if corners_est is None: return None try: r = do_est_aruco_pose(bgr, ARUCO_DICT, board, board_length, cameraMatrix=K, distCoeffs=dist, corners_est=corners_est) except Exception: return None if r == -1: return None rvec, tvec_shifted = r["rtvec"] R = cv2.Rodrigues(rvec)[0] off = np.array([board_length / 2, board_length / 2, 0]) tvec_corner = tvec_shifted - R @ off obj_cam, img_pts = r["obj_img_pts"] obj_board = (R.T @ (obj_cam.T - tvec_corner.reshape(3, 1))).T proj, _ = cv2.projectPoints( obj_board.astype(np.float32), rvec, tvec_corner, np.asarray(r["cameraMatrix"], np.float64), (np.asarray(r["distCoeffs"], np.float64).reshape(-1) if r.get("distCoeffs") is not None else dist)) resid = float(np.linalg.norm( proj.reshape(-1, 2) - img_pts.reshape(-1, 2), axis=1).mean()) hist.add(obj_board, img_pts, resid) obj_pool, img_pool, n_used, _ = hist.top_k(k=top_k) T_a_c = r["est_aruco_pose"] multi = float("nan") if obj_pool is not None and len(obj_pool) >= 4: from raiden.calibration.exo_calibrate_fidex import _solve_multiframe_pose mf, mr = _solve_multiframe_pose(obj_pool, img_pool, K, dist, board_length) if mf is not None: T_a_c = mf multi = mr T_link_in_cam = T_a_c @ T_aruco_to_link return { "T_cam_in_arm_base": np.linalg.inv(T_link_in_cam), "single_resid_px": resid, "multi_resid_px": multi, "n_used": n_used, } # --------------------------------------------------------------------- # Main sanity check. # --------------------------------------------------------------------- def main(): n_frames = 10 _patch_zed_hd1080() from raiden.recorder import load_cameras_from_config from raiden.camera_config import CameraConfig from raiden._config import CAMERA_CONFIG cfg = CameraConfig(CAMERA_CONFIG) all_names = cfg.list_camera_names() scene_names = [n for n in all_names if cfg._camera_role(n) == "scene"] if hasattr(cfg, "_camera_role") \ else [n for n in all_names if "scene" in n or "ego" in n] print(f"[sanity] scene-role cams: {scene_names}") print("[sanity] opening cameras (HD1080 patched)...") cams = load_cameras_from_config(CAMERA_CONFIG) # cams is a list; convert to dict keyed by name for our purposes. cams_by_name = dict(zip(all_names, cams)) exo_a1, exo_a2 = _apply_russet_patch() from ExoConfigs.exoskeleton import link_to_aruco_transform link1 = exo_a1.links["larger_coarse_board"] link2 = exo_a2.links["larger_coarse_board_arm2"] board1 = exo_a1.aruco_board_objects["larger_coarse_board"] board2 = exo_a2.aruco_board_objects["larger_coarse_board_arm2"] bl1 = link1.board_length bl2 = link2.board_length Ta2l1 = np.linalg.inv(link_to_aruco_transform(link1)) Ta2l2 = np.linalg.inv(link_to_aruco_transform(link2)) print(f"[sanity] board lengths: arm1={bl1*1000:.1f}mm, arm2={bl2*1000:.1f}mm") per_cam = {} for name in scene_names: if name not in cams_by_name: print(f" ⚠ {name} not in cams — skipping") continue cam = cams_by_name[name] K, dist, img_size = cam.get_intrinsics() print(f" cam {name}: K diag=({K[0,0]:.1f},{K[1,1]:.1f}), " f"size={img_size}") per_cam[name] = { "cam": cam, "K": K, "dist": dist, "img_size": img_size, "hist1": _History(30), "hist2": _History(30), "per_frame": [], } print(f"[sanity] grabbing {n_frames} frames per cam...") frame_stats = [] for i in range(n_frames): for name, s in per_cam.items(): if not s["cam"].grab(): s["per_frame"].append({"frame": i, "grab": False}) continue frame = s["cam"].get_frame() bgr = frame.color if bgr is None: s["per_frame"].append({"frame": i, "grab": True, "color": False}) continue c1, c2, n_ids = _detect_and_split(bgr) r1 = _solve_arm(bgr, c1, board1, bl1, Ta2l1, s["K"], s["dist"], s["hist1"]) r2 = _solve_arm(bgr, c2, board2, bl2, Ta2l2, s["K"], s["dist"], s["hist2"]) n_arm1 = 0 if c1 is None else len(c1[1]) n_arm2 = 0 if c2 is None else len(c2[1]) row = { "frame": i, "shape": bgr.shape, "n_ids": n_ids, "n_arm1_markers": n_arm1, "n_arm2_markers": n_arm2, "arm1_single_resid_px": (r1["single_resid_px"] if r1 else float("nan")), "arm1_multi_resid_px": (r1["multi_resid_px"] if r1 else float("nan")), "arm1_n_used": (r1["n_used"] if r1 else 0), "arm2_single_resid_px": (r2["single_resid_px"] if r2 else float("nan")), "arm2_multi_resid_px": (r2["multi_resid_px"] if r2 else float("nan")), "arm2_n_used": (r2["n_used"] if r2 else 0), } s["per_frame"].append(row) frame_stats.append((name, row)) time.sleep(0.03) # ~30 fps polling; camera runs at 15 fps print("\n[sanity] per-frame detection results:") print(f"{'cam':>16} {'frame':>5} {'ids':>3} {'a1':>2}/{'a2':>2} " f"{'a1_multi_resid':>14} {'a2_multi_resid':>14} " f"{'a1_pool':>7} {'a2_pool':>7}") for name, row in frame_stats: print(f"{name:>16} {row['frame']:>5} {row['n_ids']:>3} " f"{row['n_arm1_markers']:>2}/{row['n_arm2_markers']:>2} " f"{row['arm1_multi_resid_px']:>14.3f} " f"{row['arm2_multi_resid_px']:>14.3f} " f"{row['arm1_n_used']:>7} {row['arm2_n_used']:>7}") print("\n[sanity] terminal per-cam pooled solve:") from raiden.calibration.exo_calibrate_fidex import _solve_multiframe_pose for name, s in per_cam.items(): for arm_id, hist, board, bl, Ta2l in [ ("arm1", s["hist1"], board1, bl1, Ta2l1), ("arm2", s["hist2"], board2, bl2, Ta2l2)]: obj_p, img_p, n_used, mean_resid = hist.top_k(k=10) if obj_p is None or len(obj_p) < 4: print(f" [{name}/{arm_id}] no pooled solve — n_frames_with_det={len(hist.entries)}") continue T_a_c, resid = _solve_multiframe_pose(obj_p, img_p, s["K"], s["dist"], bl) if T_a_c is None: print(f" [{name}/{arm_id}] final pool solve failed") continue T_link = T_a_c @ Ta2l T_cam_in_base = np.linalg.inv(T_link) t = T_cam_in_base[:3, 3] print(f" [{name}/{arm_id}] n_used={n_used:2d} " f"final_pooled_resid={resid:.3f}px " f"T_cam_in_base_t=({t[0]:+.3f},{t[1]:+.3f},{t[2]:+.3f}) m") print("\n[sanity] closing cameras...") for name, s in per_cam.items(): try: s["cam"].close() except Exception: pass print("[sanity] done.") if __name__ == "__main__": try: main() except Exception: traceback.print_exc() sys.exit(1)