#!/usr/bin/env python3 # -*- coding: utf-8 -*- """ Focus Calibration Tool - OAK-FFC-3P - Sensor Aware ==================================================== Objetivo -------- Ferramenta standalone para: - detectar automaticamente os sensores conectados; - suportar CAM_A com OV9782 (1280x800) OU AR0234 (1920x1200); - visualizar RGB + RE + NIR simultaneamente; - medir foco manual por Laplacian / Tenengrad / Brenner; - permitir ROI por câmera; - salvar melhores scores e snapshots em JSON/PNG. Importante ---------- Este script NÃO usa o OakFcc3Client e NÃO depende do pipeline RAW_BRUTO. Para foco óptico ele usa: - saída ISP da câmera colorida (RGB); - saída nativa das câmeras mono (RE/NIR). Assim, Bayer pattern e normalização do dataset não interferem no teste de foco. Configuração padrão esperada: CAM_A = RGB -> OV9782 ou AR0234 CAM_B = RE -> OV9282 CAM_C = NIR -> OV9282 Exemplos -------- # Detecta e abre tudo que estiver disponível: python focus_calibration_sensor_aware.py # Primeiro teste somente da RGB nova: python focus_calibration_sensor_aware.py --mode rgb # Exige as três câmeras: python focus_calibration_sensor_aware.py --mode triple # Se RE e NIR estiverem fisicamente invertidas: python focus_calibration_sensor_aware.py --re-socket CAM_C --nir-socket CAM_B Teclas ------ 1 = RGB 2 = RE 3 = NIR M = troca métrica D = informa sentido atual da lente E = equalização ON/OFF L = trava/destrava ROI C = centraliza ROI R = reseta score da câmera/métrica ativa S = salva snapshot PNG + registro SPACE = salva JSON completo Q / ESC = sair """ import os import json import time import argparse from dataclasses import dataclass, asdict from datetime import datetime from collections import deque from typing import Dict, Optional, Tuple, List import cv2 import numpy as np import depthai as dai # ============================================================ # Helpers gerais # ============================================================ METHODS = ("laplacian", "tenengrad", "brenner") ROLES = ("rgb", "re", "nir") def now_str() -> str: return datetime.now().strftime("%Y-%m-%d %H:%M:%S") def now_file_str() -> str: return datetime.now().strftime("%Y%m%d_%H%M%S") def ensure_dir(path: str): if path: os.makedirs(path, exist_ok=True) def overlay_hud( img_bgr, lines, x=12, y=24, font_scale=0.58, line_step=22, color=(255, 255, 255), shadow=True, ): yy = y h, _ = img_bgr.shape[:2] for s in lines: if yy > h - 8: break text = str(s) if shadow: cv2.putText( img_bgr, text, (x, yy), cv2.FONT_HERSHEY_SIMPLEX, font_scale, (0, 0, 0), 3, cv2.LINE_AA, ) cv2.putText( img_bgr, text, (x, yy), cv2.FONT_HERSHEY_SIMPLEX, font_scale, color, 1, cv2.LINE_AA, ) yy += line_step def build_empty_panel(shape_hw: Tuple[int, int], title: str, text="sem frame disponivel"): h, w = shape_hw img = np.zeros((h, w, 3), dtype=np.uint8) overlay_hud( img, [title, text], x=18, y=44, font_scale=0.75, line_step=32, ) return img def socket_name(socket) -> str: name = getattr(socket, "name", None) if name: return str(name) s = str(socket) for candidate in ("CAM_A", "CAM_B", "CAM_C", "CAM_D"): if candidate in s: return candidate return s def get_socket(name: str): name = str(name).upper().strip() mapping = { "CAM_A": dai.CameraBoardSocket.CAM_A, "CAM_B": dai.CameraBoardSocket.CAM_B, "CAM_C": dai.CameraBoardSocket.CAM_C, } if hasattr(dai.CameraBoardSocket, "CAM_D"): mapping["CAM_D"] = dai.CameraBoardSocket.CAM_D if name not in mapping: raise ValueError(f"Socket inválido: {name}. Opções: {sorted(mapping)}") return mapping[name] def supported_type_strings(feature) -> List[str]: values = getattr(feature, "supportedTypes", []) or [] return [str(v).upper() for v in values] def feature_is_color(feature) -> bool: types = supported_type_strings(feature) sensor = str(getattr(feature, "sensorName", "") or "").upper() if any("COLOR" in t for t in types): return True if any("MONO" in t for t in types): return False # Fallback conhecido do nosso módulo. return sensor in {"OV9782", "AR0234"} def feature_is_mono(feature) -> bool: types = supported_type_strings(feature) if any("MONO" in t for t in types): return True if any("COLOR" in t for t in types): return False return not feature_is_color(feature) def bgr_to_gray01(img_bgr: np.ndarray) -> Optional[np.ndarray]: if img_bgr is None: return None if img_bgr.ndim == 2: gray = img_bgr elif img_bgr.ndim == 3 and img_bgr.shape[2] == 1: gray = img_bgr[:, :, 0] else: gray = cv2.cvtColor(img_bgr, cv2.COLOR_BGR2GRAY) gray = gray.astype(np.float32) / 255.0 return np.clip(gray, 0.0, 1.0) def resize_panel(img: np.ndarray, target_hw: Tuple[int, int]) -> np.ndarray: th, tw = target_hw return cv2.resize(img, (tw, th), interpolation=cv2.INTER_AREA) def colorize_mono_for_view(img_bgr: np.ndarray, role: str) -> np.ndarray: if img_bgr is None: return None if img_bgr.ndim == 3: gray = cv2.cvtColor(img_bgr, cv2.COLOR_BGR2GRAY) else: gray = img_bgr z = np.zeros_like(gray) role = str(role).lower() # Apenas visual. As métricas são calculadas no frame original. if role == "re": # Vermelho no BGR. return np.dstack([z, z, gray]) if role == "nir": # Ciano no BGR. return np.dstack([gray, gray, z]) return cv2.cvtColor(gray, cv2.COLOR_GRAY2BGR) def default_roi_for_shape(shape_hw, frac=0.42): h, w = shape_hw rw = max(8, int(w * frac)) rh = max(8, int(h * frac)) x0 = (w - rw) // 2 y0 = (h - rh) // 2 return (x0, y0, x0 + rw, y0 + rh) def sanitize_roi(rect, shape_hw): if rect is None: return None h, w = shape_hw x0, y0, x1, y1 = rect x0, x1 = sorted((int(x0), int(x1))) y0, y1 = sorted((int(y0), int(y1))) x0 = max(0, min(w - 1, x0)) x1 = max(1, min(w, x1)) y0 = max(0, min(h - 1, y0)) y1 = max(1, min(h, y1)) if x1 - x0 < 4 or y1 - y0 < 4: return None return (x0, y0, x1, y1) def crop_rect(img: np.ndarray, rect): if img is None or rect is None: return None rect = sanitize_roi(rect, img.shape[:2]) if rect is None: return None x0, y0, x1, y1 = rect return img[y0:y1, x0:x1] # ============================================================ # Descoberta / resolução sensor-aware # ============================================================ @dataclass class SensorSpec: role: str socket_name: str sensor_name: str feature_width: int feature_height: int is_color: bool configured_width: int configured_height: int resolution_name: str stream_name: str source: str def discover_cameras(mx_id: Optional[str]): device_info = dai.DeviceInfo(mx_id) if mx_id else None if device_info is not None: ctx = dai.Device(device_info) else: ctx = dai.Device() with ctx as device: features = list(device.getConnectedCameraFeatures()) actual_mx = None for attr in ("getMxId", "getDeviceId"): if hasattr(device, attr): try: actual_mx = str(getattr(device, attr)()) if actual_mx: break except Exception: pass usb_speed = None try: usb_speed = str(device.getUsbSpeed()) except Exception: pass rows = [] for f in features: rows.append({ "socket_obj": f.socket, "socket_name": socket_name(f.socket), "sensor_name": str(getattr(f, "sensorName", "") or ""), "width": int(getattr(f, "width", 0) or 0), "height": int(getattr(f, "height", 0) or 0), "supported_types": supported_type_strings(f), "is_color": feature_is_color(f), "is_mono": feature_is_mono(f), "has_autofocus_ic": int(getattr(f, "hasAutofocusIC", 0) or 0), }) return rows, actual_mx, usb_speed def enum_if_exists(enum_cls, name: str): return getattr(enum_cls, name, None) def pick_color_resolution(sensor_name: str, width: int, height: int): """ Retorna: (enum_resolution, resolution_name, configured_width, configured_height) """ sensor = str(sensor_name or "").upper() enum_cls = dai.ColorCameraProperties.SensorResolution if "AR0234" in sensor: enum_value = enum_if_exists(enum_cls, "THE_1200_P") if enum_value is None: raise RuntimeError( "Seu depthai não possui ColorCameraProperties.SensorResolution.THE_1200_P. " "Atualize a biblioteca DepthAI antes de testar a AR0234." ) return enum_value, "THE_1200_P", 1920, 1200 if "OV9782" in sensor: enum_value = enum_if_exists(enum_cls, "THE_800_P") if enum_value is None: raise RuntimeError("DepthAI sem THE_800_P para ColorCamera.") return enum_value, "THE_800_P", 1280, 800 # Fallback por resolução anunciada pelo próprio sensor. candidates = [ ((1920, 1200), "THE_1200_P"), ((1280, 800), "THE_800_P"), ((1920, 1080), "THE_1080_P"), ((1280, 720), "THE_720_P"), ((3840, 2160), "THE_4_K"), ] for (w, h), enum_name in candidates: if (width, height) == (w, h): enum_value = enum_if_exists(enum_cls, enum_name) if enum_value is not None: return enum_value, enum_name, w, h raise RuntimeError( f"Sensor colorido não mapeado: {sensor_name} ({width}x{height}). " "Adicione o modo em pick_color_resolution()." ) def pick_mono_resolution(sensor_name: str, width: int, height: int): sensor = str(sensor_name or "").upper() enum_cls = dai.MonoCameraProperties.SensorResolution if "OV9282" in sensor or (width, height) == (1280, 800): enum_value = enum_if_exists(enum_cls, "THE_800_P") if enum_value is None: raise RuntimeError("DepthAI sem THE_800_P para MonoCamera.") return enum_value, "THE_800_P", 1280, 800 candidates = [ ((1280, 800), "THE_800_P"), ((1280, 720), "THE_720_P"), ((640, 480), "THE_480_P"), ((640, 400), "THE_400_P"), ] for (w, h), enum_name in candidates: if (width, height) == (w, h): enum_value = enum_if_exists(enum_cls, enum_name) if enum_value is not None: return enum_value, enum_name, w, h raise RuntimeError( f"Sensor mono não mapeado: {sensor_name} ({width}x{height}). " "Adicione o modo em pick_mono_resolution()." ) def build_specs(camera_rows, args) -> Dict[str, SensorSpec]: by_socket = {row["socket_name"]: row for row in camera_rows} role_socket = { "rgb": args.rgb_socket.upper(), "re": args.re_socket.upper(), "nir": args.nir_socket.upper(), } if args.mode == "rgb": requested_roles = ["rgb"] else: requested_roles = ["rgb", "re", "nir"] specs = {} for role in requested_roles: sock_name = role_socket[role] row = by_socket.get(sock_name) if row is None: if args.mode == "triple": raise RuntimeError( f"Modo triple exige {role.upper()} em {sock_name}, " f"mas esse socket não foi detectado." ) continue if role == "rgb": if not row["is_color"]: raise RuntimeError( f"{sock_name} foi escolhido como RGB, mas o sensor detectado " f"({row['sensor_name']}) não foi anunciado como COLOR." ) _, res_name, cw, ch = pick_color_resolution( row["sensor_name"], row["width"], row["height"] ) specs[role] = SensorSpec( role=role, socket_name=sock_name, sensor_name=row["sensor_name"], feature_width=row["width"], feature_height=row["height"], is_color=True, configured_width=cw, configured_height=ch, resolution_name=res_name, stream_name="focus_rgb", source="ISP", ) else: if not row["is_mono"]: raise RuntimeError( f"{sock_name} foi escolhido como {role.upper()}, mas o sensor detectado " f"({row['sensor_name']}) não foi anunciado como MONO." ) _, res_name, cw, ch = pick_mono_resolution( row["sensor_name"], row["width"], row["height"] ) specs[role] = SensorSpec( role=role, socket_name=sock_name, sensor_name=row["sensor_name"], feature_width=row["width"], feature_height=row["height"], is_color=False, configured_width=cw, configured_height=ch, resolution_name=res_name, stream_name=f"focus_{role}", source="MONO_OUT", ) if "rgb" not in specs: raise RuntimeError( f"RGB não encontrada em {args.rgb_socket}. " "Confira os flats e/ou use --rgb-socket." ) if args.mode == "triple": missing = [r for r in ROLES if r not in specs] if missing: raise RuntimeError(f"Modo triple: faltando roles {missing}") return specs def build_pipeline(specs: Dict[str, SensorSpec], fps: float): pipeline = dai.Pipeline() for role, spec in specs.items(): socket = get_socket(spec.socket_name) if spec.is_color: cam = pipeline.createColorCamera() cam.setBoardSocket(socket) enum_value, _, _, _ = pick_color_resolution( spec.sensor_name, spec.feature_width, spec.feature_height, ) cam.setResolution(enum_value) cam.setFps(float(fps)) cam.setInterleaved(False) try: cam.setColorOrder(dai.ColorCameraProperties.ColorOrder.BGR) except Exception: pass xout = pipeline.createXLinkOut() xout.setStreamName(spec.stream_name) # ISP mantém a resolução útil do sensor e getCvFrame() entrega # uma imagem pronta para inspeção, sem depender do nosso Bayer. cam.isp.link(xout.input) else: cam = pipeline.createMonoCamera() cam.setBoardSocket(socket) enum_value, _, _, _ = pick_mono_resolution( spec.sensor_name, spec.feature_width, spec.feature_height, ) cam.setResolution(enum_value) cam.setFps(float(fps)) xout = pipeline.createXLinkOut() xout.setStreamName(spec.stream_name) cam.out.link(xout.input) return pipeline # ============================================================ # Métricas de foco # ============================================================ def preprocess_focus_gray(gray01: np.ndarray, equalize=False): g = np.clip(gray01 * 255.0, 0, 255).astype(np.uint8) if equalize: g = cv2.equalizeHist(g) return g def focus_laplacian_var(gray_u8: np.ndarray) -> float: lap = cv2.Laplacian(gray_u8, cv2.CV_64F, ksize=3) return float(lap.var()) def focus_tenengrad(gray_u8: np.ndarray) -> float: sx = cv2.Sobel(gray_u8, cv2.CV_64F, 1, 0, ksize=3) sy = cv2.Sobel(gray_u8, cv2.CV_64F, 0, 1, ksize=3) return float(np.mean(sx * sx + sy * sy)) def focus_brenner(gray_u8: np.ndarray) -> float: arr = gray_u8.astype(np.float32) if arr.shape[1] < 3: return 0.0 diff = arr[:, 2:] - arr[:, :-2] return float(np.mean(diff * diff)) def compute_focus_metrics(img_bgr: np.ndarray, roi_rect, equalize=False): gray01 = bgr_to_gray01(img_bgr) roi = crop_rect(gray01, roi_rect) if roi is None or roi.size < 64: return { "valid": False, "laplacian": 0.0, "tenengrad": 0.0, "brenner": 0.0, "mean": 0.0, "std": 0.0, "p95": 0.0, "pct_saturated": 0.0, "pct_dark": 0.0, "pixels": 0, } gray_u8 = preprocess_focus_gray(roi, equalize=equalize) arr = roi.astype(np.float32).reshape(-1) return { "valid": True, "laplacian": focus_laplacian_var(gray_u8), "tenengrad": focus_tenengrad(gray_u8), "brenner": focus_brenner(gray_u8), "mean": float(arr.mean()), "std": float(arr.std()), "p95": float(np.percentile(arr, 95)), "pct_saturated": float((arr >= 0.98).mean() * 100.0), "pct_dark": float((arr <= 0.02).mean() * 100.0), "pixels": int(arr.size), } def metric_value(metrics: dict, method: str) -> float: return float(metrics.get(method, 0.0) or 0.0) def smooth_from_history(history, window: int) -> float: if not history: return 0.0 vals = [float(x["score"]) for x in list(history)[-max(1, window):]] return float(np.mean(vals)) def analyze_trend(history, best_score, direction_name: str, drop_warn_pct=3.0): if len(history) < 6: return { "status": "coletando", "instruction": "gire devagar e observe o grafico", "delta": 0.0, "pct_of_best": 0.0, } recent = [float(x["smooth"]) for x in list(history)[-5:]] if len(history) >= 12: old = [float(x["smooth"]) for x in list(history)[-12:-7]] else: old = [float(x["smooth"]) for x in list(history)[:5]] recent_mean = float(np.mean(recent)) old_mean = float(np.mean(old)) delta = recent_mean - old_mean pct_of_best = 0.0 if best_score <= 0 else (recent_mean / best_score) * 100.0 drop_from_best = 100.0 - pct_of_best if best_score > 0 and drop_from_best >= drop_warn_pct: return { "status": "passou_do_pico", "instruction": f"volte um pouco: contrario de {direction_name}", "delta": delta, "pct_of_best": pct_of_best, } eps = max(best_score * 0.002, 1e-6) if delta > eps: return { "status": "melhorando", "instruction": f"continue {direction_name}", "delta": delta, "pct_of_best": pct_of_best, } if delta < -eps: return { "status": "piorando", "instruction": f"inverta: contrario de {direction_name}", "delta": delta, "pct_of_best": pct_of_best, } return { "status": "estavel", "instruction": "ajuste fino ou trave a lente", "delta": delta, "pct_of_best": pct_of_best, } # ============================================================ # Desenho # ============================================================ def draw_roi(panel, roi_src, src_shape_hw, active=False): if roi_src is None: return ph, pw = panel.shape[:2] sh, sw = src_shape_hw x0, y0, x1, y1 = roi_src px0 = int(x0 * pw / max(1, sw)) px1 = int(x1 * pw / max(1, sw)) py0 = int(y0 * ph / max(1, sh)) py1 = int(y1 * ph / max(1, sh)) color = (0, 255, 255) if active else (0, 180, 255) cv2.rectangle(panel, (px0, py0), (px1, py1), color, 2) cv2.putText( panel, "FOCUS ROI", (px0 + 6, max(20, py0 - 8)), cv2.FONT_HERSHEY_SIMPLEX, 0.55, color, 2, cv2.LINE_AA, ) def draw_crosshair(panel): h, w = panel.shape[:2] cv2.line( panel, (w // 2 - 18, h // 2), (w // 2 + 18, h // 2), (255, 255, 255), 1, cv2.LINE_AA, ) cv2.line( panel, (w // 2, h // 2 - 18), (w // 2, h // 2 + 18), (255, 255, 255), 1, cv2.LINE_AA, ) def draw_panel_title(panel, title, selected=False): color = (0, 255, 255) if selected else (255, 255, 255) overlay_hud( panel, [title], x=12, y=24, font_scale=0.60, line_step=24, color=color, ) def draw_score_bar(panel, pct, x, y, w, h, label): pct = float(max(0.0, min(100.0, pct))) cv2.rectangle(panel, (x, y), (x + w, y + h), (80, 80, 80), 1) fill_w = int((pct / 100.0) * w) cv2.rectangle(panel, (x, y), (x + fill_w, y + h), (230, 230, 230), -1) cv2.rectangle(panel, (x, y), (x + w, y + h), (180, 180, 180), 1) cv2.putText( panel, f"{label}: {pct:5.1f}%", (x, y - 8), cv2.FONT_HERSHEY_SIMPLEX, 0.50, (255, 255, 255), 1, cv2.LINE_AA, ) def draw_history_graph(panel, history, x, y, w, h, best_score): cv2.rectangle(panel, (x, y), (x + w, y + h), (35, 35, 35), -1) cv2.rectangle(panel, (x, y), (x + w, y + h), (120, 120, 120), 1) if len(history) < 2: cv2.putText( panel, "grafico aguardando historico...", (x + 10, y + h // 2), cv2.FONT_HERSHEY_SIMPLEX, 0.50, (180, 180, 180), 1, cv2.LINE_AA, ) return vals = np.array([float(item["smooth"]) for item in history], dtype=np.float32) vals = vals[-max(2, w):] max_val = max(float(np.max(vals)), float(best_score), 1e-6) min_val = min(float(np.min(vals)), max_val * 0.90) span = max(max_val - min_val, 1e-6) pts = [] for i, value in enumerate(vals): px = x + int((i / max(1, len(vals) - 1)) * (w - 1)) py = y + h - 1 - int(((float(value) - min_val) / span) * (h - 1)) pts.append((px, py)) for p0, p1 in zip(pts[:-1], pts[1:]): cv2.line(panel, p0, p1, (255, 255, 255), 2, cv2.LINE_AA) if best_score > 0: by = y + h - 1 - int(((best_score - min_val) / span) * (h - 1)) by = max(y, min(y + h - 1, by)) cv2.line(panel, (x, by), (x + w, by), (0, 255, 255), 1, cv2.LINE_AA) cv2.putText( panel, "best", (x + 6, max(y + 16, by - 4)), cv2.FONT_HERSHEY_SIMPLEX, 0.43, (0, 255, 255), 1, cv2.LINE_AA, ) def make_data_panel( shape_hw, spec: SensorSpec, selected_role, method, metrics, score, smooth, best, trend, fps_by_role, fps_view, direction_name, history, roi_locked, equalize, ): h, w = shape_hw panel = np.zeros((h, w, 3), dtype=np.uint8) best_score = float(best.get("smooth", 0.0) or 0.0) score_pct = 0.0 if best_score <= 0 else (smooth / best_score) * 100.0 lines = [ "FOCUS CALIBRATION - SENSOR AWARE", f"ativa={selected_role.upper()} | metodo={method}", f"sensor={spec.sensor_name} | socket={spec.socket_name}", f"modo={spec.resolution_name} | {spec.configured_width}x{spec.configured_height}", f"fonte={spec.source}", f"score={score:.1f} | smooth={smooth:.1f}", f"best={best_score:.1f} | atual/best={score_pct:.1f}%", f"status={trend.get('status', 'coletando')}", f"acao={trend.get('instruction', 'gire devagar')}", f"sentido={direction_name}", f"fps RGB/RE/NIR={fps_by_role.get('rgb', 0):.1f}/{fps_by_role.get('re', 0):.1f}/{fps_by_role.get('nir', 0):.1f}", f"fps_view={fps_view:.1f}", f"ROI={'travada' if roi_locked else 'editavel'} | equalize={'ON' if equalize else 'OFF'}", ] if metrics and metrics.get("valid"): lines.extend([ "-", f"mean={metrics['mean']:.3f} std={metrics['std']:.3f} p95={metrics['p95']:.3f}", f"sat={metrics['pct_saturated']:.2f}% dark={metrics['pct_dark']:.2f}%", ]) overlay_hud( panel, lines, x=14, y=26, font_scale=0.48, line_step=20, ) bar_y = min(h - 165, 315) bar_y = max(255, bar_y) draw_score_bar( panel, min(100.0, score_pct), 18, bar_y, max(80, w - 36), 22, "nitidez relativa", ) graph_y = bar_y + 46 graph_h = max(65, h - graph_y - 62) draw_history_graph( panel, history, 18, graph_y, max(100, w - 36), graph_h, best_score, ) help_lines = [ "1 RGB | 2 RE | 3 NIR | M metrica | D sentido", "mouse ROI | C centraliza | L trava | E equalize | R reset", "S snapshot PNG | SPACE salva JSON | Q sai", ] overlay_hud( panel, help_lines, x=14, y=h - 48, font_scale=0.40, line_step=16, ) return panel # ============================================================ # Estado / persistência # ============================================================ def empty_best(): return { "score": 0.0, "smooth": 0.0, "metrics": None, "timestamp": None, "roi": None, } def json_safe_best(best): return { "score": float(best.get("score", 0.0) or 0.0), "smooth": float(best.get("smooth", 0.0) or 0.0), "metrics": best.get("metrics"), "timestamp": best.get("timestamp"), "roi": best.get("roi"), } def build_result_payload( args, camera_rows, specs, best_by_role_method, snapshots, actual_mx, usb_speed, ): results = {} for role in ROLES: results[role] = {} for method in METHODS: results[role][method] = json_safe_best(best_by_role_method[role][method]) return { "schema": "oak_ffc_focus_calibration_sensor_aware_v2", "saved_at": now_str(), "depthai_version": getattr(dai, "__version__", "unknown"), "device_mx_id": actual_mx, "usb_speed": usb_speed, "mode": args.mode, "fps_requested": args.fps, "role_socket_map": { "rgb": args.rgb_socket, "re": args.re_socket, "nir": args.nir_socket, }, "camera_inventory": [ {k: v for k, v in row.items() if k != "socket_obj"} for row in camera_rows ], "active_specs": { role: asdict(spec) for role, spec in specs.items() }, "results_by_role_and_method": results, "snapshots": snapshots, "notes": args.notes or "", } def save_json(path, payload): ensure_dir(os.path.dirname(path) or ".") with open(path, "w", encoding="utf-8") as f: json.dump(payload, f, ensure_ascii=False, indent=2) # ============================================================ # Main # ============================================================ def main(): parser = argparse.ArgumentParser( description=( "Focus Calibration Tool standalone para OAK-FFC-3P, " "sensor-aware para OV9782 e AR0234." ), formatter_class=argparse.ArgumentDefaultsHelpFormatter, ) parser.add_argument( "--mode", default="auto", choices=["auto", "rgb", "triple"], help="auto abre o que existir; rgb abre só CAM_A/RGB; triple exige RGB+RE+NIR", ) parser.add_argument("--fps", type=float, default=20.0) parser.add_argument("--mx-id", default=None) parser.add_argument("--rgb-socket", default="CAM_A") parser.add_argument("--re-socket", default="CAM_B") parser.add_argument("--nir-socket", default="CAM_C") parser.add_argument( "--panel-width", type=int, default=640, help="Largura visual de cada quadrante. Métrica continua na resolução nativa.", ) parser.add_argument( "--panel-height", type=int, default=400, help="Altura visual de cada quadrante.", ) parser.add_argument( "--method", default="laplacian", choices=list(METHODS), ) parser.add_argument("--history", type=int, default=260) parser.add_argument("--smooth-window", type=int, default=5) parser.add_argument("--drop-warn-pct", type=float, default=3.0) parser.add_argument("--equalize", action="store_true") parser.add_argument( "--out-json", default="calibration/focus_calibration_sensor_aware.json", ) parser.add_argument( "--snapshot-dir", default="calibration/focus_snapshots", ) parser.add_argument("--notes", default="") args = parser.parse_args() # -------------------------------------------------------- # Descoberta # -------------------------------------------------------- camera_rows, actual_mx, usb_speed = discover_cameras(args.mx_id) print("=" * 74) print("FOCUS CALIBRATION TOOL - SENSOR AWARE") print(f"DepthAI : {getattr(dai, '__version__', 'unknown')}") print(f"MX ID : {actual_mx}") print(f"USB : {usb_speed}") print("-" * 74) for row in camera_rows: print( f"{row['socket_name']:5s} | " f"{row['sensor_name']:12s} | " f"{row['width']}x{row['height']} | " f"types={row['supported_types']} | " f"AF_IC={row['has_autofocus_ic']}" ) specs = build_specs(camera_rows, args) print("-" * 74) for role, spec in specs.items(): print( f"[PIPE] {role.upper():3s} <- {spec.socket_name} " f"{spec.sensor_name} | {spec.resolution_name} " f"{spec.configured_width}x{spec.configured_height} | {spec.source}" ) print("=" * 74) # -------------------------------------------------------- # Pipeline # -------------------------------------------------------- pipeline = build_pipeline(specs, args.fps) device_info = dai.DeviceInfo(actual_mx) if actual_mx else None if device_info is not None: device_ctx = dai.Device(pipeline, device_info) else: device_ctx = dai.Device(pipeline) # -------------------------------------------------------- # Estado da UI # -------------------------------------------------------- selected_role = "rgb" method = args.method equalize = bool(args.equalize) direction_idx = 0 direction_names = ["rosqueando", "desrosqueando"] frames: Dict[str, Optional[np.ndarray]] = {role: None for role in ROLES} roi_rects = {role: None for role in ROLES} history_by_role_method = { role: { m: deque(maxlen=args.history) for m in METHODS } for role in ROLES } best_by_role_method = { role: { m: empty_best() for m in METHODS } for role in ROLES } snapshots = [] fps_by_role = {role: 0.0 for role in ROLES} fps_count = {role: 0 for role in ROLES} fps_t0 = {role: time.time() for role in ROLES} fps_view = 0.0 view_count = 0 view_t0 = time.time() roi_locked = False dragging_roi = False drag_start_src = None panel_rects = { "rgb": None, "re": None, "nir": None, "data": None, } last_board = None last_msg = "" last_msg_t = 0.0 window_name = "Focus Calibration Tool - Sensor Aware" cv2.namedWindow(window_name, cv2.WINDOW_NORMAL) def set_msg(text): nonlocal last_msg, last_msg_t last_msg = str(text) last_msg_t = time.time() def inside(rect, px, py): if rect is None: return False x0, y0, x1, y1 = rect return x0 <= px < x1 and y0 <= py < y1 def display_to_source(role, px, py): rect = panel_rects.get(role) img = frames.get(role) if rect is None or img is None: return None x0, y0, x1, y1 = rect pw = max(1, x1 - x0) ph = max(1, y1 - y0) sh, sw = img.shape[:2] lx = max(0, min(pw - 1, int(px - x0))) ly = max(0, min(ph - 1, int(py - y0))) sx = int(lx * sw / pw) sy = int(ly * sh / ph) sx = max(0, min(sw - 1, sx)) sy = max(0, min(sh - 1, sy)) return sx, sy def on_mouse(event, x, y, flags, param): nonlocal dragging_roi, drag_start_src if roi_locked: return rect = panel_rects.get(selected_role) if rect is None or not inside(rect, x, y): return src_pt = display_to_source(selected_role, x, y) if src_pt is None: return if event == cv2.EVENT_LBUTTONDOWN: dragging_roi = True drag_start_src = src_pt sx, sy = src_pt roi_rects[selected_role] = (sx, sy, sx + 1, sy + 1) elif event == cv2.EVENT_MOUSEMOVE and dragging_roi and drag_start_src: x0, y0 = drag_start_src x1, y1 = src_pt roi_rects[selected_role] = (x0, y0, x1, y1) elif event == cv2.EVENT_LBUTTONUP and dragging_roi and drag_start_src: x0, y0 = drag_start_src x1, y1 = src_pt rect_src = sanitize_roi( (x0, y0, x1, y1), frames[selected_role].shape[:2], ) if rect_src is not None: roi_rects[selected_role] = rect_src set_msg(f"ROI atualizada: {selected_role.upper()}") dragging_roi = False drag_start_src = None cv2.setMouseCallback(window_name, on_mouse) ensure_dir(args.snapshot_dir) # -------------------------------------------------------- # Execução # -------------------------------------------------------- try: with device_ctx as device: queues = { role: device.getOutputQueue( name=spec.stream_name, maxSize=2, blocking=False, ) for role, spec in specs.items() } print("[OK] Pipeline iniciado. Ajuste a lente devagar.") print("[OK] Q/ESC sai | 1/2/3 troca câmera | S salva snapshot") while True: # ------------------------------------------------ # Coleta independente: foco não precisa sync rígido. # ------------------------------------------------ for role, queue in queues.items(): packet = queue.tryGet() if packet is None: continue try: img = packet.getCvFrame() except Exception as exc: set_msg(f"Falha getCvFrame {role}: {exc}") continue if img is None: continue if img.ndim == 2: img = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR) frames[role] = img if roi_rects[role] is None: roi_rects[role] = default_roi_for_shape(img.shape[:2], frac=0.42) fps_count[role] += 1 dt = time.time() - fps_t0[role] if dt >= 1.0: fps_by_role[role] = fps_count[role] / dt fps_count[role] = 0 fps_t0[role] = time.time() # ------------------------------------------------ # Se ativa não existe, cai para a primeira disponível. # ------------------------------------------------ if selected_role not in specs or frames.get(selected_role) is None: available = [ r for r in ROLES if r in specs and frames.get(r) is not None ] if available: selected_role = available[0] active_img = frames.get(selected_role) # ------------------------------------------------ # Métrica # ------------------------------------------------ metrics = None score = 0.0 smooth = 0.0 trend = { "status": "aguardando", "instruction": "aguardando frame", "pct_of_best": 0.0, } history = history_by_role_method[selected_role][method] best = best_by_role_method[selected_role][method] if active_img is not None: if roi_rects[selected_role] is None: roi_rects[selected_role] = default_roi_for_shape( active_img.shape[:2], frac=0.42, ) metrics = compute_focus_metrics( active_img, roi_rects[selected_role], equalize=equalize, ) if metrics.get("valid"): score = metric_value(metrics, method) history.append({ "t": time.time(), "score": score, "smooth": score, }) smooth = smooth_from_history( history, args.smooth_window, ) history[-1]["smooth"] = smooth if smooth > float(best.get("smooth", 0.0) or 0.0): best.update({ "score": float(score), "smooth": float(smooth), "metrics": metrics, "timestamp": now_str(), "roi": list(map(int, roi_rects[selected_role])), }) trend = analyze_trend( history, float(best.get("smooth", 0.0) or 0.0), direction_names[direction_idx], drop_warn_pct=args.drop_warn_pct, ) # ------------------------------------------------ # Painéis # ------------------------------------------------ ph = int(args.panel_height) pw = int(args.panel_width) target_hw = (ph, pw) panels = {} for role in ROLES: img = frames.get(role) spec = specs.get(role) if img is None: if spec is None: panels[role] = build_empty_panel( target_hw, role.upper(), "camera nao ativa neste modo", ) else: panels[role] = build_empty_panel( target_hw, role.upper(), f"aguardando {spec.sensor_name}", ) continue view = img.copy() if role in ("re", "nir"): view = colorize_mono_for_view(view, role) panel = resize_panel(view, target_hw) roi = roi_rects.get(role) if roi is not None: draw_roi( panel, roi, img.shape[:2], active=(role == selected_role), ) draw_crosshair(panel) spec = specs[role] title = ( f"{role.upper()} | {spec.socket_name} | " f"{spec.sensor_name} | {img.shape[1]}x{img.shape[0]}" ) draw_panel_title( panel, title, selected=(role == selected_role), ) panels[role] = panel active_spec = specs.get(selected_role) if active_spec is None: # Fallback apenas defensivo. active_spec = next(iter(specs.values())) data_panel = make_data_panel( target_hw, active_spec, selected_role, method, metrics or {}, score, smooth, best, trend, fps_by_role, fps_view, direction_names[direction_idx], history, roi_locked, equalize, ) # Layout: # RGB | RE # NIR | DATA panel_rects["rgb"] = (0, 0, pw, ph) panel_rects["re"] = (pw, 0, pw * 2, ph) panel_rects["nir"] = (0, ph, pw, ph * 2) panel_rects["data"] = (pw, ph, pw * 2, ph * 2) top = np.hstack([panels["rgb"], panels["re"]]) bottom = np.hstack([panels["nir"], data_panel]) board = np.vstack([top, bottom]) if last_msg and (time.time() - last_msg_t) < 2.5: cv2.putText( board, last_msg, (16, board.shape[0] - 14), cv2.FONT_HERSHEY_SIMPLEX, 0.58, (0, 255, 0), 2, cv2.LINE_AA, ) last_board = board.copy() cv2.imshow(window_name, board) # FPS da UI view_count += 1 dt_view = time.time() - view_t0 if dt_view >= 1.0: fps_view = view_count / dt_view view_count = 0 view_t0 = time.time() # ------------------------------------------------ # Teclas # ------------------------------------------------ k = cv2.waitKey(1) & 0xFF if k in (ord("q"), ord("Q"), 27): break elif k == ord("1"): if "rgb" in specs: selected_role = "rgb" set_msg("Selecionada: RGB") elif k == ord("2"): if "re" in specs: selected_role = "re" set_msg("Selecionada: RE") else: set_msg("RE nao ativa neste modo") elif k == ord("3"): if "nir" in specs: selected_role = "nir" set_msg("Selecionada: NIR") else: set_msg("NIR nao ativa neste modo") elif k in (ord("m"), ord("M")): method = METHODS[(METHODS.index(method) + 1) % len(METHODS)] set_msg(f"Metrica -> {method}") elif k in (ord("d"), ord("D")): direction_idx = 1 - direction_idx set_msg(f"Sentido -> {direction_names[direction_idx]}") elif k in (ord("e"), ord("E")): equalize = not equalize set_msg(f"Equalize -> {'ON' if equalize else 'OFF'}") elif k in (ord("l"), ord("L")): roi_locked = not roi_locked set_msg(f"ROI -> {'travada' if roi_locked else 'editavel'}") elif k in (ord("c"), ord("C")): img = frames.get(selected_role) if img is not None: roi_rects[selected_role] = default_roi_for_shape( img.shape[:2], frac=0.42, ) set_msg(f"ROI centralizada: {selected_role.upper()}") elif k in (ord("r"), ord("R")): history_by_role_method[selected_role][method].clear() best_by_role_method[selected_role][method] = empty_best() set_msg( f"Reset: {selected_role.upper()} / {method}" ) elif k in (ord("s"), ord("S")): stamp = now_file_str() png_path = os.path.join( args.snapshot_dir, f"focus_{selected_role}_{method}_{stamp}.png", ) if last_board is not None: cv2.imwrite(png_path, last_board) snap = { "timestamp": now_str(), "role": selected_role, "method": method, "sensor": asdict(specs[selected_role]), "roi": ( list(map(int, roi_rects[selected_role])) if roi_rects[selected_role] is not None else None ), "current_score": float(score), "current_smooth": float(smooth), "best": json_safe_best( best_by_role_method[selected_role][method] ), "direction_name": direction_names[direction_idx], "equalize": bool(equalize), "png_path": png_path, } snapshots.append(snap) set_msg(f"Snapshot -> {png_path}") elif k == 32: payload = build_result_payload( args, camera_rows, specs, best_by_role_method, snapshots, actual_mx, usb_speed, ) save_json(args.out_json, payload) set_msg(f"JSON salvo -> {args.out_json}") finally: cv2.destroyAllWindows() # Salva automaticamente no fechamento também. try: payload = build_result_payload( args, camera_rows, specs, best_by_role_method, snapshots, actual_mx, usb_speed, ) save_json(args.out_json, payload) print(f"[OK] Resultado final salvo em: {args.out_json}") except Exception as exc: print(f"[WARN] Não foi possível salvar resultado final: {exc}") print("Fim da calibração de foco.") if __name__ == "__main__": main()