import argparse import json import math import time from pathlib import Path from typing import Any, Dict, List, Optional, Tuple import numpy as np try: import depthai as dai except Exception as e: raise RuntimeError( "Nao consegui importar depthai. Ative o venv correto e instale depthai antes de rodar. " f"Erro original: {e}" ) # ============================================================ # OAK-FCC-3P Calibration Probe # ------------------------------------------------------------ # Objetivo: # Ler o que existe de calibracao no device OAK/DepthAI: # - cameras conectadas # - sockets / sensores # - intrinsecos por camera, quando disponivel # - distorcao por camera, quando disponivel # - extrinsecos entre pares CAM_A/CAM_B/CAM_C # - baseline estimado, quando a API permitir # - dump JSON bruto da calibracao, quando disponivel # # Uso: # python -m utils.calibration_probe --out_dir calibration_probe_out # # Para escolher device por MXID: # python -m utils.calibration_probe --mx_id 194430108133AC2F00 # ============================================================ # ============================================================ # Helpers gerais # ============================================================ def to_jsonable(x: Any): if x is None: return None if isinstance(x, (str, int, float, bool)): if isinstance(x, float) and (math.isnan(x) or math.isinf(x)): return None return x if isinstance(x, np.ndarray): return x.tolist() if isinstance(x, (list, tuple)): return [to_jsonable(v) for v in x] if isinstance(x, dict): return {str(k): to_jsonable(v) for k, v in x.items()} try: return str(x) except Exception: return repr(x) def safe_call(label: str, fn, *args, default=None, verbose: bool = False): try: return fn(*args) except Exception as e: if verbose: print(f"[WARN] {label} falhou: {type(e).__name__}: {e}") return default def get_device_id_from_info(dev_info) -> Optional[str]: for name in ("getMxId", "getDeviceId"): try: fn = getattr(dev_info, name, None) if callable(fn): value = fn() if value: return str(value) except Exception: pass for attr in ("mxid", "deviceId", "name"): try: value = getattr(dev_info, attr, None) if value: return str(value) except Exception: pass return None def resolve_device_info(mx_id: Optional[str] = None): devices = dai.Device.getAllAvailableDevices() if not devices: raise RuntimeError("Nenhum dispositivo DepthAI/OAK encontrado.") if not mx_id: return devices[0] target = str(mx_id).strip() for dev_info in devices: dev_id = get_device_id_from_info(dev_info) if dev_id == target: return dev_info available = [get_device_id_from_info(d) or str(d) for d in devices] raise RuntimeError(f"Device mx_id='{target}' nao encontrado. Disponiveis={available}") def socket_from_name(name: str): name = str(name).strip().upper() aliases = { "A": "CAM_A", "B": "CAM_B", "C": "CAM_C", "D": "CAM_D", "RGB": "CAM_A", "LEFT": "CAM_B", "RIGHT": "CAM_C", } name = aliases.get(name, name) if hasattr(dai.CameraBoardSocket, name): return getattr(dai.CameraBoardSocket, name) legacy = { "CAM_A": getattr(dai.CameraBoardSocket, "RGB", None), "CAM_B": getattr(dai.CameraBoardSocket, "LEFT", None), "CAM_C": getattr(dai.CameraBoardSocket, "RIGHT", None), "CAM_D": getattr(dai.CameraBoardSocket, "CAM_D", None), } if legacy.get(name) is not None: return legacy[name] raise ValueError(f"Socket invalido: {name}") def socket_name(socket_obj) -> str: try: return str(socket_obj.name) except Exception: return str(socket_obj) def matrix_shape_ok(m, rows: int, cols: int) -> bool: try: arr = np.asarray(m, dtype=np.float64) return arr.shape == (rows, cols) and np.all(np.isfinite(arr)) except Exception: return False def flatten_matrix(m): try: return np.asarray(m, dtype=np.float64).tolist() except Exception: return to_jsonable(m) # ============================================================ # Calibration read helpers # ============================================================ def read_calibration(device, verbose: bool = False): # Contratos comuns: readCalibration(), readCalibration2(). for method in ("readCalibration", "readCalibration2"): fn = getattr(device, method, None) if callable(fn): calib = safe_call(method, fn, default=None, verbose=verbose) if calib is not None: print(f"[OK] Calibracao lida via device.{method}()") return calib, method raise RuntimeError("Nao encontrei device.readCalibration/readCalibration2 nesta versao do DepthAI.") def dump_calibration_json(calib, out_dir: Path, verbose: bool = False) -> Dict[str, Any]: """Tenta extrair dump bruto da calibracao por varios contratos de API.""" result = { "available": False, "method": None, "path": None, "data": None, "error": None, } # 1) eepromToJson() costuma devolver dict/json. for method in ("eepromToJson", "toJson"): fn = getattr(calib, method, None) if callable(fn): try: data = fn() if isinstance(data, str): try: data_obj = json.loads(data) except Exception: data_obj = data else: data_obj = data path = out_dir / f"calibration_{method}.json" with open(path, "w", encoding="utf-8") as f: json.dump(to_jsonable(data_obj), f, ensure_ascii=False, indent=2) result.update({"available": True, "method": method, "path": str(path), "data": to_jsonable(data_obj)}) print(f"[OK] Dump bruto salvo via calib.{method}(): {path}") return result except Exception as e: result["error"] = f"{method}: {type(e).__name__}: {e}" if verbose: print(f"[WARN] dump {method} falhou: {e}") # 2) Alguns handlers escrevem direto em arquivo. for method in ("saveToJsonFile", "saveCalibrationFile", "saveToFile"): fn = getattr(calib, method, None) if callable(fn): path = out_dir / f"calibration_{method}.json" try: fn(str(path)) result.update({"available": True, "method": method, "path": str(path), "data": None}) print(f"[OK] Dump bruto salvo via calib.{method}(): {path}") return result except Exception as e: result["error"] = f"{method}: {type(e).__name__}: {e}" if verbose: print(f"[WARN] dump {method} falhou: {e}") print("[WARN] Nao consegui gerar dump JSON bruto da calibracao por API conhecida.") return result def get_connected_cameras(device) -> List[Dict[str, Any]]: features = safe_call("getConnectedCameraFeatures", device.getConnectedCameraFeatures, default=[], verbose=False) out = [] for f in features: item = {} try: item["socket"] = socket_name(f.socket) except Exception: item["socket"] = None for attr in ("sensorName", "width", "height", "orientation", "supportedTypes"): try: v = getattr(f, attr, None) item[attr] = to_jsonable(v) except Exception: pass out.append(item) return out def get_intrinsics(calib, socket, width: int, height: int, verbose: bool = False): # Contratos comuns: # getCameraIntrinsics(socket) # getCameraIntrinsics(socket, width, height) fn = getattr(calib, "getCameraIntrinsics", None) if not callable(fn): return None, "missing:getCameraIntrinsics" for args in ((socket, width, height), (socket,)): try: value = fn(*args) if value is not None: return flatten_matrix(value), f"getCameraIntrinsics{len(args)}args" except Exception as e: if verbose: print(f"[WARN] intrinsics {socket_name(socket)} args={len(args)} falhou: {e}") return None, "failed:getCameraIntrinsics" def get_distortion(calib, socket, verbose: bool = False): for method in ("getDistortionCoefficients", "getDistortionCoeff"): fn = getattr(calib, method, None) if callable(fn): try: value = fn(socket) return to_jsonable(value), method except Exception as e: if verbose: print(f"[WARN] distortion {socket_name(socket)} {method} falhou: {e}") return None, "missing:distortion" def get_fov(calib, socket, verbose: bool = False): fn = getattr(calib, "getFov", None) if callable(fn): try: return float(fn(socket)), "getFov" except Exception as e: if verbose: print(f"[WARN] fov {socket_name(socket)} falhou: {e}") return None, "missing:getFov" def get_extrinsics(calib, src_socket, dst_socket, verbose: bool = False): fn = getattr(calib, "getCameraExtrinsics", None) if not callable(fn): return None, "missing:getCameraExtrinsics" # Contratos comuns: # getCameraExtrinsics(src, dst) # getCameraExtrinsics(src, dst, useSpecTranslation) for args in ((src_socket, dst_socket), (src_socket, dst_socket, False), (src_socket, dst_socket, True)): try: value = fn(*args) if value is not None: return flatten_matrix(value), f"getCameraExtrinsics{len(args)}args" except Exception as e: if verbose: print(f"[WARN] extrinsics {socket_name(src_socket)}->{socket_name(dst_socket)} args={len(args)} falhou: {e}") return None, "failed:getCameraExtrinsics" def get_baseline(calib, src_socket, dst_socket, verbose: bool = False): # Varia entre versoes; em algumas, getBaselineDistance(cam1, cam2, useSpecTranslation) fn = getattr(calib, "getBaselineDistance", None) if callable(fn): for args in ((src_socket, dst_socket), (src_socket, dst_socket, False), (src_socket, dst_socket, True)): try: value = fn(*args) if value is not None: return float(value), f"getBaselineDistance{len(args)}args" except Exception as e: if verbose: print(f"[WARN] baseline {socket_name(src_socket)}-{socket_name(dst_socket)} args={len(args)} falhou: {e}") # Fallback: calcula norma da translacao da matriz 4x4, se existir. ext, method = get_extrinsics(calib, src_socket, dst_socket, verbose=False) if ext is not None: try: arr = np.asarray(ext, dtype=np.float64) if arr.shape == (4, 4): t = arr[:3, 3] return float(np.linalg.norm(t)), f"norm_translation_from_{method}" except Exception: pass return None, "missing:getBaselineDistance" def inspect_socket(calib, socket_name_str: str, width: int, height: int, verbose: bool = False) -> Dict[str, Any]: socket = socket_from_name(socket_name_str) intr, intr_method = get_intrinsics(calib, socket, width, height, verbose=verbose) dist, dist_method = get_distortion(calib, socket, verbose=verbose) fov, fov_method = get_fov(calib, socket, verbose=verbose) intr_ok = matrix_shape_ok(intr, 3, 3) dist_ok = dist is not None return { "socket": socket_name_str, "intrinsics": intr, "intrinsics_method": intr_method, "intrinsics_ok": bool(intr_ok), "distortion": dist, "distortion_method": dist_method, "distortion_ok": bool(dist_ok), "fov_deg": fov, "fov_method": fov_method, } def inspect_pair(calib, src_name: str, dst_name: str, verbose: bool = False) -> Dict[str, Any]: src = socket_from_name(src_name) dst = socket_from_name(dst_name) ext, ext_method = get_extrinsics(calib, src, dst, verbose=verbose) base, base_method = get_baseline(calib, src, dst, verbose=verbose) ext_ok = matrix_shape_ok(ext, 4, 4) translation = None if ext_ok: try: arr = np.asarray(ext, dtype=np.float64) translation = arr[:3, 3].tolist() except Exception: translation = None return { "pair": f"{src_name}->{dst_name}", "src": src_name, "dst": dst_name, "extrinsics": ext, "extrinsics_method": ext_method, "extrinsics_ok": bool(ext_ok), "translation": translation, "baseline": base, "baseline_method": base_method, "baseline_ok": bool(base is not None), } # ============================================================ # Report # ============================================================ def print_summary(report: Dict[str, Any]): print("\n================ CALIBRATION PROBE SUMMARY ================") print(f"device_id : {report.get('device_id')}") print(f"read_method : {report.get('calibration_read_method')}") print(f"dump_json : {report.get('calibration_dump', {}).get('path')}") print("\n[Cameras conectadas]") for cam in report.get("connected_cameras", []): print(f" - socket={cam.get('socket')} sensor={cam.get('sensorName')} size={cam.get('width')}x{cam.get('height')}") print("\n[Intrinsecos por socket]") for s in report.get("sockets", []): print( f" - {s['socket']}: intrinsics_ok={s['intrinsics_ok']} " f"distortion_ok={s['distortion_ok']} fov={s.get('fov_deg')}" ) print("\n[Extrinsecos entre pares]") for p in report.get("pairs", []): flag = "OK" if p.get("extrinsics_ok") else "MISSING" print( f" - {p['pair']}: {flag} | baseline={p.get('baseline')} " f"| method={p.get('extrinsics_method')}" ) # Diagnostico direto para o caso de depth RE/NIR. bc = next((p for p in report.get("pairs", []) if p.get("pair") == "CAM_B->CAM_C"), None) cb = next((p for p in report.get("pairs", []) if p.get("pair") == "CAM_C->CAM_B"), None) print("\n[Diagnostico CAM_B/CAM_C para StereoDepth]") if (bc and bc.get("extrinsics_ok")) or (cb and cb.get("extrinsics_ok")): print(" ✅ Existe extrinseco entre CAM_B e CAM_C. O StereoDepth deve ter chance de iniciar.") else: print(" ❌ Nao existe extrinseco CAM_B<->CAM_C legivel pela API.") print(" Isso explica erro: 'There is no available extrinsic calibration between camera ID: 1 and 2'.") print(" Proximo passo: calibrar o par CAM_B/CAM_C ou carregar um calibration.json valido.") print("===========================================================\n") # ============================================================ # Main # ============================================================ def main(args: argparse.Namespace): out_dir = Path(args.out_dir) out_dir.mkdir(parents=True, exist_ok=True) dev_info = resolve_device_info(args.mx_id) device_id = get_device_id_from_info(dev_info) print(f"[INFO] Abrindo device: {device_id}") with dai.Device(dev_info) as device: connected = get_connected_cameras(device) calib, read_method = read_calibration(device, verbose=args.verbose) dump = dump_calibration_json(calib, out_dir=out_dir, verbose=args.verbose) sockets = [s.strip().upper() for s in args.sockets.split(",") if s.strip()] pairs = [] socket_reports = [] for s in sockets: try: socket_reports.append(inspect_socket(calib, s, args.width, args.height, verbose=args.verbose)) except Exception as e: socket_reports.append({ "socket": s, "error": f"{type(e).__name__}: {e}", "intrinsics_ok": False, "distortion_ok": False, }) for src in sockets: for dst in sockets: if src == dst: continue try: pairs.append(inspect_pair(calib, src, dst, verbose=args.verbose)) except Exception as e: pairs.append({ "pair": f"{src}->{dst}", "src": src, "dst": dst, "error": f"{type(e).__name__}: {e}", "extrinsics_ok": False, "baseline_ok": False, }) report = { "created_at": time.strftime("%Y-%m-%d %H:%M:%S"), "device_id": device_id, "calibration_read_method": read_method, "connected_cameras": connected, "sockets_requested": sockets, "width": int(args.width), "height": int(args.height), "calibration_dump": { "available": dump.get("available"), "method": dump.get("method"), "path": dump.get("path"), "error": dump.get("error"), }, "sockets": socket_reports, "pairs": pairs, } out_path = out_dir / "calibration_probe_report.json" with open(out_path, "w", encoding="utf-8") as f: json.dump(to_jsonable(report), f, ensure_ascii=False, indent=2) print(f"[OK] Relatorio salvo em: {out_path}") print_summary(report) # ============================================================ # CLI # ============================================================ def build_argparser() -> argparse.ArgumentParser: ap = argparse.ArgumentParser(description="Inspeciona calibracao EEPROM/JSON do device OAK/DepthAI.") ap.add_argument("--mx_id", type=str, default=None, help="MXID opcional do device.") ap.add_argument("--out_dir", type=str, default="calibration_probe_out", help="Pasta de saida.") ap.add_argument("--sockets", type=str, default="CAM_A,CAM_B,CAM_C", help="Sockets para testar. Ex: CAM_A,CAM_B,CAM_C") ap.add_argument("--width", type=int, default=1280, help="Largura usada ao pedir intrinsecos escalados.") ap.add_argument("--height", type=int, default=800, help="Altura usada ao pedir intrinsecos escalados.") ap.add_argument("--verbose", action="store_true", help="Mostra warnings detalhados de APIs que falharam.") return ap if __name__ == "__main__": main(build_argparser().parse_args())