654 lines
23 KiB
Python
654 lines
23 KiB
Python
|
import argparse
|
||
|
|
import json
|
||
|
|
import time
|
||
|
|
from pathlib import Path
|
||
|
|
|
||
|
|
import cv2
|
||
|
|
import numpy as np
|
||
|
|
|
||
|
|
from core.oak_fcc3_client import OakFcc3Client as MultiSpectralClient
|
||
|
|
|
||
|
|
|
||
|
|
# ============================================================
|
||
|
|
# Config padrão do projeto
|
||
|
|
# ============================================================
|
||
|
|
|
||
|
|
with open("config.json", "r", encoding="utf-8") as f:
|
||
|
|
config = json.load(f)
|
||
|
|
|
||
|
|
RAW_SIZE = config.get("raw_size", [1280, 800])
|
||
|
|
MODULE_PARAMS = config.get("module_params_json")
|
||
|
|
|
||
|
|
|
||
|
|
# ============================================================
|
||
|
|
# Visual helpers
|
||
|
|
# ============================================================
|
||
|
|
|
||
|
|
def normalize_to_u8(img: np.ndarray, p_low=1.0, p_high=99.0) -> np.ndarray:
|
||
|
|
arr = img.astype(np.float32)
|
||
|
|
valid = np.isfinite(arr)
|
||
|
|
|
||
|
|
if np.count_nonzero(valid) < 20:
|
||
|
|
return np.zeros(arr.shape[:2], dtype=np.uint8)
|
||
|
|
|
||
|
|
vals = arr[valid]
|
||
|
|
lo = np.percentile(vals, p_low)
|
||
|
|
hi = np.percentile(vals, p_high)
|
||
|
|
|
||
|
|
out = (arr - lo) / (hi - lo + 1e-6)
|
||
|
|
out = np.clip(out, 0.0, 1.0)
|
||
|
|
return (out * 255).astype(np.uint8)
|
||
|
|
|
||
|
|
|
||
|
|
def ensure_bgr_u8(img) -> np.ndarray:
|
||
|
|
if img is None:
|
||
|
|
return None
|
||
|
|
|
||
|
|
arr = np.asarray(img)
|
||
|
|
|
||
|
|
if arr.ndim == 2:
|
||
|
|
if arr.dtype != np.uint8:
|
||
|
|
arr = normalize_to_u8(arr)
|
||
|
|
return cv2.cvtColor(arr, cv2.COLOR_GRAY2BGR)
|
||
|
|
|
||
|
|
if arr.ndim == 3 and arr.shape[2] == 3:
|
||
|
|
if arr.dtype == np.uint8:
|
||
|
|
return arr.copy()
|
||
|
|
arr = np.clip(arr.astype(np.float32), 0.0, 1.0)
|
||
|
|
return (arr * 255.0).astype(np.uint8)
|
||
|
|
|
||
|
|
raise RuntimeError(f"Imagem inválida para BGR: shape={arr.shape}, dtype={arr.dtype}")
|
||
|
|
|
||
|
|
|
||
|
|
def to_gray_u8(img_bgr: np.ndarray) -> np.ndarray:
|
||
|
|
if img_bgr.ndim == 2:
|
||
|
|
return img_bgr if img_bgr.dtype == np.uint8 else normalize_to_u8(img_bgr)
|
||
|
|
return cv2.cvtColor(img_bgr, cv2.COLOR_BGR2GRAY)
|
||
|
|
|
||
|
|
|
||
|
|
def resize_to_fit(img: np.ndarray, w: int, h: int) -> np.ndarray:
|
||
|
|
return cv2.resize(img, (w, h), interpolation=cv2.INTER_AREA)
|
||
|
|
|
||
|
|
|
||
|
|
def put_label(img: np.ndarray, text: str, color=(255, 255, 255)) -> np.ndarray:
|
||
|
|
out = img.copy()
|
||
|
|
cv2.rectangle(out, (0, 0), (out.shape[1], 34), (0, 0, 0), -1)
|
||
|
|
cv2.putText(out, text, (10, 24), cv2.FONT_HERSHEY_SIMPLEX, 0.65, color, 1, cv2.LINE_AA)
|
||
|
|
return out
|
||
|
|
|
||
|
|
|
||
|
|
def overlay_hud(img_bgr: np.ndarray, lines: list[str]):
|
||
|
|
y = 24
|
||
|
|
for s in lines:
|
||
|
|
cv2.putText(img_bgr, s, (12, y), cv2.FONT_HERSHEY_SIMPLEX, 0.55, (0, 0, 0), 3, cv2.LINE_AA)
|
||
|
|
cv2.putText(img_bgr, s, (12, y), cv2.FONT_HERSHEY_SIMPLEX, 0.55, (255, 255, 255), 1, cv2.LINE_AA)
|
||
|
|
y += 24
|
||
|
|
|
||
|
|
|
||
|
|
def colorize_scalar_0_1(x: np.ndarray, cmap=cv2.COLORMAP_TURBO) -> np.ndarray:
|
||
|
|
x8 = np.clip(x * 255.0, 0, 255).astype(np.uint8)
|
||
|
|
return cv2.applyColorMap(x8, cmap)
|
||
|
|
|
||
|
|
|
||
|
|
def make_2x2(a, b, c, d, cell_w=640, cell_h=400, hud_lines=None) -> np.ndarray:
|
||
|
|
a = resize_to_fit(a, cell_w, cell_h)
|
||
|
|
b = resize_to_fit(b, cell_w, cell_h)
|
||
|
|
c = resize_to_fit(c, cell_w, cell_h)
|
||
|
|
d = resize_to_fit(d, cell_w, cell_h)
|
||
|
|
|
||
|
|
top = np.hstack([a, b])
|
||
|
|
bot = np.hstack([c, d])
|
||
|
|
canvas = np.vstack([top, bot])
|
||
|
|
|
||
|
|
if hud_lines:
|
||
|
|
overlay_hud(canvas, hud_lines)
|
||
|
|
|
||
|
|
return canvas
|
||
|
|
|
||
|
|
|
||
|
|
def make_3x2(a, b, c, d, e, f, cell_w=520, cell_h=320, hud_lines=None) -> np.ndarray:
|
||
|
|
imgs = [resize_to_fit(x, cell_w, cell_h) for x in [a, b, c, d, e, f]]
|
||
|
|
row1 = np.hstack(imgs[:3])
|
||
|
|
row2 = np.hstack(imgs[3:])
|
||
|
|
canvas = np.vstack([row1, row2])
|
||
|
|
|
||
|
|
if hud_lines:
|
||
|
|
overlay_hud(canvas, hud_lines)
|
||
|
|
|
||
|
|
return canvas
|
||
|
|
|
||
|
|
|
||
|
|
# ============================================================
|
||
|
|
# Calibração product bundle
|
||
|
|
# ============================================================
|
||
|
|
|
||
|
|
def scalar_str(x):
|
||
|
|
arr = np.array(x)
|
||
|
|
if arr.shape == ():
|
||
|
|
return str(arr.item())
|
||
|
|
return str(x)
|
||
|
|
|
||
|
|
|
||
|
|
def load_calibration_bundle(path: str | Path) -> dict:
|
||
|
|
data = np.load(str(path), allow_pickle=True)
|
||
|
|
keys = set(data.files)
|
||
|
|
|
||
|
|
required = [
|
||
|
|
"rgb_cam",
|
||
|
|
"re_cam",
|
||
|
|
"nir_cam",
|
||
|
|
"image_size",
|
||
|
|
"H_RE_to_RGB",
|
||
|
|
"H_NIR_to_RGB",
|
||
|
|
]
|
||
|
|
|
||
|
|
for k in required:
|
||
|
|
if k not in keys:
|
||
|
|
raise RuntimeError(f"Calibração sem chave obrigatória: {k}")
|
||
|
|
|
||
|
|
calib = {
|
||
|
|
"data": data,
|
||
|
|
"keys": keys,
|
||
|
|
"rgb_cam": scalar_str(data["rgb_cam"]),
|
||
|
|
"re_cam": scalar_str(data["re_cam"]),
|
||
|
|
"nir_cam": scalar_str(data["nir_cam"]),
|
||
|
|
"image_size": tuple(data["image_size"].astype(int).tolist()),
|
||
|
|
"H_RE_to_RGB": data["H_RE_to_RGB"].astype(np.float64),
|
||
|
|
"H_NIR_to_RGB": data["H_NIR_to_RGB"].astype(np.float64),
|
||
|
|
"overlap_RE_to_RGB": data["overlap_RE_to_RGB"] if "overlap_RE_to_RGB" in keys else None,
|
||
|
|
"overlap_NIR_to_RGB": data["overlap_NIR_to_RGB"] if "overlap_NIR_to_RGB" in keys else None,
|
||
|
|
"overlap_common_RGB": data["overlap_common_RGB"] if "overlap_common_RGB" in keys else None,
|
||
|
|
}
|
||
|
|
|
||
|
|
return calib
|
||
|
|
|
||
|
|
|
||
|
|
def get_pair_prefix(calib: dict, cam1: str, cam2: str):
|
||
|
|
keys = calib["keys"]
|
||
|
|
direct = f"pair_{cam1}_{cam2}"
|
||
|
|
inv = f"pair_{cam2}_{cam1}"
|
||
|
|
|
||
|
|
if f"{direct}_map1x" in keys:
|
||
|
|
return direct, False
|
||
|
|
|
||
|
|
if f"{inv}_map1x" in keys:
|
||
|
|
return inv, True
|
||
|
|
|
||
|
|
raise RuntimeError(f"Par estéreo {cam1}<->{cam2} não encontrado no .npz")
|
||
|
|
|
||
|
|
|
||
|
|
def rectify_pair_gray(gray1, gray2, calib: dict, cam1: str, cam2: str):
|
||
|
|
data = calib["data"]
|
||
|
|
image_w, image_h = calib["image_size"]
|
||
|
|
|
||
|
|
if gray1.shape[::-1] != (image_w, image_h):
|
||
|
|
gray1 = cv2.resize(gray1, (image_w, image_h), interpolation=cv2.INTER_AREA)
|
||
|
|
if gray2.shape[::-1] != (image_w, image_h):
|
||
|
|
gray2 = cv2.resize(gray2, (image_w, image_h), interpolation=cv2.INTER_AREA)
|
||
|
|
|
||
|
|
prefix, inverted = get_pair_prefix(calib, cam1, cam2)
|
||
|
|
|
||
|
|
if not inverted:
|
||
|
|
map1x = data[f"{prefix}_map1x"]
|
||
|
|
map1y = data[f"{prefix}_map1y"]
|
||
|
|
map2x = data[f"{prefix}_map2x"]
|
||
|
|
map2y = data[f"{prefix}_map2y"]
|
||
|
|
else:
|
||
|
|
map1x = data[f"{prefix}_map2x"]
|
||
|
|
map1y = data[f"{prefix}_map2y"]
|
||
|
|
map2x = data[f"{prefix}_map1x"]
|
||
|
|
map2y = data[f"{prefix}_map1y"]
|
||
|
|
|
||
|
|
rect1 = cv2.remap(gray1, map1x, map1y, cv2.INTER_LINEAR, borderMode=cv2.BORDER_CONSTANT, borderValue=0)
|
||
|
|
rect2 = cv2.remap(gray2, map2x, map2y, cv2.INTER_LINEAR, borderMode=cv2.BORDER_CONSTANT, borderValue=0)
|
||
|
|
return rect1, rect2
|
||
|
|
|
||
|
|
|
||
|
|
# ============================================================
|
||
|
|
# Frame extraction from OakFcc3Client
|
||
|
|
# ============================================================
|
||
|
|
|
||
|
|
def role_map_from_meta(meta: dict) -> dict:
|
||
|
|
camera_info = meta.get("camera_info", {}) or {}
|
||
|
|
out = {}
|
||
|
|
for cam_id, info in camera_info.items():
|
||
|
|
role = info.get("role") or cam_id
|
||
|
|
out[role] = cam_id
|
||
|
|
return out
|
||
|
|
|
||
|
|
|
||
|
|
def build_live_role_images(cam, frame, meta, calib: dict, beauty_preview=False) -> dict:
|
||
|
|
"""
|
||
|
|
Retorna imagens BGR por role: rgb/re/nir.
|
||
|
|
|
||
|
|
Usa os helpers do OakFcc3Client para reconstruir previews a partir do RAW_BRUTO.
|
||
|
|
O script de referência usa get_next_decoded(...) e build_preview_from_raw_payload(...)
|
||
|
|
para RAW_BRUTO; aqui aproveitamos build_visual_preview_from_raw(...), quando disponível.
|
||
|
|
"""
|
||
|
|
frame_type = meta.get("frame_type", "RAW_BRUTO")
|
||
|
|
|
||
|
|
if frame_type != "RAW_BRUTO" or not isinstance(frame, dict):
|
||
|
|
raise RuntimeError("Este viewer espera frame_type=RAW_BRUTO e frame como dict por câmera.")
|
||
|
|
|
||
|
|
camera_info = meta.get("camera_info", {}) or {}
|
||
|
|
|
||
|
|
previews = None
|
||
|
|
if hasattr(cam, "build_visual_preview_from_raw"):
|
||
|
|
try:
|
||
|
|
previews = cam.build_visual_preview_from_raw(frame, meta)
|
||
|
|
except Exception:
|
||
|
|
previews = None
|
||
|
|
|
||
|
|
if previews is None:
|
||
|
|
preview_bgr, _, preview_source_id = cam.build_preview_from_raw_payload(frame=frame, meta=meta)
|
||
|
|
previews = {preview_source_id: preview_bgr}
|
||
|
|
|
||
|
|
by_role = {}
|
||
|
|
for cam_id, img in previews.items():
|
||
|
|
role = camera_info.get(cam_id, {}).get("role", cam_id)
|
||
|
|
by_role[role] = ensure_bgr_u8(img)
|
||
|
|
|
||
|
|
missing = [r for r in ["rgb", "re", "nir"] if r not in by_role]
|
||
|
|
if missing:
|
||
|
|
raise RuntimeError(f"Previews sem roles necessários: {missing}. Roles disponíveis={list(by_role.keys())}")
|
||
|
|
|
||
|
|
return by_role
|
||
|
|
|
||
|
|
|
||
|
|
# ============================================================
|
||
|
|
# Alignment / depth / confidence
|
||
|
|
# ============================================================
|
||
|
|
|
||
|
|
def warp_spectral_to_rgb(rgb_bgr, re_bgr, nir_bgr, calib: dict):
|
||
|
|
h, w = rgb_bgr.shape[:2]
|
||
|
|
|
||
|
|
re_to_rgb = cv2.warpPerspective(
|
||
|
|
re_bgr,
|
||
|
|
calib["H_RE_to_RGB"],
|
||
|
|
(w, h),
|
||
|
|
flags=cv2.INTER_LINEAR,
|
||
|
|
borderMode=cv2.BORDER_CONSTANT,
|
||
|
|
borderValue=0,
|
||
|
|
)
|
||
|
|
|
||
|
|
nir_to_rgb = cv2.warpPerspective(
|
||
|
|
nir_bgr,
|
||
|
|
calib["H_NIR_to_RGB"],
|
||
|
|
(w, h),
|
||
|
|
flags=cv2.INTER_LINEAR,
|
||
|
|
borderMode=cv2.BORDER_CONSTANT,
|
||
|
|
borderValue=0,
|
||
|
|
)
|
||
|
|
|
||
|
|
return re_to_rgb, nir_to_rgb
|
||
|
|
|
||
|
|
|
||
|
|
def make_sgbm(args):
|
||
|
|
num_disp = max(16, int(round(args.num_disp / 16)) * 16)
|
||
|
|
|
||
|
|
block_size = max(3, int(args.block_size))
|
||
|
|
if block_size % 2 == 0:
|
||
|
|
block_size += 1
|
||
|
|
|
||
|
|
matcher = cv2.StereoSGBM_create(
|
||
|
|
minDisparity=args.min_disp,
|
||
|
|
numDisparities=num_disp,
|
||
|
|
blockSize=block_size,
|
||
|
|
P1=8 * block_size * block_size,
|
||
|
|
P2=32 * block_size * block_size,
|
||
|
|
disp12MaxDiff=1,
|
||
|
|
uniquenessRatio=args.uniqueness,
|
||
|
|
speckleWindowSize=args.speckle_window,
|
||
|
|
speckleRange=args.speckle_range,
|
||
|
|
preFilterCap=63,
|
||
|
|
mode=cv2.STEREO_SGBM_MODE_SGBM_3WAY,
|
||
|
|
)
|
||
|
|
|
||
|
|
return matcher, num_disp, block_size
|
||
|
|
|
||
|
|
|
||
|
|
def compute_stereo_disparity_and_confidence(re_bgr, nir_bgr, calib: dict, args):
|
||
|
|
re_cam = calib["re_cam"]
|
||
|
|
nir_cam = calib["nir_cam"]
|
||
|
|
|
||
|
|
re_gray = to_gray_u8(re_bgr)
|
||
|
|
nir_gray = to_gray_u8(nir_bgr)
|
||
|
|
|
||
|
|
# Par estéreo no espaço RE/NIR retificado.
|
||
|
|
re_rect, nir_rect = rectify_pair_gray(re_gray, nir_gray, calib, re_cam, nir_cam)
|
||
|
|
|
||
|
|
matcher, num_disp, block_size = make_sgbm(args)
|
||
|
|
disp = matcher.compute(re_rect, nir_rect).astype(np.float32) / 16.0
|
||
|
|
|
||
|
|
valid = disp > args.min_valid_disp
|
||
|
|
|
||
|
|
# Normalização visual do depth/disparity.
|
||
|
|
disp_vis = disp.copy()
|
||
|
|
disp_vis[~valid] = 0.0
|
||
|
|
|
||
|
|
if np.count_nonzero(valid) > 20:
|
||
|
|
vals = disp_vis[valid]
|
||
|
|
p2 = np.percentile(vals, 2)
|
||
|
|
p98 = np.percentile(vals, 98)
|
||
|
|
disp_norm = (disp_vis - p2) / (p98 - p2 + 1e-6)
|
||
|
|
disp_norm = np.clip(disp_norm, 0.0, 1.0)
|
||
|
|
else:
|
||
|
|
p2 = 0.0
|
||
|
|
p98 = 1.0
|
||
|
|
disp_norm = np.zeros_like(disp_vis, dtype=np.float32)
|
||
|
|
|
||
|
|
disp_color = colorize_scalar_0_1(disp_norm)
|
||
|
|
|
||
|
|
# Confiança geométrica simples:
|
||
|
|
# - válida no SGBM
|
||
|
|
# - penaliza saltos fortes de disparity
|
||
|
|
# - penaliza regiões com baixa textura no par retificado
|
||
|
|
valid_f = valid.astype(np.float32)
|
||
|
|
|
||
|
|
disp_smooth = cv2.GaussianBlur(disp_vis, (5, 5), 0)
|
||
|
|
grad_x = cv2.Sobel(disp_smooth, cv2.CV_32F, 1, 0, ksize=3)
|
||
|
|
grad_y = cv2.Sobel(disp_smooth, cv2.CV_32F, 0, 1, ksize=3)
|
||
|
|
grad_mag = np.sqrt(grad_x * grad_x + grad_y * grad_y)
|
||
|
|
|
||
|
|
grad_penalty = np.clip(grad_mag / max(args.parallax_grad_ref, 1e-6), 0.0, 1.0)
|
||
|
|
|
||
|
|
tex_re = cv2.Laplacian(re_rect, cv2.CV_32F, ksize=3)
|
||
|
|
tex_nir = cv2.Laplacian(nir_rect, cv2.CV_32F, ksize=3)
|
||
|
|
texture = (np.abs(tex_re) + np.abs(tex_nir)) * 0.5
|
||
|
|
texture_norm = np.clip(texture / max(args.texture_ref, 1e-6), 0.0, 1.0)
|
||
|
|
texture_norm = cv2.GaussianBlur(texture_norm, (5, 5), 0)
|
||
|
|
|
||
|
|
confidence_rect = valid_f * (1.0 - grad_penalty) * (0.35 + 0.65 * texture_norm)
|
||
|
|
confidence_rect = np.clip(confidence_rect, 0.0, 1.0)
|
||
|
|
|
||
|
|
# Para exibir junto com RGB, trazemos a confiança do espaço estéreo RE/NIR para RGB usando a homografia RE->RGB.
|
||
|
|
h_rgb, w_rgb = re_bgr.shape[:2]
|
||
|
|
confidence_rgb = cv2.warpPerspective(
|
||
|
|
confidence_rect.astype(np.float32),
|
||
|
|
calib["H_RE_to_RGB"],
|
||
|
|
(w_rgb, h_rgb),
|
||
|
|
flags=cv2.INTER_LINEAR,
|
||
|
|
borderMode=cv2.BORDER_CONSTANT,
|
||
|
|
borderValue=0,
|
||
|
|
)
|
||
|
|
|
||
|
|
disp_rgb = cv2.warpPerspective(
|
||
|
|
disp_norm.astype(np.float32),
|
||
|
|
calib["H_RE_to_RGB"],
|
||
|
|
(w_rgb, h_rgb),
|
||
|
|
flags=cv2.INTER_LINEAR,
|
||
|
|
borderMode=cv2.BORDER_CONSTANT,
|
||
|
|
borderValue=0,
|
||
|
|
)
|
||
|
|
|
||
|
|
confidence_color = colorize_scalar_0_1(confidence_rgb, cmap=cv2.COLORMAP_VIRIDIS)
|
||
|
|
disp_rgb_color = colorize_scalar_0_1(disp_rgb, cmap=cv2.COLORMAP_TURBO)
|
||
|
|
|
||
|
|
stats = {
|
||
|
|
"num_disp": num_disp,
|
||
|
|
"block_size": block_size,
|
||
|
|
"valid_pct": float(np.mean(valid) * 100.0),
|
||
|
|
"conf_mean": float(np.mean(confidence_rect)),
|
||
|
|
"disp_p02": float(p2),
|
||
|
|
"disp_p98": float(p98),
|
||
|
|
"disp_p50": float(np.percentile(disp_vis[valid], 50)) if np.count_nonzero(valid) > 20 else 0.0,
|
||
|
|
}
|
||
|
|
|
||
|
|
return {
|
||
|
|
"re_rect": cv2.cvtColor(re_rect, cv2.COLOR_GRAY2BGR),
|
||
|
|
"nir_rect": cv2.cvtColor(nir_rect, cv2.COLOR_GRAY2BGR),
|
||
|
|
"disp_rect_color": disp_color,
|
||
|
|
"disp_rgb_color": disp_rgb_color,
|
||
|
|
"confidence_rgb": confidence_rgb,
|
||
|
|
"confidence_color": confidence_color,
|
||
|
|
"stats": stats,
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
def apply_overlap_mask(img_bgr, calib: dict, enabled=True):
|
||
|
|
if not enabled:
|
||
|
|
return img_bgr
|
||
|
|
|
||
|
|
mask = calib.get("overlap_common_RGB")
|
||
|
|
if mask is None:
|
||
|
|
return img_bgr
|
||
|
|
|
||
|
|
if mask.shape[:2] != img_bgr.shape[:2]:
|
||
|
|
mask = cv2.resize(mask, (img_bgr.shape[1], img_bgr.shape[0]), interpolation=cv2.INTER_NEAREST)
|
||
|
|
|
||
|
|
mask_bool = mask > 0
|
||
|
|
out = img_bgr.copy()
|
||
|
|
out[~mask_bool] = (out[~mask_bool] * 0.2).astype(np.uint8)
|
||
|
|
return out
|
||
|
|
|
||
|
|
|
||
|
|
def confidence_overlay_on_rgb(rgb_bgr, confidence_rgb, alpha=0.45):
|
||
|
|
conf_color = colorize_scalar_0_1(confidence_rgb, cmap=cv2.COLORMAP_VIRIDIS)
|
||
|
|
return cv2.addWeighted(rgb_bgr, 1.0 - alpha, conf_color, alpha, 0)
|
||
|
|
|
||
|
|
|
||
|
|
# ============================================================
|
||
|
|
# Main
|
||
|
|
# ============================================================
|
||
|
|
|
||
|
|
def main():
|
||
|
|
parser = argparse.ArgumentParser(
|
||
|
|
description="Live preview: RGB referência + RE/NIR alinhados por homografia + depth/confiança por estéreo RE-NIR.",
|
||
|
|
formatter_class=argparse.ArgumentDefaultsHelpFormatter,
|
||
|
|
)
|
||
|
|
|
||
|
|
parser.add_argument("--calib_path", required=True, help=".npz com H_RE_to_RGB, H_NIR_to_RGB e mapas estéreo RE-NIR")
|
||
|
|
parser.add_argument("--fps", type=int, default=20)
|
||
|
|
parser.add_argument("--width", type=int, default=RAW_SIZE[0])
|
||
|
|
parser.add_argument("--height", type=int, default=RAW_SIZE[1])
|
||
|
|
parser.add_argument("--bayer", default="BGGR", choices=["GBRG", "GRBG", "RGGB", "BGGR"])
|
||
|
|
parser.add_argument("--module_calibration_json", default=MODULE_PARAMS)
|
||
|
|
|
||
|
|
parser.add_argument("--output_dtype", default="float32", choices=["uint8", "uint16", "float32"])
|
||
|
|
parser.add_argument("--capture_mode", default="TRIPLE", choices=["AUTO", "SINGLE", "DOUBLE", "TRIPLE"])
|
||
|
|
parser.add_argument("--raw_policy", default="require_triple", choices=["allow_single", "require_triple"])
|
||
|
|
|
||
|
|
parser.add_argument("--cell_w", type=int, default=640)
|
||
|
|
parser.add_argument("--cell_h", type=int, default=400)
|
||
|
|
parser.add_argument("--layout", default="2x2", choices=["2x2", "3x2"])
|
||
|
|
parser.add_argument("--mask_overlap", action="store_true", help="Escurece área fora da interseção RE/NIR->RGB salva no .npz")
|
||
|
|
parser.add_argument("--show_conf_overlay", action="store_true")
|
||
|
|
|
||
|
|
parser.add_argument("--num_disp", type=int, default=128)
|
||
|
|
parser.add_argument("--block_size", type=int, default=7)
|
||
|
|
parser.add_argument("--min_disp", type=int, default=0)
|
||
|
|
parser.add_argument("--min_valid_disp", type=float, default=1.0)
|
||
|
|
parser.add_argument("--uniqueness", type=int, default=8)
|
||
|
|
parser.add_argument("--speckle_window", type=int, default=80)
|
||
|
|
parser.add_argument("--speckle_range", type=int, default=2)
|
||
|
|
parser.add_argument("--parallax_grad_ref", type=float, default=6.0)
|
||
|
|
parser.add_argument("--texture_ref", type=float, default=25.0)
|
||
|
|
|
||
|
|
args = parser.parse_args()
|
||
|
|
|
||
|
|
calib = load_calibration_bundle(args.calib_path)
|
||
|
|
|
||
|
|
print("============================================")
|
||
|
|
print("Live Multispec Alignment Preview")
|
||
|
|
print(f"calib_path : {args.calib_path}")
|
||
|
|
print(f"RGB cam : {calib['rgb_cam']}")
|
||
|
|
print(f"RE cam : {calib['re_cam']}")
|
||
|
|
print(f"NIR cam : {calib['nir_cam']}")
|
||
|
|
print(f"image_size : {calib['image_size']}")
|
||
|
|
print(f"raw : {args.width}x{args.height} | bayer={args.bayer}")
|
||
|
|
print(f"capture : RAW_BRUTO | {args.capture_mode} | {args.raw_policy}")
|
||
|
|
print("Keys : Q/Esc sair | O overlap | C conf overlay | [ ] numDisp | - + block")
|
||
|
|
print("============================================")
|
||
|
|
|
||
|
|
window_name = "Live RGB/RE/NIR + Stereo Confidence"
|
||
|
|
cv2.namedWindow(window_name, cv2.WINDOW_NORMAL)
|
||
|
|
|
||
|
|
fps_view = 0.0
|
||
|
|
n_view = 0
|
||
|
|
t_fps = time.time()
|
||
|
|
last_frame_id = -1
|
||
|
|
msg = ""
|
||
|
|
msg_t = 0.0
|
||
|
|
|
||
|
|
mask_overlap = args.mask_overlap
|
||
|
|
conf_overlay = args.show_conf_overlay
|
||
|
|
|
||
|
|
try:
|
||
|
|
with MultiSpectralClient(
|
||
|
|
width=args.width,
|
||
|
|
height=args.height,
|
||
|
|
bayer=args.bayer,
|
||
|
|
fps=args.fps,
|
||
|
|
frame_type="RAW_BRUTO",
|
||
|
|
output_dtype=args.output_dtype,
|
||
|
|
capture_mode=args.capture_mode,
|
||
|
|
raw_policy=args.raw_policy,
|
||
|
|
module_calibration_json=args.module_calibration_json,
|
||
|
|
) as cam:
|
||
|
|
while True:
|
||
|
|
frame, meta, decoded = cam.get_next_decoded(timeout=1.0)
|
||
|
|
|
||
|
|
if meta is None or frame is None:
|
||
|
|
k = cv2.waitKey(1) & 0xFF
|
||
|
|
if k in (ord("q"), ord("Q"), 27):
|
||
|
|
break
|
||
|
|
continue
|
||
|
|
|
||
|
|
frame_id = meta.get("frame_id", -1)
|
||
|
|
if frame_id == last_frame_id:
|
||
|
|
k = cv2.waitKey(1) & 0xFF
|
||
|
|
if k in (ord("q"), ord("Q"), 27):
|
||
|
|
break
|
||
|
|
continue
|
||
|
|
|
||
|
|
last_frame_id = frame_id
|
||
|
|
|
||
|
|
try:
|
||
|
|
role_imgs = build_live_role_images(cam, frame, meta, calib)
|
||
|
|
|
||
|
|
rgb = role_imgs["rgb"]
|
||
|
|
re = role_imgs["re"]
|
||
|
|
nir = role_imgs["nir"]
|
||
|
|
|
||
|
|
# Garante que tudo use o tamanho do RGB como referência visual.
|
||
|
|
rgb_h, rgb_w = rgb.shape[:2]
|
||
|
|
if re.shape[:2] != (rgb_h, rgb_w):
|
||
|
|
re = cv2.resize(re, (rgb_w, rgb_h), interpolation=cv2.INTER_AREA)
|
||
|
|
if nir.shape[:2] != (rgb_h, rgb_w):
|
||
|
|
nir = cv2.resize(nir, (rgb_w, rgb_h), interpolation=cv2.INTER_AREA)
|
||
|
|
|
||
|
|
re_rgb, nir_rgb = warp_spectral_to_rgb(rgb, re, nir, calib)
|
||
|
|
stereo = compute_stereo_disparity_and_confidence(re, nir, calib, args)
|
||
|
|
|
||
|
|
rgb_show = apply_overlap_mask(rgb, calib, enabled=mask_overlap)
|
||
|
|
re_show = apply_overlap_mask(re_rgb, calib, enabled=mask_overlap)
|
||
|
|
nir_show = apply_overlap_mask(nir_rgb, calib, enabled=mask_overlap)
|
||
|
|
|
||
|
|
if conf_overlay:
|
||
|
|
rgb_panel = confidence_overlay_on_rgb(rgb_show, stereo["confidence_rgb"])
|
||
|
|
rgb_panel = put_label(rgb_panel, "RGB + confidence overlay")
|
||
|
|
else:
|
||
|
|
rgb_panel = put_label(rgb_show, "RGB reference")
|
||
|
|
|
||
|
|
re_panel = put_label(re_show, "RE -> RGB plane")
|
||
|
|
nir_panel = put_label(nir_show, "NIR -> RGB plane")
|
||
|
|
depth_panel = put_label(stereo["disp_rgb_color"], "Stereo disparity RE/NIR -> RGB")
|
||
|
|
conf_panel = put_label(stereo["confidence_color"], "Spectral confidence / parallax risk")
|
||
|
|
|
||
|
|
s = stereo["stats"]
|
||
|
|
|
||
|
|
n_view += 1
|
||
|
|
now = time.time()
|
||
|
|
dt = now - t_fps
|
||
|
|
if dt >= 1.0:
|
||
|
|
fps_view = n_view / dt
|
||
|
|
n_view = 0
|
||
|
|
t_fps = now
|
||
|
|
|
||
|
|
hud = [
|
||
|
|
f"frame_id={frame_id} | FPS_VIEW={fps_view:.1f} | overlap={'ON' if mask_overlap else 'OFF'} | conf_overlay={'ON' if conf_overlay else 'OFF'}",
|
||
|
|
f"stereo valid={s['valid_pct']:.1f}% | conf_mean={s['conf_mean']:.2f} | disp p50={s['disp_p50']:.2f} | p02/p98={s['disp_p02']:.2f}/{s['disp_p98']:.2f}",
|
||
|
|
f"numDisp={s['num_disp']} | block={s['block_size']} | O overlap | C conf | [ ] numDisp | - + block | Q sair",
|
||
|
|
]
|
||
|
|
|
||
|
|
if msg and (time.time() - msg_t) < 2.0:
|
||
|
|
hud.append(msg)
|
||
|
|
|
||
|
|
if args.layout == "2x2":
|
||
|
|
canvas = make_2x2(
|
||
|
|
rgb_panel,
|
||
|
|
re_panel,
|
||
|
|
nir_panel,
|
||
|
|
depth_panel,
|
||
|
|
cell_w=args.cell_w,
|
||
|
|
cell_h=args.cell_h,
|
||
|
|
hud_lines=hud,
|
||
|
|
)
|
||
|
|
else:
|
||
|
|
canvas = make_3x2(
|
||
|
|
rgb_panel,
|
||
|
|
re_panel,
|
||
|
|
nir_panel,
|
||
|
|
depth_panel,
|
||
|
|
conf_panel,
|
||
|
|
put_label(stereo["disp_rect_color"], "Raw rectified disparity space"),
|
||
|
|
cell_w=args.cell_w,
|
||
|
|
cell_h=args.cell_h,
|
||
|
|
hud_lines=hud,
|
||
|
|
)
|
||
|
|
|
||
|
|
cv2.imshow(window_name, canvas)
|
||
|
|
|
||
|
|
except Exception as e:
|
||
|
|
err = np.zeros((500, 1200, 3), dtype=np.uint8)
|
||
|
|
cv2.putText(err, f"Erro: {e}", (20, 70), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 0, 255), 2, cv2.LINE_AA)
|
||
|
|
cv2.imshow(window_name, err)
|
||
|
|
print(f"[ERRO FRAME] {e}")
|
||
|
|
|
||
|
|
k = cv2.waitKey(1) & 0xFF
|
||
|
|
|
||
|
|
if k in (ord("q"), ord("Q"), 27):
|
||
|
|
break
|
||
|
|
|
||
|
|
elif k in (ord("o"), ord("O")):
|
||
|
|
mask_overlap = not mask_overlap
|
||
|
|
msg = f"overlap mask -> {mask_overlap}"
|
||
|
|
msg_t = time.time()
|
||
|
|
|
||
|
|
elif k in (ord("c"), ord("C")):
|
||
|
|
conf_overlay = not conf_overlay
|
||
|
|
msg = f"confidence overlay -> {conf_overlay}"
|
||
|
|
msg_t = time.time()
|
||
|
|
|
||
|
|
elif k == ord("["):
|
||
|
|
args.num_disp = max(16, args.num_disp - 16)
|
||
|
|
msg = f"num_disp -> {args.num_disp}"
|
||
|
|
msg_t = time.time()
|
||
|
|
|
||
|
|
elif k == ord("]"):
|
||
|
|
args.num_disp = min(512, args.num_disp + 16)
|
||
|
|
msg = f"num_disp -> {args.num_disp}"
|
||
|
|
msg_t = time.time()
|
||
|
|
|
||
|
|
elif k in (ord("-"), ord("_")):
|
||
|
|
args.block_size = max(3, args.block_size - 2)
|
||
|
|
if args.block_size % 2 == 0:
|
||
|
|
args.block_size -= 1
|
||
|
|
msg = f"block_size -> {args.block_size}"
|
||
|
|
msg_t = time.time()
|
||
|
|
|
||
|
|
elif k in (ord("+"), ord("=")):
|
||
|
|
args.block_size = min(31, args.block_size + 2)
|
||
|
|
if args.block_size % 2 == 0:
|
||
|
|
args.block_size += 1
|
||
|
|
msg = f"block_size -> {args.block_size}"
|
||
|
|
msg_t = time.time()
|
||
|
|
|
||
|
|
finally:
|
||
|
|
cv2.destroyAllWindows()
|
||
|
|
print("Fim do preview live.")
|
||
|
|
|
||
|
|
|
||
|
|
if __name__ == "__main__":
|
||
|
|
main()
|