421 lines
14 KiB
Python
421 lines
14 KiB
Python
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())
|