691 lines
25 KiB
Python
691 lines
25 KiB
Python
|
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())
|