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()