import argparse import json import time from pathlib import Path from collections import deque from typing import Dict, Optional, Tuple import cv2 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 Depth Probe - API v3 style # ------------------------------------------------------------ # Este script evita XLinkOut/getOutputQueue, porque seu ambiente DepthAI # nao expoe dai.node.XLinkOut. Ele usa createOutputQueue() direto nas saidas. # # Exemplo: # python -m utils.depth_probe --left CAM_B --right CAM_C --rgb CAM_A --enable-rgb --lrcheck --extended --subpixel --confidence 200 --median 7 # # Se depth/disparity parecer invertido ou muito ruim: # python -m utils.depth_probe --left CAM_C --right CAM_B --rgb CAM_A --enable-rgb --lrcheck --extended --subpixel # ============================================================ # ============================================================ # DepthAI helpers # ============================================================ def socket_from_name(name: str): name = str(name).strip().upper() aliases = { "A": "CAM_A", "B": "CAM_B", "C": "CAM_C", "LEFT": "CAM_B", "RIGHT": "CAM_C", "RGB": "CAM_A", } 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), } if legacy.get(name) is not None: return legacy[name] raise ValueError(f"Socket invalido: {name}. Use CAM_A, CAM_B ou CAM_C.") def create_node(pipeline: dai.Pipeline, node_type): """Wrapper pequeno para manter o codigo legivel.""" return pipeline.create(node_type) def mono_resolution_from_name(name: str): name = str(name).strip().lower() r = dai.MonoCameraProperties.SensorResolution table = { "400p": getattr(r, "THE_400_P", None), "480p": getattr(r, "THE_480_P", None), "720p": getattr(r, "THE_720_P", None), "800p": getattr(r, "THE_800_P", None), } if name not in table or table[name] is None: valid = ", ".join(k for k, v in table.items() if v is not None) raise ValueError(f"Resolucao mono invalida: {name}. Valid={valid}") return table[name] def median_filter_from_name(name: str): name = str(name).strip().upper() enum_candidates = [] if hasattr(dai, "MedianFilter"): enum_candidates.append(dai.MedianFilter) if hasattr(dai, "StereoDepthProperties") and hasattr(dai.StereoDepthProperties, "MedianFilter"): enum_candidates.append(dai.StereoDepthProperties.MedianFilter) key_map = { "OFF": ("MEDIAN_OFF", "KERNEL_NONE", "OFF"), "3": ("KERNEL_3x3", "MEDIAN_3x3"), "5": ("KERNEL_5x5", "MEDIAN_5x5"), "7": ("KERNEL_7x7", "MEDIAN_7x7"), } if name not in key_map: raise ValueError("--median deve ser OFF, 3, 5 ou 7") for enum in enum_candidates: for attr in key_map[name]: value = getattr(enum, attr, None) if value is not None: return value print("[WARN] Esta versao do DepthAI nao expos enum de MedianFilter; seguindo sem aplicar median filter.") return None def set_if_exists(obj, method_name: str, *args) -> bool: fn = getattr(obj, method_name, None) if callable(fn): try: fn(*args) return True except Exception as e: print(f"[WARN] {method_name} falhou: {e}") return False def apply_stereo_config(stereo, args): # Preset: tenta alguns nomes comuns. try: preset = getattr(dai.node.StereoDepth.PresetMode, "HIGH_DENSITY", None) if preset is None: preset = getattr(dai.node.StereoDepth.PresetMode, "FAST_DENSITY", None) if preset is not None: stereo.setDefaultProfilePreset(preset) except Exception as e: print(f"[WARN] preset StereoDepth nao aplicado: {e}") set_if_exists(stereo, "setLeftRightCheck", bool(args.lrcheck)) set_if_exists(stereo, "setExtendedDisparity", bool(args.extended)) set_if_exists(stereo, "setSubpixel", bool(args.subpixel)) # Confidence threshold: mudou bastante entre versoes. applied_conf = False applied_conf = set_if_exists(stereo, "setConfidenceThreshold", int(args.confidence)) or applied_conf if not applied_conf: try: applied_conf = set_if_exists(stereo.initialConfig, "setConfidenceThreshold", int(args.confidence)) or applied_conf except Exception: pass # Algumas APIs v3 nao tem initialConfig.get(); tentamos manipular config direto se existir. try: cfg = stereo.initialConfig if hasattr(cfg, "costMatching") and hasattr(cfg.costMatching, "confidenceThreshold"): cfg.costMatching.confidenceThreshold = int(args.confidence) applied_conf = True except Exception: pass if not applied_conf: print("[WARN] Nao consegui aplicar confidenceThreshold nesta versao. Seguindo com default.") median_value = median_filter_from_name(args.median) if median_value is not None: applied_median = False try: applied_median = set_if_exists(stereo.initialConfig, "setMedianFilter", median_value) except Exception: pass if not applied_median: try: cfg = stereo.initialConfig if hasattr(cfg, "postProcessing") and hasattr(cfg.postProcessing, "median"): cfg.postProcessing.median = median_value applied_median = True except Exception: pass if not applied_median: print("[WARN] Nao consegui aplicar median filter nesta versao. Seguindo com default.") # Pos-processamento opcional. Tudo defensivo. try: cfg = stereo.initialConfig pp = getattr(cfg, "postProcessing", None) if pp is not None: if hasattr(pp, "speckleFilter"): pp.speckleFilter.enable = bool(args.speckle) pp.speckleFilter.speckleRange = int(args.speckle_range) if hasattr(pp, "temporalFilter"): pp.temporalFilter.enable = bool(args.temporal) if hasattr(pp, "spatialFilter"): pp.spatialFilter.enable = bool(args.spatial) if hasattr(pp.spatialFilter, "holeFillingRadius"): pp.spatialFilter.holeFillingRadius = int(args.hole_filling_radius) if hasattr(pp.spatialFilter, "numIterations"): pp.spatialFilter.numIterations = int(args.spatial_iterations) except Exception as e: print(f"[WARN] Nao consegui aplicar filtros de pos-processamento: {e}") # ============================================================ # Visual helpers # ============================================================ def normalize_u8(arr: np.ndarray, p_low: float = 1.0, p_high: float = 99.0) -> np.ndarray: x = np.asarray(arr, dtype=np.float32) finite = np.isfinite(x) if not np.any(finite): return np.zeros(x.shape[:2], dtype=np.uint8) vals = x[finite] lo = float(np.percentile(vals, p_low)) hi = float(np.percentile(vals, p_high)) if hi <= lo + 1e-6: hi = lo + 1.0 y = np.clip((x - lo) / (hi - lo), 0.0, 1.0) return (y * 255).astype(np.uint8) def heatmap(arr: np.ndarray, p_low: float = 1.0, p_high: float = 99.0, cmap=cv2.COLORMAP_TURBO) -> np.ndarray: return cv2.applyColorMap(normalize_u8(arr, p_low, p_high), cmap) def put_label(img: np.ndarray, title: str, subtitle: str = "") -> np.ndarray: if img is None: img = np.zeros((300, 400, 3), dtype=np.uint8) if img.ndim == 2: img = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR) out = img.copy() hbox = 58 if subtitle else 36 cv2.rectangle(out, (0, 0), (out.shape[1], hbox), (0, 0, 0), -1) cv2.putText(out, str(title)[:90], (10, 24), cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 255, 255), 2, cv2.LINE_AA) if subtitle: cv2.putText(out, str(subtitle)[:120], (10, 48), cv2.FONT_HERSHEY_SIMPLEX, 0.44, (255, 255, 255), 1, cv2.LINE_AA) return out def resize_keep(img: np.ndarray, width: int) -> np.ndarray: scale = width / img.shape[1] height = max(1, int(img.shape[0] * scale)) return cv2.resize(img, (width, height), interpolation=cv2.INTER_AREA) def make_grid(panels, panel_w: int = 430, cols: int = 3) -> np.ndarray: rendered = [] for title, img, subtitle in panels: if img is None: img = np.zeros((300, 400, 3), dtype=np.uint8) if img.ndim == 2: img = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR) small = resize_keep(img, panel_w) rendered.append(put_label(small, title, subtitle)) if not rendered: return np.zeros((300, 600, 3), dtype=np.uint8) max_h = max(x.shape[0] for x in rendered) padded = [] for im in rendered: if im.shape[0] < max_h: im = np.vstack([im, np.zeros((max_h - im.shape[0], im.shape[1], 3), dtype=np.uint8)]) padded.append(im) gap = 10 gap_w = np.full((max_h, gap, 3), 22, dtype=np.uint8) filler = np.zeros_like(padded[0]) rows = [] for i in range(0, len(padded), cols): items = padded[i:i + cols] while len(items) < cols: items.append(filler.copy()) row = items[0] for j in range(1, cols): row = np.hstack([row, gap_w, items[j]]) rows.append(row) gap_h = np.full((gap, rows[0].shape[1], 3), 22, dtype=np.uint8) canvas = rows[0] for row in rows[1:]: canvas = np.vstack([canvas, gap_h, row]) return canvas def safe_stats_depth_mm(depth: np.ndarray, min_mm: int, max_mm: int) -> Dict[str, float]: d = np.asarray(depth, dtype=np.float32) valid = np.isfinite(d) & (d > min_mm) & (d < max_mm) total = int(d.size) count = int(np.count_nonzero(valid)) if count <= 0: return { "valid_pct": 0.0, "count": 0, "mean_mm": 0.0, "median_mm": 0.0, "p10_mm": 0.0, "p90_mm": 0.0, "std_mm": 0.0, } vals = d[valid] return { "valid_pct": float(count * 100.0 / max(1, total)), "count": count, "mean_mm": float(np.mean(vals)), "median_mm": float(np.median(vals)), "p10_mm": float(np.percentile(vals, 10)), "p90_mm": float(np.percentile(vals, 90)), "std_mm": float(np.std(vals)), } def stats_disparity(disp: np.ndarray) -> Dict[str, float]: d = np.asarray(disp, dtype=np.float32) valid = np.isfinite(d) & (d > 0) total = int(d.size) count = int(np.count_nonzero(valid)) if count <= 0: return {"valid_pct": 0.0, "mean": 0.0, "median": 0.0, "p90": 0.0, "std": 0.0} vals = d[valid] return { "valid_pct": float(count * 100.0 / max(1, total)), "mean": float(np.mean(vals)), "median": float(np.median(vals)), "p90": float(np.percentile(vals, 90)), "std": float(np.std(vals)), } def draw_metrics_panel(metrics: Dict[str, float], disp_stats: Dict[str, float], fps: float, args: argparse.Namespace, size: Tuple[int, int] = (900, 260)) -> np.ndarray: w, h = size img = np.zeros((h, w, 3), dtype=np.uint8) lines = [ "OAK-FCC-3P depth probe - API v3 queues", f"left={args.left} right={args.right} rgb={args.rgb} | fps={fps:.1f}", f"lrcheck={args.lrcheck} extended={args.extended} subpixel={args.subpixel} median={args.median} confidence={args.confidence}", f"depth valid={metrics['valid_pct']:.1f}% | median={metrics['median_mm']:.0f}mm mean={metrics['mean_mm']:.0f}mm p10={metrics['p10_mm']:.0f} p90={metrics['p90_mm']:.0f} std={metrics['std_mm']:.0f}", f"disp valid={disp_stats['valid_pct']:.1f}% | median={disp_stats['median']:.2f} mean={disp_stats['mean']:.2f} p90={disp_stats['p90']:.2f} std={disp_stats['std']:.2f}", "teclas: Q/ESC sair | S salvar snapshot | H ajuda", "Leitura: heatmap coerente + valid% alto = vale investigar depth. Ruido/sopa = descartar depth metrico.", ] y = 28 for i, line in enumerate(lines): color = (0, 255, 255) if i == 0 else (235, 235, 235) cv2.putText(img, line[:145], (12, y), cv2.FONT_HERSHEY_SIMPLEX, 0.55, color, 1, cv2.LINE_AA) y += 26 return img # ============================================================ # Queue helpers # ============================================================ def create_output_queue(output, name: str, max_size: int = 4, blocking: bool = False): if output is None: return None fn = getattr(output, "createOutputQueue", None) if callable(fn): return fn(maxSize=max_size, blocking=blocking) raise RuntimeError( f"A saida '{name}' nao possui createOutputQueue(). " "Seu DepthAI parece nao ter XLinkOut, mas tambem nao expos queues v3 nessa saida." ) def get_frame(q) -> Optional[np.ndarray]: if q is None: return None try: msg = q.tryGet() except Exception: return None if msg is None: return None # ImgFrame normalmente tem getFrame(). Alguns previews coloridos podem ter getCvFrame(). try: return msg.getFrame() except Exception: pass try: return msg.getCvFrame() except Exception: return None # ============================================================ # Pipeline # ============================================================ def create_pipeline_and_outputs(args: argparse.Namespace): pipeline = dai.Pipeline() left = create_node(pipeline, dai.node.MonoCamera) right = create_node(pipeline, dai.node.MonoCamera) left.setBoardSocket(socket_from_name(args.left)) right.setBoardSocket(socket_from_name(args.right)) left.setResolution(mono_resolution_from_name(args.mono_resolution)) right.setResolution(mono_resolution_from_name(args.mono_resolution)) left.setFps(float(args.fps)) right.setFps(float(args.fps)) stereo = create_node(pipeline, dai.node.StereoDepth) apply_stereo_config(stereo, args) left.out.link(stereo.left) right.out.link(stereo.right) outputs = { "left": left.out, "right": right.out, "disparity": stereo.disparity, "depth": stereo.depth, "rectified_left": stereo.rectifiedLeft, "rectified_right": stereo.rectifiedRight, } nodes = { "left": left, "right": right, "stereo": stereo, } if args.enable_rgb: rgb = create_node(pipeline, dai.node.ColorCamera) rgb.setBoardSocket(socket_from_name(args.rgb)) rgb.setResolution(dai.ColorCameraProperties.SensorResolution.THE_800_P) rgb.setFps(float(args.fps)) rgb.setInterleaved(False) rgb.setColorOrder(dai.ColorCameraProperties.ColorOrder.BGR) rgb.setPreviewSize(int(args.rgb_preview_w), int(args.rgb_preview_h)) outputs["rgb"] = rgb.preview nodes["rgb"] = rgb return pipeline, outputs, nodes # ============================================================ # Runtime # ============================================================ def save_snapshot(out_dir: Path, frames: Dict[str, np.ndarray], metrics: Dict[str, float], disp_stats: Dict[str, float], args: argparse.Namespace): ts = time.strftime("%Y%m%d_%H%M%S") folder = out_dir / f"depth_probe_{ts}" folder.mkdir(parents=True, exist_ok=True) for name, frame in frames.items(): if frame is None: continue if frame.ndim == 2: if frame.dtype == np.uint16: np.save(str(folder / f"{name}.npy"), frame) cv2.imwrite(str(folder / f"{name}_preview.png"), normalize_u8(frame)) else: cv2.imwrite(str(folder / f"{name}.png"), normalize_u8(frame)) else: cv2.imwrite(str(folder / f"{name}.png"), frame) meta = { "created_at": ts, "args": vars(args), "depth_metrics": metrics, "disparity_metrics": disp_stats, } with open(folder / "metrics.json", "w", encoding="utf-8") as f: json.dump(meta, f, ensure_ascii=False, indent=2) print(f"[OK] snapshot salvo em: {folder}") def start_pipeline_v3(pipeline): fn = getattr(pipeline, "start", None) if not callable(fn): raise RuntimeError( "Este ambiente nao tem pipeline.start(). " "Tambem nao tinha XLinkOut. Pode ser uma build DepthAI intermediaria/incompleta." ) fn() def stop_pipeline_v3(pipeline): try: fn = getattr(pipeline, "stop", None) if callable(fn): fn() except Exception: pass def pipeline_running(pipeline) -> bool: fn = getattr(pipeline, "isRunning", None) if callable(fn): try: return bool(fn()) except Exception: return True return True def main(args: argparse.Namespace): out_dir = Path(args.out_dir) out_dir.mkdir(parents=True, exist_ok=True) pipeline, outputs, _nodes = create_pipeline_and_outputs(args) print("[INFO] Pipeline criado em modo API v3/sem XLinkOut.") print(f"[INFO] left={args.left} right={args.right} rgb={args.rgb} enable_rgb={args.enable_rgb}") print("[INFO] Se depth vier ruim, teste invertendo --left/--right.") queues = { name: create_output_queue(output, name, max_size=4, blocking=False) for name, output in outputs.items() } start_pipeline_v3(pipeline) cv2.namedWindow("OAK-FCC-3P Depth Probe", cv2.WINDOW_NORMAL) cv2.resizeWindow("OAK-FCC-3P Depth Probe", 1500, 900) last_frames: Dict[str, Optional[np.ndarray]] = { "left": None, "right": None, "rectified_left": None, "rectified_right": None, "disparity": None, "depth": None, "rgb": None, "canvas": None, } frame_times = deque(maxlen=40) last_metrics = safe_stats_depth_mm(np.zeros((1, 1), dtype=np.uint16), args.min_depth_mm, args.max_depth_mm) last_disp_stats = stats_disparity(np.zeros((1, 1), dtype=np.float32)) try: while pipeline_running(pipeline): updated = False for name, queue in queues.items(): frame = get_frame(queue) if frame is not None: last_frames[name] = frame updated = True if not updated: key = cv2.waitKey(1) & 0xFF if key in (27, ord("q"), ord("Q")): break continue if last_frames["disparity"] is not None: frame_times.append(time.time()) if len(frame_times) >= 2: fps = (len(frame_times) - 1) / max(1e-6, frame_times[-1] - frame_times[0]) else: fps = 0.0 left = last_frames["left"] right = last_frames["right"] rect_left = last_frames["rectified_left"] rect_right = last_frames["rectified_right"] disp = last_frames["disparity"] depth = last_frames["depth"] rgb = last_frames["rgb"] if disp is None or depth is None or left is None or right is None: continue metrics = safe_stats_depth_mm(depth, args.min_depth_mm, args.max_depth_mm) disp_s = stats_disparity(disp) last_metrics = metrics last_disp_stats = disp_s depth_f = depth.astype(np.float32) depth_valid = np.where( (depth_f > args.min_depth_mm) & (depth_f < args.max_depth_mm), depth_f, np.nan, ) disp_hm = heatmap(disp, 1, 99, cv2.COLORMAP_TURBO) depth_hm = heatmap(depth_valid, 1, 99, cv2.COLORMAP_TURBO) valid_mask = np.where(np.isfinite(depth_valid), 255, 0).astype(np.uint8) valid_bgr = cv2.cvtColor(valid_mask, cv2.COLOR_GRAY2BGR) base_for_overlay = rect_left if rect_left is not None else left base_bgr = cv2.cvtColor(normalize_u8(base_for_overlay), cv2.COLOR_GRAY2BGR) depth_hm_res = cv2.resize(depth_hm, (base_bgr.shape[1], base_bgr.shape[0]), interpolation=cv2.INTER_AREA) overlay = cv2.addWeighted(base_bgr, 0.55, depth_hm_res, 0.45, 0) panels = [ ("Left mono", normalize_u8(left), f"{args.left}"), ("Right mono", normalize_u8(right), f"{args.right}"), ("Metrics", draw_metrics_panel(metrics, disp_s, fps, args), ""), ("Rectified left", normalize_u8(rect_left), "stereo.rectifiedLeft"), ("Rectified right", normalize_u8(rect_right), "stereo.rectifiedRight"), ("Disparity heatmap", disp_hm, f"valid={disp_s['valid_pct']:.1f}%"), ("Depth heatmap", depth_hm, f"valid={metrics['valid_pct']:.1f}% median={metrics['median_mm']:.0f}mm"), ("Valid depth mask", valid_bgr, f"range={args.min_depth_mm}-{args.max_depth_mm}mm"), ("Depth overlay", overlay, "heatmap sobre rectified left"), ] if rgb is not None: panels.append(("RGB preview", rgb, f"{args.rgb}")) canvas = make_grid(panels, panel_w=args.panel_w, cols=3) last_frames["canvas"] = canvas cv2.imshow("OAK-FCC-3P Depth Probe", canvas) key = cv2.waitKey(1) & 0xFF if key in (27, ord("q"), ord("Q")): break if key in (ord("s"), ord("S")): frames_to_save = {k: v for k, v in last_frames.items() if v is not None} save_snapshot(out_dir, frames_to_save, last_metrics, last_disp_stats, args) if key in (ord("h"), ord("H")): print("\n=== HELP ===") print("Q/ESC : sair") print("S : salvar snapshot") print("Teste tambem invertendo --left/--right se disparity/depth parecer quebrado.") print("===========\n") finally: stop_pipeline_v3(pipeline) cv2.destroyAllWindows() # ============================================================ # CLI # ============================================================ def build_argparser() -> argparse.ArgumentParser: ap = argparse.ArgumentParser(description="Teste de depth/disparity na OAK-FFC-3P usando par mono RE/NIR, sem XLinkOut.") ap.add_argument("--left", type=str, default="CAM_B", help="Socket mono esquerda. Ex: CAM_B ou CAM_C") ap.add_argument("--right", type=str, default="CAM_C", help="Socket mono direita. Ex: CAM_C ou CAM_B") ap.add_argument("--rgb", type=str, default="CAM_A", help="Socket RGB opcional.") ap.add_argument("--enable-rgb", action="store_true", help="Tambem mostra preview RGB.") ap.add_argument("--mono-resolution", type=str, default="800p", choices=["400p", "480p", "720p", "800p"]) ap.add_argument("--fps", type=float, default=10.0) ap.add_argument("--rgb-preview-w", type=int, default=640) ap.add_argument("--rgb-preview-h", type=int, default=400) ap.add_argument("--lrcheck", action="store_true", help="Ativa left-right check para remover matches ruins/oclusoes.") ap.add_argument("--extended", action="store_true", help="Ativa extended disparity, util para curto alcance.") ap.add_argument("--subpixel", action="store_true", help="Ativa subpixel disparity, util para suavidade/maior precisao.") ap.add_argument("--confidence", type=int, default=200, help="Confidence threshold do StereoDepth. Tente 180-245.") ap.add_argument("--median", type=str, default="7", choices=["OFF", "3", "5", "7"], help="Filtro de mediana.") ap.add_argument("--speckle", action="store_true", help="Ativa speckle filter no post-processing.") ap.add_argument("--speckle-range", type=int, default=50) ap.add_argument("--temporal", action="store_true", help="Ativa temporal filter, se suportado pela versao.") ap.add_argument("--spatial", action="store_true", help="Ativa spatial filter, se suportado pela versao.") ap.add_argument("--hole-filling-radius", type=int, default=2) ap.add_argument("--spatial-iterations", type=int, default=1) ap.add_argument("--min-depth-mm", type=int, default=150) ap.add_argument("--max-depth-mm", type=int, default=5000) ap.add_argument("--panel-w", type=int, default=430) ap.add_argument("--out-dir", type=str, default="depth_probe_out") return ap if __name__ == "__main__": main(build_argparser().parse_args())