import argparse import time from collections import deque from typing import Optional from pathlib import Path 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 Charuco Preview Probe # ------------------------------------------------------------ # Objetivo: # Abrir CAM_A/CAM_B/CAM_C ao vivo para verificar se o Charuco no monitor # ou impresso aparece bem nas tres cameras, principalmente nas mono RE/NIR. # # Exemplo: # python -m utils.charuco_preview_probe --fps 10 # # Teclas: # Q/ESC = sair # S = salvar snapshot # E = alterna detector de bordas # C = alterna contraste auto/normal nas mono # ============================================================ # ============================================================ # DepthAI helpers # ============================================================ def socket_from_name(name: str): name = str(name).strip().upper() aliases = { "A": "CAM_A", "B": "CAM_B", "C": "CAM_C", "RGB": "CAM_A", "RE": "CAM_B", "NIR": "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), } 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 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 create_output_queue(output, name: str, max_size: int = 4, blocking: bool = False): fn = getattr(output, "createOutputQueue", None) if callable(fn): return fn(maxSize=max_size, blocking=blocking) raise RuntimeError(f"A saida '{name}' nao possui createOutputQueue().") 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 try: return msg.getCvFrame() except Exception: pass try: return msg.getFrame() except Exception: return None # ============================================================ # Visual helpers # ============================================================ def normalize_u8(img: np.ndarray, auto: bool = True) -> np.ndarray: if img is None: return np.zeros((300, 400), dtype=np.uint8) arr = np.asarray(img) if arr.ndim == 3: return arr.astype(np.uint8) arr = arr.astype(np.float32) if not auto: if arr.max() <= 1.5: return np.clip(arr * 255.0, 0, 255).astype(np.uint8) return np.clip(arr, 0, 255).astype(np.uint8) finite = np.isfinite(arr) if not np.any(finite): return np.zeros(arr.shape[:2], dtype=np.uint8) vals = arr[finite] lo = float(np.percentile(vals, 1)) hi = float(np.percentile(vals, 99)) if hi <= lo + 1e-6: hi = lo + 1.0 out = np.clip((arr - lo) / (hi - lo), 0, 1) return (out * 255).astype(np.uint8) def edge_view(gray_u8: np.ndarray) -> np.ndarray: if gray_u8.ndim == 3: gray_u8 = cv2.cvtColor(gray_u8, cv2.COLOR_BGR2GRAY) edges = cv2.Canny(gray_u8, 60, 140) return edges def put_label(img: np.ndarray, title: str, subtitle: str = "") -> np.ndarray: 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)[:80], (10, 24), cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 255, 255), 2, cv2.LINE_AA) if subtitle: cv2.putText(out, str(subtitle)[:115], (10, 48), cv2.FONT_HERSHEY_SIMPLEX, 0.43, (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 = 520, cols: int = 3) -> np.ndarray: rendered = [] for title, img, subtitle in panels: if img.ndim == 2: img = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR) small = resize_keep(img, panel_w) rendered.append(put_label(small, title, subtitle)) max_h = max(x.shape[0] for x in rendered) padded = [] for im in rendered: if im.shape[0] < max_h: pad = np.zeros((max_h - im.shape[0], im.shape[1], 3), dtype=np.uint8) im = np.vstack([im, pad]) padded.append(im) gap = 10 gap_w = np.full((max_h, gap, 3), 25, dtype=np.uint8) rows = [] for i in range(0, len(padded), cols): items = padded[i:i + cols] while len(items) < cols: items.append(np.zeros_like(padded[0])) 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), 25, dtype=np.uint8) canvas = rows[0] for row in rows[1:]: canvas = np.vstack([canvas, gap_h, row]) return canvas def stats_line(img: np.ndarray) -> str: if img is None: return "sem frame" arr = np.asarray(img, dtype=np.float32) if arr.ndim == 3: gray = cv2.cvtColor(arr.astype(np.uint8), cv2.COLOR_BGR2GRAY).astype(np.float32) else: gray = arr return f"mean={gray.mean():.1f} p05={np.percentile(gray,5):.1f} p95={np.percentile(gray,95):.1f}" # ============================================================ # Pipeline # ============================================================ def create_pipeline_and_outputs(args): pipeline = dai.Pipeline() # CAM_A color rgb = pipeline.create(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.preview_w), int(args.preview_h)) # CAM_B/C mono mono_b = pipeline.create(dai.node.MonoCamera) mono_c = pipeline.create(dai.node.MonoCamera) mono_b.setBoardSocket(socket_from_name(args.cam_b)) mono_c.setBoardSocket(socket_from_name(args.cam_c)) mono_b.setResolution(mono_resolution_from_name(args.mono_resolution)) mono_c.setResolution(mono_resolution_from_name(args.mono_resolution)) mono_b.setFps(float(args.fps)) mono_c.setFps(float(args.fps)) outputs = { "rgb": rgb.preview, "cam_b": mono_b.out, "cam_c": mono_c.out, } return pipeline, outputs # ============================================================ # Main # ============================================================ def start_pipeline(pipeline): fn = getattr(pipeline, "start", None) if not callable(fn): raise RuntimeError("pipeline.start() nao existe nesta versao do DepthAI.") fn() def stop_pipeline(pipeline): try: fn = getattr(pipeline, "stop", None) if callable(fn): fn() except Exception: pass def save_snapshot(out_dir: str, rgb, cam_b, cam_c, canvas): folder = Path(out_dir) folder.mkdir(parents=True, exist_ok=True) ts = time.strftime("%Y%m%d_%H%M%S") if rgb is not None: cv2.imwrite(str(folder / f"{ts}_CAM_A_rgb.png"), rgb) if cam_b is not None: cv2.imwrite(str(folder / f"{ts}_CAM_B_mono.png"), normalize_u8(cam_b, auto=True)) if cam_c is not None: cv2.imwrite(str(folder / f"{ts}_CAM_C_mono.png"), normalize_u8(cam_c, auto=True)) if canvas is not None: cv2.imwrite(str(folder / f"{ts}_canvas.png"), canvas) print(f"[OK] snapshot salvo em {folder}") def main(args): pipeline, outputs = create_pipeline_and_outputs(args) queues = { name: create_output_queue(output, name, max_size=4, blocking=False) for name, output in outputs.items() } print("[INFO] Pipeline preview criado sem StereoDepth.") print("[INFO] Abra o PDF Charuco em tela cheia no monitor e aponte a camera para ele.") print("[INFO] O objetivo e ver se CAM_B e CAM_C enxergam marcadores/cantos com contraste.") start_pipeline(pipeline) cv2.namedWindow("OAK-FCC-3P Charuco Preview Probe", cv2.WINDOW_NORMAL) cv2.resizeWindow("OAK-FCC-3P Charuco Preview Probe", 1600, 900) show_edges = False auto_contrast = True frame_times = deque(maxlen=40) last_canvas = None rgb_frame = None b_frame = None c_frame = None try: while True: updated = False for name, q in queues.items(): frame = get_frame(q) if frame is None: continue updated = True if name == "rgb": rgb_frame = frame elif name == "cam_b": b_frame = frame elif name == "cam_c": c_frame = frame if updated: 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 if rgb_frame is None or b_frame is None or c_frame is None: key = cv2.waitKey(1) & 0xFF if key in (27, ord("q"), ord("Q")): break continue rgb_vis = rgb_frame.copy() b_vis = normalize_u8(b_frame, auto=auto_contrast) c_vis = normalize_u8(c_frame, auto=auto_contrast) if show_edges: rgb_gray = cv2.cvtColor(rgb_vis, cv2.COLOR_BGR2GRAY) rgb_panel = edge_view(rgb_gray) b_panel = edge_view(b_vis) c_panel = edge_view(c_vis) mode = "edges" else: rgb_panel = rgb_vis b_panel = b_vis c_panel = c_vis mode = "preview" panels = [ ("CAM_A RGB", rgb_panel, f"{stats_line(rgb_frame)} | fps={fps:.1f}"), ("CAM_B mono / RE", b_panel, stats_line(b_frame)), ("CAM_C mono / NIR", c_panel, stats_line(c_frame)), ("CAM_B edges" if not show_edges else "CAM_B preview", edge_view(b_vis) if not show_edges else b_vis, "bordas para ver marcador"), ("CAM_C edges" if not show_edges else "CAM_C preview", edge_view(c_vis) if not show_edges else c_vis, "bordas para ver marcador"), ("Info", np.zeros((300, 600, 3), dtype=np.uint8), f"mode={mode} auto_contrast={auto_contrast} | E edges | C contraste | S save | Q sair"), ] canvas = make_grid(panels, panel_w=args.panel_w, cols=3) # Escreve texto grande no painel Info vazio, ultimo quadrante. info_y0 = canvas.shape[0] - resize_keep(np.zeros((300, 600, 3), dtype=np.uint8), args.panel_w).shape[0] cv2.putText(canvas, "Charuco visibility test", (2 * (args.panel_w + 10) + 15, info_y0 + 95), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (0, 255, 255), 2, cv2.LINE_AA) cv2.putText(canvas, "Olhe CAM_B/C: marcadores precisam aparecer nitidos", (2 * (args.panel_w + 10) + 15, info_y0 + 135), cv2.FONT_HERSHEY_SIMPLEX, 0.52, (255, 255, 255), 1, cv2.LINE_AA) cv2.putText(canvas, "E=edges C=auto contrast S=snapshot Q=sair", (2 * (args.panel_w + 10) + 15, info_y0 + 170), cv2.FONT_HERSHEY_SIMPLEX, 0.52, (255, 255, 255), 1, cv2.LINE_AA) last_canvas = canvas cv2.imshow("OAK-FCC-3P Charuco Preview Probe", canvas) key = cv2.waitKey(1) & 0xFF if key in (27, ord("q"), ord("Q")): break elif key in (ord("e"), ord("E")): show_edges = not show_edges elif key in (ord("c"), ord("C")): auto_contrast = not auto_contrast elif key in (ord("s"), ord("S")): save_snapshot(args.out_dir, rgb_frame, b_frame, c_frame, last_canvas) finally: stop_pipeline(pipeline) cv2.destroyAllWindows() # ============================================================ # CLI # ============================================================ def build_argparser(): ap = argparse.ArgumentParser(description="Preview rapido CAM_A/CAM_B/CAM_C para testar visibilidade do Charuco.") ap.add_argument("--rgb", type=str, default="CAM_A") ap.add_argument("--cam-b", type=str, default="CAM_B") ap.add_argument("--cam-c", type=str, default="CAM_C") 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("--preview-w", type=int, default=640) ap.add_argument("--preview-h", type=int, default=400) ap.add_argument("--panel-w", type=int, default=500) ap.add_argument("--out-dir", type=str, default="charuco_preview_out") return ap if __name__ == "__main__": main(build_argparser().parse_args())