547 lines
19 KiB
Python
547 lines
19 KiB
Python
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())
|