From 2557bdba9fab44a829e3ca46c0279f3659d05988 Mon Sep 17 00:00:00 2001 From: Diego Freitas Date: Mon, 4 May 2026 17:15:40 -0300 Subject: [PATCH] iniciado modulo multiespectral oak-fcc-3 --- .gitignore | 1 + Python/OAK/OAK-FCC-3-test.py | 58 + .../datasets/oak-fcc-3/build_module_params.py | 94 ++ .../datasets/oak-fcc-3/check_saved_files.py | 541 +++++++ .../oak-fcc-3/manual_fusion_calibrator.py | 689 ++++++++ .../OAK/datasets/oak-fcc-3/oak_fcc3_client.py | 391 +++++ .../datasets/oak-fcc-3/oak_fcc3_manager.py | 363 +++++ .../datasets/oak-fcc-3/oak_fcc3_service.py | 133 ++ .../oak-fcc-3/sensor_calibration_tool.py | 1409 +++++++++++++++++ .../datasets/oak-fcc-3/test_captude_mode.py | 23 + Python/OAK/datasets/oak-fcc-3/test_manager.py | 35 + .../oak-fcc-3/test_oak_fcc3_client.py | 48 + .../oak-fcc-3/test_oak_fcc3_service.py | 74 + 13 files changed, 3859 insertions(+) create mode 100644 Python/OAK/OAK-FCC-3-test.py create mode 100644 Python/OAK/datasets/oak-fcc-3/build_module_params.py create mode 100644 Python/OAK/datasets/oak-fcc-3/check_saved_files.py create mode 100644 Python/OAK/datasets/oak-fcc-3/manual_fusion_calibrator.py create mode 100644 Python/OAK/datasets/oak-fcc-3/oak_fcc3_client.py create mode 100644 Python/OAK/datasets/oak-fcc-3/oak_fcc3_manager.py create mode 100644 Python/OAK/datasets/oak-fcc-3/oak_fcc3_service.py create mode 100644 Python/OAK/datasets/oak-fcc-3/sensor_calibration_tool.py create mode 100644 Python/OAK/datasets/oak-fcc-3/test_captude_mode.py create mode 100644 Python/OAK/datasets/oak-fcc-3/test_manager.py create mode 100644 Python/OAK/datasets/oak-fcc-3/test_oak_fcc3_client.py create mode 100644 Python/OAK/datasets/oak-fcc-3/test_oak_fcc3_service.py diff --git a/.gitignore b/.gitignore index 84ee260e9..63d2b6d33 100644 --- a/.gitignore +++ b/.gitignore @@ -58,6 +58,7 @@ AgroBase/OperationControl/bin/ **/__pycache__/ t_cor/ +/Python/OAK/venv Python/OAK/datasets/venv/ Python/OAK/datasets/oak-1/dataset/ Python/OAK/datasets/oak-1/backup/ diff --git a/Python/OAK/OAK-FCC-3-test.py b/Python/OAK/OAK-FCC-3-test.py new file mode 100644 index 000000000..57f85d0b4 --- /dev/null +++ b/Python/OAK/OAK-FCC-3-test.py @@ -0,0 +1,58 @@ +import cv2 +import depthai as dai +import time + +FPS = 30 +SIZE = (640, 400) + +device = dai.Device() + +print("[INFO] DepthAI:", dai.__version__) +print("[INFO] Cameras conectadas:") +print(device.getConnectedCameraFeatures()) +print("[INFO] Sockets:", device.getConnectedCameras()) + +with dai.Pipeline(device) as pipeline: + queues = {} + + sockets = device.getConnectedCameras() + + for socket in sockets: + print(f"[INFO] Criando câmera no socket: {socket}") + + cam = pipeline.create(dai.node.Camera).build(socket) + + out = cam.requestOutput( + SIZE, + type=dai.ImgFrame.Type.BGR888p, + fps=FPS + ) + + queues[str(socket)] = out.createOutputQueue() + + pipeline.start() + + last = time.time() + frames = 0 + + while pipeline.isRunning(): + for name, q in queues.items(): + msg = q.tryGet() + + if msg is not None: + frame = msg.getCvFrame() + cv2.imshow(f"OAK {name}", frame) + + frames += 1 + now = time.time() + + if now - last >= 1.0: + print(f"[FPS LOOP] {frames / (now - last):.1f}") + frames = 0 + last = now + + key = cv2.waitKey(1) + if key == 27 or key == ord("q"): + break + +cv2.destroyAllWindows() \ No newline at end of file diff --git a/Python/OAK/datasets/oak-fcc-3/build_module_params.py b/Python/OAK/datasets/oak-fcc-3/build_module_params.py new file mode 100644 index 000000000..57be6ddab --- /dev/null +++ b/Python/OAK/datasets/oak-fcc-3/build_module_params.py @@ -0,0 +1,94 @@ +import json +import argparse +from datetime import datetime + + +def now_str(): + return datetime.now().strftime("%Y-%m-%d %H:%M:%S") + + +def load_json(path): + with open(path, "r", encoding="utf-8") as f: + return json.load(f) + + +def main(): + parser = argparse.ArgumentParser() + parser.add_argument("--camera_json", default="calibration/sensor_calibration.json") + parser.add_argument("--fusion_json", default="calibration/manual_offsets.json") + parser.add_argument("--radiometric_json", default="") + parser.add_argument("--out", default="calibration/module_params.json") + args = parser.parse_args() + + cam_data = load_json(args.camera_json) + fusion_data = load_json(args.fusion_json) + + radiometric_data = {} + if args.radiometric_json: + radiometric_data = load_json(args.radiometric_json) + + camera_settings = cam_data.get("camera_settings") + if not isinstance(camera_settings, dict): + raise RuntimeError("camera_json sem camera_settings válido") + + radiometric_config = radiometric_data.get("radiometric_config") + + if not isinstance(radiometric_config, dict): + radiometric_config = cam_data.get("radiometric_config") + + if not isinstance(radiometric_config, dict): + radiometric_config = { + "interval_s": 0.5, + "strip_y0_pct": 0.95, + "strip_y1_pct": 1.0, + "patch_x0_pct": 0.35, + "patch_x1_pct": 0.75, + "target_mean": 0.70, + "deadband": 0.03, + "alpha": 0.18, + "exp_min_us": 100, + "exp_max_us": 80000, + "gain_min": 1.0, + "gain_max": 8.0, + "verbose": True, + "exp_apply_threshold_us": 50, + "gain_apply_threshold": 0.02, + } + + fusion_config = { + "alignment_mode": fusion_data.get("alignment_mode", "manual_affine"), + "baseline_mm": fusion_data.get("baseline_mm", 75.0), + "reference_camera": fusion_data.get("reference_camera", "cam2"), + "manual_offsets": fusion_data.get("manual_offsets", {}), + "homographies": fusion_data.get("homographies", {}), + "crop_valid_common": fusion_data.get("crop_valid_common", True), + "resize_after_crop": fusion_data.get("resize_after_crop", True), + "target_size": fusion_data.get("target_size", None), + } + + module_params = { + "schema": "multispec_module_params_v1", + "saved_at": now_str(), + + "frame_type": cam_data.get("frame_type", fusion_data.get("frame_type", "RAW_BRUTO")), + "capture_mode_requested": cam_data.get("capture_mode_requested", "AUTO"), + "capture_mode_effective": cam_data.get("capture_mode_effective", "AUTO"), + "raw_policy": cam_data.get("raw_policy", "allow_single"), + + "sensor_width": cam_data.get("sensor_width", fusion_data.get("sensor_width")), + "sensor_height": cam_data.get("sensor_height", fusion_data.get("sensor_height")), + "bayer_pattern": cam_data.get("bayer_pattern", fusion_data.get("bayer_pattern", "GBRG")), + + "camera_settings": camera_settings, + "fusion_config": fusion_config, + "radiometric_config": radiometric_config, + } + + with open(args.out, "w", encoding="utf-8") as f: + json.dump(module_params, f, ensure_ascii=False, indent=2) + + print(f"[OK] module_params gerado em: {args.out}") + + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/Python/OAK/datasets/oak-fcc-3/check_saved_files.py b/Python/OAK/datasets/oak-fcc-3/check_saved_files.py new file mode 100644 index 000000000..92237d376 --- /dev/null +++ b/Python/OAK/datasets/oak-fcc-3/check_saved_files.py @@ -0,0 +1,541 @@ +import os +import json +import argparse +from pathlib import Path + +import cv2 +import numpy as np + +from cam_3.pi.raw_processor_core import RawProcessorCore +from cam_3.pi.raw_processor_preview import RawProcessorPreview + + +def load_json(path: Path) -> dict: + with open(path, "r", encoding="utf-8") as f: + return json.load(f) + + +def normalize_float01_to_bgr(img_float: np.ndarray) -> np.ndarray: + """ + Recebe RGB float32 [0..1] em HWC e devolve BGR uint8. + """ + rgb_u8 = np.clip(img_float * 255.0, 0, 255).astype(np.uint8) + return cv2.cvtColor(rgb_u8, cv2.COLOR_RGB2BGR) + + +def chw_to_hwc(arr: np.ndarray) -> np.ndarray: + if arr.ndim != 3: + raise ValueError(f"Esperado CHW 3D, recebido shape={arr.shape}") + return np.transpose(arr, (1, 2, 0)) + + +def build_visual_from_saved_payload(payload_path: Path, meta: dict, cam_id: str | None = None) -> tuple[np.ndarray, str]: + """ + Retorna: + preview_bgr_reconstructed + texto_descritivo + """ + saved_type = meta.get("saved_payload_type") + + # ========================================================= + # Caso MULTI payload por câmera + # ========================================================= + if saved_type == "raw_native_multi": + if cam_id is None: + raise RuntimeError("cam_id é obrigatório para saved_payload_type='raw_native_multi'") + + saved_dtypes = meta.get("saved_payload_dtypes", {}) or {} + saved_shapes = meta.get("saved_payload_shapes", {}) or {} + + saved_dtype = saved_dtypes.get(cam_id) + saved_shape = saved_shapes.get(cam_id) + + if saved_dtype is None or saved_shape is None: + raise RuntimeError( + f"JSON não contém saved_payload_dtypes/saved_payload_shapes para {cam_id}" + ) + + np_dtype = np.dtype(saved_dtype) + raw = np.fromfile(str(payload_path), dtype=np_dtype) + arr = raw.reshape(tuple(saved_shape)) + + # Busca metadados da câmera no stream_meta + stream_meta = meta.get("stream_meta", {}) or {} + cam_frames = stream_meta.get("camera_frames", {}) or {} + cam_meta = cam_frames.get(cam_id, {}) or {} + + role = cam_meta.get("role", cam_id) + interface = cam_meta.get("interface", "") + bit_depth = int(cam_meta.get("bit_depth", 10)) + bayer = cam_meta.get("bayer_pattern", meta.get("bayer_pattern", "GBRG")) + + # USB RGB nativo + if interface.upper() == "USB" or (arr.ndim == 3 and arr.shape[2] == 3 and arr.dtype == np.uint8): + preview_bgr = arr.copy() + desc = f"{cam_id} | role={role} | USB/RGB nativo | dtype={arr.dtype} | shape={arr.shape}" + return preview_bgr, desc + + # CSI RAW packed mono + sensor_width, sensor_height = resolve_sensor_dims_for_raw10_packed(arr, cam_meta, meta) + + core = RawProcessorCore( + sensor_width=sensor_width, + sensor_height=sensor_height, + bayer_pattern=bayer, + ) + preview = RawProcessorPreview( + sensor_width=sensor_width, + sensor_height=sensor_height, + bayer_pattern=bayer, + ) + + packed = arr + if packed.ndim == 3 and packed.shape[2] == 1: + packed = packed[:, :, 0] + + raw16 = core.unpack_raw10_packed(packed) + preview_bgr = preview.raw16_to_preview_bgr(raw16, bit_depth=bit_depth) + + desc = ( + f"{cam_id} | role={role} | RAW packed mono | " + f"dtype={arr.dtype} | shape={arr.shape} | " + f"sensor={sensor_width}x{sensor_height} | " + f"bayer={bayer} | bit_depth={bit_depth}" + ) + return preview_bgr, desc + + # ========================================================= + # Caso payload único + # ========================================================= + saved_dtype = meta.get("saved_payload_dtype") + saved_shape = meta.get("saved_payload_shape") + + if saved_type is None or saved_dtype is None or saved_shape is None: + raise RuntimeError( + "JSON não contém saved_payload_type / saved_payload_dtype / saved_payload_shape" + ) + + np_dtype = np.dtype(saved_dtype) + raw = np.fromfile(str(payload_path), dtype=np_dtype) + arr = raw.reshape(tuple(saved_shape)) + + if saved_type == "rgb": + if arr.ndim != 3 or arr.shape[0] != 3: + raise RuntimeError(f"Payload RGB inválido, shape={arr.shape}") + + rgb_hwc = chw_to_hwc(arr.astype(np.float32)) + preview_bgr = normalize_float01_to_bgr(rgb_hwc) + desc = f"Reconstruido de RGB salvo | dtype={arr.dtype} | shape={arr.shape}" + return preview_bgr, desc + + if saved_type == "multispec": + if arr.ndim != 3 or arr.shape[0] < 3: + raise RuntimeError(f"Payload MULTISPEC inválido, shape={arr.shape}") + + rgb_hwc = chw_to_hwc(arr[:3].astype(np.float32)) + preview_bgr = normalize_float01_to_bgr(rgb_hwc) + desc = f"Reconstruido de MULTISPEC salvo | dtype={arr.dtype} | shape={arr.shape}" + return preview_bgr, desc + + if saved_type == "raw_native_single": + if arr.ndim == 3 and arr.shape[2] == 3 and arr.dtype == np.uint8: + preview_bgr = arr.copy() + desc = f"Reconstruido de RAW nativo USB | dtype={arr.dtype} | shape={arr.shape}" + return preview_bgr, desc + + stream_meta = meta.get("stream_meta", {}) + source_camera = stream_meta.get("source_camera", {}) or {} + + bayer = source_camera.get("bayer_pattern", meta.get("bayer_pattern", "GBRG")) + bit_depth = int(source_camera.get("bit_depth", 10)) + + sensor_height = int(meta.get("sensor_height")) + sensor_width = int(meta.get("sensor_width")) + + core = RawProcessorCore( + sensor_width=sensor_width, + sensor_height=sensor_height, + bayer_pattern=bayer, + ) + preview = RawProcessorPreview( + sensor_width=sensor_width, + sensor_height=sensor_height, + bayer_pattern=bayer, + ) + + packed = arr + if packed.ndim == 3 and packed.shape[2] == 1: + packed = packed[:, :, 0] + + raw16 = core.unpack_raw10_packed(packed) + preview_bgr = preview.raw16_to_preview_bgr(raw16, bit_depth=bit_depth) + + desc = f"Reconstruido de RAW packed mono | dtype={arr.dtype} | shape={arr.shape} | bayer={bayer} | bit_depth={bit_depth}" + return preview_bgr, desc + + if saved_type == "raw10_packed": + stream_meta = meta.get("stream_meta", {}) or {} + source_camera = stream_meta.get("source_camera", {}) or {} + + bayer = source_camera.get("bayer_pattern", meta.get("bayer_pattern", "GBRG")) + bit_depth = int(source_camera.get("bit_depth", 10)) + + sensor_height = int(meta.get("sensor_height")) + sensor_width = int(meta.get("sensor_width")) + + core = RawProcessorCore( + sensor_width=sensor_width, + sensor_height=sensor_height, + bayer_pattern=bayer, + ) + preview = RawProcessorPreview( + sensor_width=sensor_width, + sensor_height=sensor_height, + bayer_pattern=bayer, + ) + + packed = arr + if packed.ndim == 3 and packed.shape[2] == 1: + packed = packed[:, :, 0] + + raw16 = core.unpack_raw10_packed(packed) + preview_bgr = preview.raw16_to_preview_bgr(raw16, bit_depth=bit_depth) + + desc = ( + f"Reconstruido de RAW10 packed | " + f"dtype={arr.dtype} | shape={arr.shape} | " + f"sensor={sensor_width}x{sensor_height} | " + f"bayer={bayer} | bit_depth={bit_depth}" + ) + return preview_bgr, desc + + raise RuntimeError(f"saved_payload_type não suportado neste script: {saved_type}") + + +def build_panels_from_group(group): + panels = [] + + meta = load_json(group["json"]) + + preview_saved = cv2.imread(str(group["png"]), cv2.IMREAD_COLOR) + if preview_saved is None: + raise RuntimeError(f"Falha ao ler preview PNG: {group['png']}") + panels.append(("Preview salvo", preview_saved, f"{preview_saved.shape[1]}x{preview_saved.shape[0]}")) + + if group["final_raw"] is not None: + img, desc = build_visual_from_saved_payload(group["final_raw"], meta) + panels.append(("Reconstruido (final)", img, desc)) + + for cam_id, path in group["cameras"].items(): + img, desc = build_visual_from_saved_payload(path, meta, cam_id=cam_id) + panels.append((f"{cam_id} reconstruido", img, desc)) + + return panels + + +def compose_panels(panels, max_width=1600): + imgs = [] + + # aplica label + for title, img, subtitle in panels: + img_labeled = put_label(img, title, subtitle) + imgs.append(img_labeled) + + # normaliza tamanho base + max_h = max(img.shape[0] for img in imgs) + + resized = [] + for img in imgs: + scale = max_h / img.shape[0] + w = int(img.shape[1] * scale) + resized.append(cv2.resize(img, (w, max_h), interpolation=cv2.INTER_NEAREST)) + + # ========================= + # Montagem em grid 2x2 + # ========================= + rows = [] + gap = np.full((max_h, 20, 3), 30, dtype=np.uint8) + + for i in range(0, len(resized), 2): + row_imgs = resized[i:i+2] + + # se só tiver 1 imagem na linha, duplica espaço vazio + if len(row_imgs) == 1: + blank = np.zeros_like(row_imgs[0]) + row_imgs.append(blank) + + row = np.hstack([row_imgs[0], gap, row_imgs[1]]) + rows.append(row) + + # junta linhas + gap_h = np.full((20, rows[0].shape[1], 3), 30, dtype=np.uint8) + + canvas = rows[0] + for r in rows[1:]: + canvas = np.vstack([canvas, gap_h, r]) + + # ========================= + # Resize final + # ========================= + if canvas.shape[1] > max_width: + scale = max_width / canvas.shape[1] + canvas = cv2.resize( + canvas, + (int(canvas.shape[1] * scale), int(canvas.shape[0] * scale)), + interpolation=cv2.INTER_AREA + ) + + return canvas + + +def sort_panels(panels): + order = ["Preview salvo", "cam2", "cam0", "cam1"] + + def key(p): + title = p[0].lower() + for i, k in enumerate(order): + if k in title: + return i + return 99 + + return sorted(panels, key=key) + + +def fit_same_height(img_a: np.ndarray, img_b: np.ndarray, target_h: int = None): + if target_h is None: + target_h = max(img_a.shape[0], img_b.shape[0]) + + def resize_to_h(img, h): + scale = h / img.shape[0] + w = int(img.shape[1] * scale) + return cv2.resize(img, (w, h), interpolation=cv2.INTER_NEAREST) + + return resize_to_h(img_a, target_h), resize_to_h(img_b, target_h) + + +def put_label(img: np.ndarray, title: str, subtitle: str = "") -> np.ndarray: + out = img.copy() + cv2.putText(out, title, (12, 28), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 0, 0), 3, cv2.LINE_AA) + cv2.putText(out, title, (12, 28), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 255, 255), 2, cv2.LINE_AA) + + if subtitle: + cv2.putText(out, subtitle, (12, 56), cv2.FONT_HERSHEY_SIMPLEX, 0.55, (0, 0, 0), 3, cv2.LINE_AA) + cv2.putText(out, subtitle, (12, 56), cv2.FONT_HERSHEY_SIMPLEX, 0.55, (255, 255, 255), 1, cv2.LINE_AA) + + return out + + +def resolve_capture_group(input_path: Path): + """ + Resolve todos os arquivos relacionados a uma captura. + + Retorna: + { + "json": Path, + "png": Path, + "final_raw": Path | None, + "cameras": { "cam0": Path, ... } + } + """ + input_path = input_path.resolve() + folder = input_path.parent + + name = input_path.stem + + # remove sufixo _camX se existir + if "_cam" in name: + base_name = name.split("_cam")[0] + else: + base_name = name + + json_path = folder / f"{base_name}.json" + png_path = folder / f"{base_name}.png" + + if not json_path.exists(): + raise FileNotFoundError(f"JSON não encontrado: {json_path}") + if not png_path.exists(): + raise FileNotFoundError(f"PNG não encontrado: {png_path}") + + meta = load_json(json_path) + + group = { + "json": json_path, + "png": png_path, + "final_raw": None, + "cameras": {} + } + + # ========================= + # Caso MULTI payload + # ========================= + if "saved_payload_paths" in meta: + for cam_id, fname in meta["saved_payload_paths"].items(): + path = folder / fname + if path.exists(): + group["cameras"][cam_id] = path + + # ========================= + # Caso payload único (.raw) + # ========================= + elif "saved_payload_path" in meta: + path = folder / meta["saved_payload_path"] + if path.exists(): + group["final_raw"] = path + + return group + + +def resolve_sensor_dims_for_raw10_packed(arr: np.ndarray, cam_meta: dict, meta: dict) -> tuple[int, int]: + """ + Para CSI RAW10 packed: + packed_width = ceil(sensor_width * 5 / 4) + Na prática aqui usamos: + sensor_width = packed_width * 4 // 5 + + Altura permanece a mesma. + """ + packed_h = int(arr.shape[0]) + packed_w = int(arr.shape[1]) + + interface = str(cam_meta.get("interface", "")).upper() + bit_depth = int(cam_meta.get("bit_depth", 10)) + + # USB ou RGB HWC não entra nessa lógica + if interface == "USB": + return packed_w, packed_h + + # Caso esperado: CSI RAW10 packed mono + if bit_depth == 10: + sensor_w = (packed_w * 4) // 5 + sensor_h = packed_h + return sensor_w, sensor_h + + # fallback conservador + return packed_w, packed_h + + +def list_capture_groups_from_dir(folder: Path) -> list[Path]: + """ + Lista todos os JSONs de captura do diretório, ordenados por nome. + Cada JSON representa uma captura. + """ + if not folder.exists() or not folder.is_dir(): + raise FileNotFoundError(f"Diretório não encontrado: {folder}") + + items = sorted(folder.glob("*.json")) + if not items: + raise RuntimeError(f"Nenhum arquivo .json encontrado em: {folder}") + + return items + + +def resolve_navigation_inputs(input_path: Path) -> tuple[list[Path], int]: + """ + Retorna: + entries: lista de JSONs de captura + start_index: índice inicial baseado no input fornecido + """ + input_path = input_path.resolve() + + # Caso 1: usuário passou uma pasta + if input_path.is_dir(): + entries = list_capture_groups_from_dir(input_path) + return entries, 0 + + # Caso 2: usuário passou arquivo + if not input_path.exists(): + raise FileNotFoundError(f"Arquivo não encontrado: {input_path}") + + folder = input_path.parent + entries = list_capture_groups_from_dir(folder) + + # Tenta descobrir qual JSON corresponde ao input + if input_path.suffix.lower() == ".json": + target_json = input_path.resolve() + else: + group = resolve_capture_group(input_path) + target_json = group["json"].resolve() + + try: + idx = entries.index(target_json) + except ValueError: + idx = 0 + + return entries, idx + + +def render_group_to_canvas(json_path: Path, max_width: int): + group = resolve_capture_group(json_path) + meta = load_json(group["json"]) + + panels = build_panels_from_group(group) + panels = sort_panels(panels) + canvas = compose_panels(panels, max_width=max_width) + + info = { + "json": group["json"], + "png": group["png"], + "final_raw": group["final_raw"], + "cameras": group["cameras"], + "meta": meta, + } + return canvas, info + + +def main(): + parser = argparse.ArgumentParser( + description="Valida visualmente payload salvo (.bin/.raw/.json/.png) comparando com o preview .png" + ) + parser.add_argument("--input_path", help="Caminho para .json, .png, .bin, .raw ou diretório") + parser.add_argument("--max-width", type=int, default=1600, help="Largura máxima da janela final") + args = parser.parse_args() + + input_path = Path(args.input_path) + entries, current_idx = resolve_navigation_inputs(input_path) + + window_name = "Validacao do payload salvo | A=anterior | D=proximo | Q/Esc=sair" + cv2.namedWindow(window_name, cv2.WINDOW_NORMAL) + + while True: + current_json = entries[current_idx] + canvas, info = render_group_to_canvas(current_json, max_width=args.max_width) + + # Cabeçalho adicional na imagem + overlay = canvas.copy() + text = f"{current_idx + 1}/{len(entries)} | {current_json.name}" + cv2.putText(overlay, text, (12, overlay.shape[0] - 16), + cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 0, 0), 3, cv2.LINE_AA) + cv2.putText(overlay, text, (12, overlay.shape[0] - 16), + cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 255, 0), 1, cv2.LINE_AA) + + cv2.imshow(window_name, overlay) + + meta = info["meta"] + print("=" * 60) + print(f"[{current_idx + 1}/{len(entries)}]") + print("Entrada JSON :", info["json"]) + print("PNG :", info["png"]) + print("Final RAW :", info["final_raw"]) + print("Câmeras :", {k: str(v) for k, v in info["cameras"].items()}) + print("saved_payload_type :", meta.get("saved_payload_type")) + print("saved_payload_dtype:", meta.get("saved_payload_dtype")) + print("saved_payload_shape:", meta.get("saved_payload_shape")) + print("saved_payload_dtypes:", meta.get("saved_payload_dtypes")) + print("saved_payload_shapes:", meta.get("saved_payload_shapes")) + print("stream frame_type :", (meta.get("stream_meta") or {}).get("frame_type")) + print("=" * 60) + + k = cv2.waitKey(0) & 0xFF + + if k in (ord("q"), ord("Q"), 27): + break + elif k in (ord("d"), ord("D")): + current_idx = min(current_idx + 1, len(entries) - 1) + elif k in (ord("a"), ord("A")): + current_idx = max(current_idx - 1, 0) + + cv2.destroyAllWindows() + + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/Python/OAK/datasets/oak-fcc-3/manual_fusion_calibrator.py b/Python/OAK/datasets/oak-fcc-3/manual_fusion_calibrator.py new file mode 100644 index 000000000..4efa19039 --- /dev/null +++ b/Python/OAK/datasets/oak-fcc-3/manual_fusion_calibrator.py @@ -0,0 +1,689 @@ +import os +import json +import time +import argparse +from datetime import datetime + +import cv2 +import numpy as np + +from oak_fcc3_client import OakFcc3Client as MultiSpectralClient + + +# ============================================================ +# Helpers gerais +# ============================================================ + +def now_str() -> str: + return datetime.now().strftime("%Y-%m-%d %H:%M:%S") + + +def ensure_dir(path: str): + os.makedirs(path, exist_ok=True) + + +def overlay_hud( + img_bgr: np.ndarray, + lines: list[str], + x: int = 12, + y: int = 22, + font_scale: float = 0.6, + line_step: int = 24, +): + yy = y + for s in lines: + cv2.putText(img_bgr, s, (x, yy), cv2.FONT_HERSHEY_SIMPLEX, font_scale, (0, 0, 0), 3, cv2.LINE_AA) + cv2.putText(img_bgr, s, (x, yy), cv2.FONT_HERSHEY_SIMPLEX, font_scale, (255, 255, 255), 1, cv2.LINE_AA) + yy += line_step + + +def normalize_gray01(img: np.ndarray) -> np.ndarray: + arr = img.astype(np.float32) + mn = float(arr.min()) + mx = float(arr.max()) + if mx <= mn + 1e-9: + return np.zeros_like(arr, dtype=np.float32) + return (arr - mn) / (mx - mn) + + +def to_bgr_u8_from_rgb01(rgb01: np.ndarray) -> np.ndarray: + rgb_u8 = np.clip(rgb01 * 255.0, 0, 255).astype(np.uint8) + return cv2.cvtColor(rgb_u8, cv2.COLOR_RGB2BGR) + + +def gray_to_color_bgr(gray01: np.ndarray, color_name: str) -> np.ndarray: + g = np.clip(gray01 * 255.0, 0, 255).astype(np.uint8) + z = np.zeros_like(g, dtype=np.uint8) + + color_name = color_name.upper() + if color_name == "RE": + # vermelho artificial + rgb = np.stack([g, z, z], axis=2) + elif color_name == "NIR": + # ciano artificial + rgb = np.stack([z, g, g], axis=2) + else: + rgb = np.stack([g, g, g], axis=2) + + return cv2.cvtColor(rgb, cv2.COLOR_RGB2BGR) + + +def apply_affine(img: np.ndarray, dx: int, dy: int, theta_deg: float = 0.0) -> np.ndarray: + h, w = img.shape[:2] + center = (w * 0.5, h * 0.5) + M = cv2.getRotationMatrix2D(center, theta_deg, 1.0) + M[0, 2] += dx + M[1, 2] += dy + + if img.ndim == 2: + return cv2.warpAffine( + img, + M, + (w, h), + flags=cv2.INTER_LINEAR, + borderMode=cv2.BORDER_CONSTANT, + borderValue=0, + ) + return cv2.warpAffine( + img, + M, + (w, h), + flags=cv2.INTER_LINEAR, + borderMode=cv2.BORDER_CONSTANT, + borderValue=(0, 0, 0), + ) + + +def apply_homography(img: np.ndarray, H) -> np.ndarray: + if H is None: + return img + + h, w = img.shape[:2] + H = np.asarray(H, dtype=np.float32) + + return cv2.warpPerspective( + img, + H, + (w, h), + flags=cv2.INTER_LINEAR, + borderMode=cv2.BORDER_CONSTANT, + borderValue=0 if img.ndim == 2 else (0, 0, 0), + ) + + +def build_overlay_fuse( + rgb01: np.ndarray, + spec01: np.ndarray | None, + spec_name: str, + dx: int, + dy: int, + theta_deg: float = 0.0, + alpha: float = 0.45, + calibration_mode: str = "manual_affine", + H=None, +): + base_bgr = to_bgr_u8_from_rgb01(rgb01) + if spec01 is None: + return base_bgr + + if calibration_mode == "homography": + warped = apply_homography(spec01, H) + else: + warped = apply_affine(spec01, dx, dy, theta_deg) + + spec_bgr = gray_to_color_bgr(warped, spec_name) + fused = cv2.addWeighted(base_bgr, 1.0 - alpha, spec_bgr, alpha, 0.0) + return fused + + +def resize_if_needed(img: np.ndarray, target_hw: tuple[int, int]) -> np.ndarray: + target_h, target_w = target_hw + if img.shape[:2] == (target_h, target_w): + return img + interp = cv2.INTER_LINEAR + return cv2.resize(img, (target_w, target_h), interpolation=interp) + + +def stack_2x2(a: np.ndarray, b: np.ndarray, c: np.ndarray, d: np.ndarray) -> np.ndarray: + h = max(a.shape[0], b.shape[0], c.shape[0], d.shape[0]) + w = max(a.shape[1], b.shape[1], c.shape[1], d.shape[1]) + + def fit(img): + if img.shape[:2] != (h, w): + return cv2.resize(img, (w, h), interpolation=cv2.INTER_NEAREST) + return img + + a = fit(a) + b = fit(b) + c = fit(c) + d = fit(d) + top = np.hstack([a, b]) + bottom = np.hstack([c, d]) + return np.vstack([top, bottom]) + + +def build_empty_panel_like(ref_bgr: np.ndarray, title: str) -> np.ndarray: + img = np.zeros_like(ref_bgr) + overlay_hud(img, [title, "sem frame disponivel"], x=18, y=40, font_scale=0.8, line_step=34) + return img + + +def validate_module_ready(status: dict, frame_type: str, raw_policy: str, capture_mode: str): + if not status.get("ok", True): + raise RuntimeError(f"Status inválido retornado pelo módulo: {status}") + + active_ids = list(status.get("active_camera_ids", [])) + active_count = int(status.get("camera_count_active", 0)) + + if frame_type == "RAW_BRUTO": + if raw_policy == "require_triple": + missing = [cid for cid in ("cam0", "cam1", "cam2") if cid not in active_ids] + if missing: + raise RuntimeError( + f"RAW_BRUTO com política require_triple exige três câmeras ativas. " + f"Faltando: {missing}. Ativas atuais: {active_ids}" + ) + else: + if active_count < 1: + raise RuntimeError("RAW_BRUTO requer ao menos uma câmera ativa, mas nenhuma foi detectada.") + return + + raise RuntimeError(f"frame_type desconhecido para validação: {frame_type}") + + +# ============================================================ +# Persistência dos offsets +# ============================================================ + +def default_offsets_payload(args, effective_capture_mode: str): + return { + "schema": "manual_multispec_offsets_v1", + "saved_at": now_str(), + "pi_host": args.pi_host, + "pc_host": args.pc_host, + "stream_port": args.stream_port, + "frame_type": "RAW_BRUTO", + "capture_mode_requested": args.capture_mode, + "capture_mode_effective": effective_capture_mode, + "raw_policy": args.raw_policy, + "sensor_width": args.width, + "sensor_height": args.height, + "bayer_pattern": args.bayer, + "reference_camera": "cam2", + "baseline_mm": args.baseline_mm, + "alignment_mode": "manual_affine", + "manual_offsets": { + "cam0": {"dx": 0, "dy": 0, "theta_deg": 0.0}, + "cam1": {"dx": 0, "dy": 0, "theta_deg": 0.0}, + }, + "homographies": { + "cam0_to_cam2": None, + "cam1_to_cam2": None, + }, + "notes": args.notes or "", + } + + +def load_offsets_json(path: str, args, effective_capture_mode: str): + if not path or not os.path.isfile(path): + return default_offsets_payload(args, effective_capture_mode) + + with open(path, "r", encoding="utf-8") as f: + data = json.load(f) + + data.setdefault("schema", "manual_multispec_offsets_v1") + data.setdefault("reference_camera", "cam2") + data.setdefault("baseline_mm", args.baseline_mm) + data.setdefault("alignment_mode", "manual_affine") + data.setdefault("manual_offsets", {}) + data["manual_offsets"].setdefault("cam0", {"dx": 0, "dy": 0, "theta_deg": 0.0}) + data["manual_offsets"].setdefault("cam1", {"dx": 0, "dy": 0, "theta_deg": 0.0}) + data.setdefault("homographies", {}) + data["homographies"].setdefault("cam0_to_cam2", None) + data["homographies"].setdefault("cam1_to_cam2", None) + return data + + +def save_offsets_json(path: str, data: dict): + ensure_dir(os.path.dirname(path) or ".") + data = dict(data) + data["saved_at"] = now_str() + with open(path, "w", encoding="utf-8") as f: + json.dump(data, f, ensure_ascii=False, indent=2) + + +# ============================================================ +# Main UI +# ============================================================ + +def main(): + parser = argparse.ArgumentParser( + description="Calibrador manual de offsets para fusão RGB/RE/NIR a partir do stream RAW_BRUTO.", + formatter_class=argparse.ArgumentDefaultsHelpFormatter, + ) + parser.add_argument("--pi_host", default="192.168.105.6") + parser.add_argument("--pc_host", default="192.168.105.5") + parser.add_argument("--stream_port", type=int, default=6001) + parser.add_argument("--server_port", type=int, default=5000) + parser.add_argument("--fps", type=int, default=20) + parser.add_argument("--width", type=int, default=640) + parser.add_argument("--height", type=int, default=480) + parser.add_argument("--bayer", default="GBRG", choices=["GBRG", "GRBG", "RGGB", "BGGR"]) + parser.add_argument("--capture_mode", default="AUTO", choices=["AUTO", "SINGLE", "DOUBLE", "TRIPLE"]) + parser.add_argument("--raw_policy", default="allow_single", choices=["allow_single", "require_triple"]) + parser.add_argument("--baseline_mm", type=float, default=75.0) + parser.add_argument("--preview_scale", type=float, default=1.0) + parser.add_argument("--step", type=int, default=1, help="Passo inicial em pixels ao usar as setas.") + parser.add_argument("--alpha", type=float, default=0.45, help="Alpha do overlay sobre RGB.") + parser.add_argument("--angle_step", type=float, default=0.10, help="Passo angular em graus para rotação manual.") + parser.add_argument("--out_json", default="calibration/manual_offsets.json") + parser.add_argument("--load_json", default="", help="Se informado, carrega offsets iniciais deste arquivo.") + parser.add_argument("--notes", default="") + args = parser.parse_args() + + def on_mouse(event, x, y, flags, param): + nonlocal last_msg, last_msg_t + + if event != cv2.EVENT_LBUTTONDOWN: + return + + if calibration_mode != "homography": + return + + if selected_cam not in ("cam0", "cam1"): + return + + rgb_rect = panel_rects.get("rgb") + spec_rect = panel_rects.get(selected_cam) + + def inside(rect, px, py): + if rect is None: + return False + x0, y0, x1, y1 = rect + return x0 <= px < x1 and y0 <= py < y1 + + def to_local(rect, px, py): + x0, y0, x1, y1 = rect + return float(px - x0), float(py - y0) + + if inside(spec_rect, x, y): + pt = to_local(spec_rect, x, y) + #if len(selected_points_spec[selected_cam]) < 4: + selected_points_spec[selected_cam].append(pt) + last_msg = f"{selected_cam}: ponto SPEC #{len(selected_points_spec[selected_cam])}" + last_msg_t = time.time() + return + + if inside(rgb_rect, x, y): + pt = to_local(rgb_rect, x, y) + #if len(selected_points_rgb[selected_cam]) < 4: + selected_points_rgb[selected_cam].append(pt) + last_msg = f"{selected_cam}: ponto RGB #{len(selected_points_rgb[selected_cam])}" + last_msg_t = time.time() + return + + effective_capture_mode = args.capture_mode + + offsets_data = load_offsets_json(args.load_json, args, effective_capture_mode) + offsets = offsets_data["manual_offsets"] + + selected_cam = "cam0" + calibration_mode = offsets_data.get("alignment_mode", "manual_affine") + + selected_points_spec = { + "cam0": [], + "cam1": [], + } + selected_points_rgb = { + "cam0": [], + "cam1": [], + } + + panel_rects = { + "fuse": None, + "rgb": None, + "cam0": None, + "cam1": None, + } + last_msg = "" + last_msg_t = 0.0 + last_frame_id = -1 + fps_view = 0.0 + fps_stream = 0.0 + t_view_fps = time.time() + t_stream_fps = time.time() + view_frames = 0 + stream_frames_accum = 0 + last_stream_frame_id = None + + decoded_last = {} + window_name = "Manual Fusion Calibrator" + cv2.namedWindow(window_name, cv2.WINDOW_NORMAL) + cv2.setMouseCallback(window_name, on_mouse) + + try: + with MultiSpectralClient( + pi_host=args.pi_host, + pc_host=args.pc_host, + server_port=args.server_port, + stream_port=args.stream_port, + width=args.width, + height=args.height, + bayer=args.bayer, + fps=args.fps, + frame_type="RAW_BRUTO", + output_dtype="uint8", + capture_mode=effective_capture_mode, + raw_policy=args.raw_policy, + module_calibration_json=None, + ) as cam: + while True: + t0 = time.time() + + frame, meta, decoded = cam.get_next_decoded(timeout=2.0) + + if meta is not None and frame is not None and meta.get("frame_id") != last_frame_id: + last_frame_id = meta["frame_id"] + + if not isinstance(frame, dict): + raise RuntimeError("Este calibrador espera RAW_BRUTO multi-payload como dict de câmeras.") + + decoded_last = decoded + + curr_frame_id = meta.get("frame_id") + if curr_frame_id is not None and last_stream_frame_id != curr_frame_id: + stream_frames_accum += 1 + last_stream_frame_id = curr_frame_id + + dt_stream = time.time() - t_stream_fps + if dt_stream >= 1.0: + fps_stream = stream_frames_accum / dt_stream + stream_frames_accum = 0 + t_stream_fps = time.time() + + view_frames += 1 + dt_view = time.time() - t_view_fps + if dt_view >= 1.0: + fps_view = view_frames / dt_view + view_frames = 0 + t_view_fps = time.time() + + if decoded_last: + rgb01 = decoded_last.get("cam2", {}).get("image") + re01 = decoded_last.get("cam0", {}).get("image") + nir01 = decoded_last.get("cam1", {}).get("image") + + if rgb01 is None: + # fallback para exibição quando não houver RGB + if re01 is not None: + rgb01 = np.stack([re01, re01, re01], axis=2) + elif nir01 is not None: + rgb01 = np.stack([nir01, nir01, nir01], axis=2) + else: + rgb01 = np.zeros((args.height, args.width, 3), dtype=np.float32) + + base_h, base_w = rgb01.shape[:2] + if re01 is not None: + re01 = resize_if_needed(re01, (base_h, base_w)) + if nir01 is not None: + nir01 = resize_if_needed(nir01, (base_h, base_w)) + + rgb_panel = to_bgr_u8_from_rgb01(rgb01) + re_panel = gray_to_color_bgr(re01, "RE") if re01 is not None else build_empty_panel_like(rgb_panel, "RE") + nir_panel = gray_to_color_bgr(nir01, "NIR") if nir01 is not None else build_empty_panel_like(rgb_panel, "NIR") + + active_spec_name = "RE" if selected_cam == "cam0" else "NIR" + active_spec = re01 if selected_cam == "cam0" else nir01 + dx = int(offsets.get(selected_cam, {}).get("dx", 0)) + dy = int(offsets.get(selected_cam, {}).get("dy", 0)) + theta_deg = float(offsets.get(selected_cam, {}).get("theta_deg", 0.0)) + + H_key = f"{selected_cam}_to_cam2" + H = offsets_data.get("homographies", {}).get(H_key) + + fuse_panel = build_overlay_fuse( + rgb01, + active_spec, + active_spec_name, + dx, + dy, + theta_deg=theta_deg, + alpha=args.alpha, + calibration_mode=calibration_mode, + H=H, + ) + + spec_pts = len(selected_points_spec[selected_cam]) + rgb_pts = len(selected_points_rgb[selected_cam]) + + lines_fuse = [ + f"FUSE: RGB + {active_spec_name}", + f"mode={calibration_mode} | selecionada={selected_cam}", + f"dx={dx} | dy={dy} | theta={theta_deg:.2f}g | step={args.step} | ang_step={args.angle_step:.2f}g", + f"pts_spec={spec_pts} | pts_rgb={rgb_pts} | min=4 | fps_stream={fps_stream:.1f} | fps_view={fps_view:.1f}" + ] + overlay_hud(fuse_panel, lines_fuse) + + lines_rgb = ["RGB (cam2)"] + overlay_hud(rgb_panel, lines_rgb) + + re_dx = int(offsets.get("cam0", {}).get("dx", 0)) + re_dy = int(offsets.get("cam0", {}).get("dy", 0)) + re_theta = float(offsets.get("cam0", {}).get("theta_deg", 0.0)) + nir_dx = int(offsets.get("cam1", {}).get("dx", 0)) + nir_dy = int(offsets.get("cam1", {}).get("dy", 0)) + nir_theta = float(offsets.get("cam1", {}).get("theta_deg", 0.0)) + overlay_hud(re_panel, [f"RE (cam0) | dx={re_dx} dy={re_dy} th={re_theta:.2f}g", "2 seleciona RE"], y=24) + overlay_hud(nir_panel, [f"NIR (cam1) | dx={nir_dx} dy={nir_dy} th={nir_theta:.2f}g", "3 seleciona NIR"], y=24) + + ph = max(fuse_panel.shape[0], rgb_panel.shape[0], re_panel.shape[0], nir_panel.shape[0]) + pw = max(fuse_panel.shape[1], rgb_panel.shape[1], re_panel.shape[1], nir_panel.shape[1]) + + def fit_panel(img): + if img.shape[:2] != (ph, pw): + return cv2.resize(img, (pw, ph), interpolation=cv2.INTER_NEAREST) + return img + + fuse_panel = fit_panel(fuse_panel) + rgb_panel = fit_panel(rgb_panel) + re_panel = fit_panel(re_panel) + nir_panel = fit_panel(nir_panel) + + panel_rects["fuse"] = (0, 0, pw, ph) + panel_rects["rgb"] = (pw, 0, pw * 2, ph) + panel_rects["cam0"] = (0, ph, pw, ph * 2) + panel_rects["cam1"] = (pw, ph, pw * 2, ph * 2) + + top = np.hstack([fuse_panel, rgb_panel]) + bottom = np.hstack([re_panel, nir_panel]) + board = np.vstack([top, bottom]) + + help_lines = [ + "M=manual_affine | H=homography | clique pares correspondentes | >=4 pares | SPACE=salva | C=limpa pts | Z=zera sel | X=zera tudo", + "A/W/S/D movem | J/L rotacionam | O/P muda passo angular | I/U remove ultimo ponto | ENTER calcula H | TAB alterna camera | Q/Esc sai", + ] + overlay_hud(board, help_lines, x=16, y=board.shape[0] - 44, font_scale=0.55, line_step=20) + + if last_msg and (time.time() - last_msg_t) < 2.5: + cv2.putText(board, last_msg, (16, board.shape[0] - 72), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2, cv2.LINE_AA) + + if args.preview_scale != 1.0: + board = cv2.resize( + board, + (int(board.shape[1] * args.preview_scale), int(board.shape[0] * args.preview_scale)), + interpolation=cv2.INTER_NEAREST, + ) + + if calibration_mode == "homography": + color_spec = (0, 255, 255) + color_rgb = (0, 255, 0) + + for idx, pt in enumerate(selected_points_spec[selected_cam]): + rect = panel_rects[selected_cam] + if rect is not None: + x0, y0, _, _ = rect + px = int(x0 + pt[0]) + py = int(y0 + pt[1]) + cv2.circle(board, (px, py), 5, color_spec, -1) + cv2.putText(board, str(idx + 1), (px + 6, py - 6), + cv2.FONT_HERSHEY_SIMPLEX, 0.5, color_spec, 1, cv2.LINE_AA) + + for idx, pt in enumerate(selected_points_rgb[selected_cam]): + rect = panel_rects["rgb"] + if rect is not None: + x0, y0, _, _ = rect + px = int(x0 + pt[0]) + py = int(y0 + pt[1]) + cv2.circle(board, (px, py), 5, color_rgb, -1) + cv2.putText(board, str(idx + 1), (px + 6, py - 6), + cv2.FONT_HERSHEY_SIMPLEX, 0.5, color_rgb, 1, cv2.LINE_AA) + + cv2.imshow(window_name, board) + else: + blank = np.zeros((720, 1280, 3), dtype=np.uint8) + overlay_hud(blank, ["Aguardando frames do módulo..."], x=40, y=80, font_scale=1.0, line_step=34) + cv2.imshow(window_name, blank) + + k = cv2.waitKey(1) & 0xFF + if k in (ord("q"), ord("Q"), 27): + break + elif k in (ord("m"), ord("M")): + calibration_mode = "manual_affine" + offsets_data["alignment_mode"] = calibration_mode + last_msg = "Modo: manual_affine" + last_msg_t = time.time() + elif k in (ord("h"), ord("H")): + calibration_mode = "homography" + offsets_data["alignment_mode"] = calibration_mode + last_msg = "Modo: homography" + last_msg_t = time.time() + elif k in (ord("c"), ord("C")): + selected_points_spec[selected_cam] = [] + selected_points_rgb[selected_cam] = [] + last_msg = f"Pontos limpos: {selected_cam}" + last_msg_t = time.time() + elif k == 13: # ENTER + spec_pts = selected_points_spec[selected_cam] + rgb_pts = selected_points_rgb[selected_cam] + + if len(spec_pts) >= 4 and len(rgb_pts) >= 4 and len(spec_pts) == len(rgb_pts): + src = np.array(spec_pts, dtype=np.float32) + dst = np.array(rgb_pts, dtype=np.float32) + + H, status = cv2.findHomography(src, dst, method=cv2.RANSAC) + if H is not None: + offsets_data.setdefault("homographies", {}) + offsets_data["homographies"][f"{selected_cam}_to_cam2"] = H.tolist() + inliers = int(status.sum()) if status is not None else len(spec_pts) + last_msg = f"H calculada para {selected_cam} | pts={len(spec_pts)} | inliers={inliers}" + else: + last_msg = f"Falha ao calcular H para {selected_cam}" + else: + last_msg = f"{selected_cam}: precisa de >=4 pares e mesmo numero de pontos" + + last_msg_t = time.time() + elif k == ord("2"): + if "cam0" in decoded_last: + selected_cam = "cam0" + last_msg = "Selecionada: cam0 / RE" + else: + last_msg = "cam0 / RE nao disponivel neste frame" + last_msg_t = time.time() + elif k == ord("3"): + if "cam1" in decoded_last: + selected_cam = "cam1" + last_msg = "Selecionada: cam1 / NIR" + else: + last_msg = "cam1 / NIR nao disponivel neste frame" + last_msg_t = time.time() + elif k == 9: # TAB + choices = [cid for cid in ("cam0", "cam1") if cid in decoded_last] + if len(choices) >= 2: + selected_cam = choices[1] if selected_cam == choices[0] else choices[0] + last_msg = f"Selecionada: {selected_cam}" + last_msg_t = time.time() + elif k in (ord("z"), ord("Z")): + offsets.setdefault(selected_cam, {"dx": 0, "dy": 0, "theta_deg": 0.0}) + offsets[selected_cam]["dx"] = 0 + offsets[selected_cam]["dy"] = 0 + offsets[selected_cam]["theta_deg"] = 0.0 + last_msg = f"Offset zerado: {selected_cam}" + last_msg_t = time.time() + elif k in (ord("x"), ord("X")): + offsets["cam0"] = {"dx": 0, "dy": 0, "theta_deg": 0.0} + offsets["cam1"] = {"dx": 0, "dy": 0, "theta_deg": 0.0} + last_msg = "Todos offsets zerados" + last_msg_t = time.time() + elif k == 32: + # Se uma câmera não apareceu, salva zerada como pedido + if "cam0" not in decoded_last: + offsets["cam0"] = {"dx": 0, "dy": 0, "theta_deg": 0.0} + if "cam1" not in decoded_last: + offsets["cam1"] = {"dx": 0, "dy": 0, "theta_deg": 0.0} + + offsets_data["manual_offsets"] = offsets + save_offsets_json(args.out_json, offsets_data) + last_msg = f"Offsets salvos em: {args.out_json}" + last_msg_t = time.time() + elif k in (ord("+"), ord("=")): + args.step = min(args.step + 1, 50) + last_msg = f"Step -> {args.step}px" + last_msg_t = time.time() + elif k in (ord("-"), ord("_")): + args.step = max(args.step - 1, 1) + last_msg = f"Step -> {args.step}px" + last_msg_t = time.time() + elif k in (ord("a"), ord("A")): + if selected_cam in decoded_last: + offsets.setdefault(selected_cam, {"dx": 0, "dy": 0, "theta_deg": 0.0}) + offsets[selected_cam]["dx"] -= args.step + elif k in (ord("d"), ord("D")): + if selected_cam in decoded_last: + offsets.setdefault(selected_cam, {"dx": 0, "dy": 0, "theta_deg": 0.0}) + offsets[selected_cam]["dx"] += args.step + elif k in (ord("w"), ord("W")): + if selected_cam in decoded_last: + offsets.setdefault(selected_cam, {"dx": 0, "dy": 0, "theta_deg": 0.0}) + offsets[selected_cam]["dy"] -= args.step + elif k in (ord("s"), ord("S")): + if selected_cam in decoded_last: + offsets.setdefault(selected_cam, {"dx": 0, "dy": 0, "theta_deg": 0.0}) + offsets[selected_cam]["dy"] += args.step + elif k in (ord("j"), ord("J")): + if selected_cam in decoded_last: + offsets.setdefault(selected_cam, {"dx": 0, "dy": 0, "theta_deg": 0.0}) + offsets[selected_cam]["theta_deg"] -= args.angle_step + elif k in (ord("l"), ord("L")): + if selected_cam in decoded_last: + offsets.setdefault(selected_cam, {"dx": 0, "dy": 0, "theta_deg": 0.0}) + offsets[selected_cam]["theta_deg"] += args.angle_step + elif k in (ord("o"), ord("O")): + args.angle_step = max(args.angle_step - 0.05, 0.01) + last_msg = f"Angle step -> {args.angle_step:.2f}°" + last_msg_t = time.time() + elif k in (ord("p"), ord("P")): + args.angle_step = min(args.angle_step + 0.05, 5.0) + last_msg = f"Angle step -> {args.angle_step:.2f}°" + last_msg_t = time.time() + elif k in (ord("u"), ord("U")): + if selected_points_spec[selected_cam]: + selected_points_spec[selected_cam].pop() + last_msg = f"Removido ultimo ponto SPEC de {selected_cam}" + last_msg_t = time.time() + elif k in (ord("i"), ord("I")): + if selected_points_rgb[selected_cam]: + selected_points_rgb[selected_cam].pop() + last_msg = f"Removido ultimo ponto RGB de {selected_cam}" + last_msg_t = time.time() + + dt_loop = time.time() - t0 + if dt_loop < 0.001: + time.sleep(0.001) + + finally: + cv2.destroyAllWindows() + print("Fim da calibração manual.") + + +if __name__ == "__main__": + main() diff --git a/Python/OAK/datasets/oak-fcc-3/oak_fcc3_client.py b/Python/OAK/datasets/oak-fcc-3/oak_fcc3_client.py new file mode 100644 index 000000000..7a4addba2 --- /dev/null +++ b/Python/OAK/datasets/oak-fcc-3/oak_fcc3_client.py @@ -0,0 +1,391 @@ +import numpy as np +import cv2 +import json +import os + +from oak_fcc3_service import OakFcc3Service + + +class OakFcc3Client: + def __init__( + self, + pi_host=None, + pc_host=None, + server_port=None, + stream_port=None, + width=640, + height=400, + bayer="GBRG", + fps=30, + frame_type="RAW_BRUTO", + output_dtype="uint8", + capture_mode="AUTO", + raw_policy="allow_single", + module_calibration_json=None, + radiometric_enabled=False, + sync_mode="best", + sync_tolerance_ms=25.0, + **kwargs, + ): + self.width = width + self.height = height + self.bayer = bayer + self.fps = fps + self.frame_type = frame_type + self.output_dtype = output_dtype + self.capture_mode = capture_mode + self.raw_policy = raw_policy + self.module_calibration_json = module_calibration_json + self.module_params = self._load_module_params(module_calibration_json) + self.fusion_config = self.module_params.get("fusion_config", {}) or {} + self.radiometric_enabled = radiometric_enabled + + self.svc = OakFcc3Service( + timeout=10, + fps=fps, + width=width, + height=height, + frame_type=frame_type, + output_dtype=output_dtype, + capture_mode=capture_mode, + raw_policy=raw_policy, + sync_mode=sync_mode, + sync_tolerance_ms=sync_tolerance_ms, + **kwargs, + ) + + self.applied_camera_controls = {} + self.radiometric_controller = None + + def __enter__(self): + self.start() + return self + + def __exit__(self, exc_type, exc, tb): + self.stop() + + def _load_module_params(self, path): + if not path or not os.path.isfile(path): + return {} + + with open(path, "r", encoding="utf-8") as f: + return json.load(f) + + def start(self, print_debug=False): + self.svc.connect() + + resp = self.svc.begin( + frame_type=self.frame_type, + output_dtype=self.output_dtype, + capture_mode=self.capture_mode, + ) + + if print_debug: + print("[OAK CLIENT] START:", resp) + + return resp + + def stop(self): + try: + self.svc.stop() + finally: + self.svc.disconnect() + + def get_status(self): + return self.svc.get_status() + + def get_next_frame(self, timeout=1.0): + return self.svc.capture_frame(timeout=timeout) + + def get_next_decoded(self, timeout=1.0, update_radiometry=True): + raw_frame, meta = self.get_next_frame(timeout=timeout) + + decoded = self.decode_stream_cameras(raw_frame, meta) + + frame_type = str(self.frame_type).upper() + + if frame_type == "RAW_BRUTO": + meta["output_layout"] = "dict_by_camera" + return raw_frame, meta, decoded + + if frame_type == "RGB": + tensor = self.build_rgb_tensor(decoded) + meta["frame_type"] = "RGB" + meta["output_layout"] = "CHW" + meta["channels"] = ["R", "G", "B"] + meta["dtype"] = "float32" + meta["output_dtype"] = "float32" + meta["tensor_shape"] = list(tensor.shape) + return tensor, meta, decoded + + if frame_type == "MULTISPEC": + tensor = self.build_multispec_tensor(decoded) + meta["frame_type"] = "MULTISPEC" + meta["output_layout"] = "CHW" + meta["channels"] = ["R", "G", "B", "RE", "NIR"] + meta["fusion_applied"] = True + meta["fusion_alignment_mode"] = self.fusion_config.get("alignment_mode", "identity") + meta["module_calibration_json"] = self.module_calibration_json + meta["dtype"] = "float32" + meta["output_dtype"] = "float32" + meta["tensor_shape"] = list(tensor.shape) + return tensor, meta, decoded + + raise RuntimeError(f"frame_type não suportado: {self.frame_type}") + + def build_rgb_tensor(self, decoded): + if "cam2" not in decoded: + raise RuntimeError("RGB exige cam2 disponível.") + + rgb01 = decoded["cam2"]["image"] + + if rgb01.ndim != 3 or rgb01.shape[2] != 3: + raise RuntimeError(f"cam2 RGB inválida: shape={rgb01.shape}") + + tensor = np.transpose(rgb01.astype(np.float32), (2, 0, 1)) + return np.ascontiguousarray(tensor) + + def build_multispec_tensor(self, decoded): + if "cam2" not in decoded: + raise RuntimeError("MULTISPEC exige cam2/RGB disponível.") + if "cam0" not in decoded: + raise RuntimeError("MULTISPEC exige cam0/RE disponível.") + if "cam1" not in decoded: + raise RuntimeError("MULTISPEC exige cam1/NIR disponível.") + + rgb01 = decoded["cam2"]["image"] + re01 = decoded["cam0"]["image"] + nir01 = decoded["cam1"]["image"] + + if rgb01.ndim != 3 or rgb01.shape[2] != 3: + raise RuntimeError(f"cam2 RGB inválida: shape={rgb01.shape}") + + h, w = rgb01.shape[:2] + + re01 = self._align_spectral_to_rgb(re01, "cam0", h, w) + nir01 = self._align_spectral_to_rgb(nir01, "cam1", h, w) + + r = rgb01[:, :, 0] + g = rgb01[:, :, 1] + b = rgb01[:, :, 2] + + tensor = np.stack( + [ + r, + g, + b, + re01, + nir01, + ], + axis=0, + ).astype(np.float32) + + return np.ascontiguousarray(tensor) + + @staticmethod + def _resize_gray_to(img01, h, w): + if img01 is None: + raise RuntimeError("Canal espectral ausente.") + + if img01.ndim == 3: + img01 = img01[:, :, 0] + + if img01.shape[:2] != (h, w): + img01 = cv2.resize(img01, (w, h), interpolation=cv2.INTER_LINEAR) + + return np.clip(img01.astype(np.float32), 0.0, 1.0) + + def _align_spectral_to_rgb(self, img01, cam_id, h, w): + if img01 is None: + raise RuntimeError(f"Canal ausente: {cam_id}") + + if img01.ndim == 3: + img01 = img01[:, :, 0] + + if img01.shape[:2] != (h, w): + img01 = cv2.resize(img01, (w, h), interpolation=cv2.INTER_LINEAR) + + cfg = self.fusion_config or {} + mode = cfg.get("alignment_mode", "identity") + + if mode == "identity": + return np.clip(img01.astype(np.float32), 0.0, 1.0) + + if mode == "manual_offset": + offs = cfg.get("manual_offsets", {}).get(cam_id, {}) + dx = int(offs.get("dx", 0)) + dy = int(offs.get("dy", 0)) + + M = np.float32([[1, 0, dx], [0, 1, dy]]) + + out = cv2.warpAffine( + img01, + M, + (w, h), + flags=cv2.INTER_LINEAR, + borderMode=cv2.BORDER_CONSTANT, + borderValue=0, + ) + + return np.clip(out.astype(np.float32), 0.0, 1.0) + + if mode == "manual_affine": + offs = cfg.get("manual_offsets", {}).get(cam_id, {}) + dx = int(offs.get("dx", 0)) + dy = int(offs.get("dy", 0)) + theta_deg = float(offs.get("theta_deg", 0.0)) + + center = (w * 0.5, h * 0.5) + M = cv2.getRotationMatrix2D(center, theta_deg, 1.0) + M[0, 2] += dx + M[1, 2] += dy + + out = cv2.warpAffine( + img01, + M, + (w, h), + flags=cv2.INTER_LINEAR, + borderMode=cv2.BORDER_CONSTANT, + borderValue=0, + ) + + return np.clip(out.astype(np.float32), 0.0, 1.0) + + if mode == "homography": + H = cfg.get("homographies", {}).get(f"{cam_id}_to_cam2") + + if H is None: + return np.clip(img01.astype(np.float32), 0.0, 1.0) + + H = np.asarray(H, dtype=np.float32) + + if H.shape != (3, 3): + raise RuntimeError(f"Homografia inválida para {cam_id}: shape={H.shape}") + + out = cv2.warpPerspective( + img01, + H, + (w, h), + flags=cv2.INTER_LINEAR, + borderMode=cv2.BORDER_CONSTANT, + borderValue=0, + ) + + return np.clip(out.astype(np.float32), 0.0, 1.0) + + raise RuntimeError(f"alignment_mode inválido: {mode}") + + def decode_stream_cameras(self, frame, meta): + """ + Compatível com os calibradores antigos. + + Retorna: + decoded["cam2"]["image"] = RGB float32 HWC [0..1] + decoded["cam0"]["image"] = RE float32 HW [0..1] + decoded["cam1"]["image"] = NIR float32 HW [0..1] + """ + decoded = {} + + if not isinstance(frame, dict): + raise RuntimeError("OakFcc3Client espera frame como dict por câmera.") + + camera_info = meta.get("camera_info", {}) or {} + + for cam_id, img in frame.items(): + info = camera_info.get(cam_id, {}) or {} + role = info.get("role", cam_id) + + img01 = self._frame_to_float01(cam_id, img, role) + + if role == "rgb": + name = "RGB" + elif role == "re": + name = "RE" + elif role == "nir": + name = "NIR" + else: + name = role.upper() + + decoded[cam_id] = { + "name": name, + "role": role, + "image": img01, + "meta": { + "cam_id": cam_id, + "role": role, + "socket": info.get("socket"), + "sensor": info.get("sensor"), + "timestamp": (meta.get("timestamps") or {}).get(cam_id), + "shape": list(img.shape), + "dtype": str(img.dtype), + }, + } + + return decoded + + def build_preview_from_raw_payload(self, frame, meta): + """ + Usado pelo capture antigo. + Retorna: + preview_bgr + payload_float_preview + preview_source_id + """ + decoded = self.decode_stream_cameras(frame, meta) + + if "cam2" in decoded: + rgb01 = decoded["cam2"]["image"] + preview_bgr = self._rgb01_to_bgr(rgb01) + return preview_bgr, rgb01.copy(), "cam2" + + first_id = list(decoded.keys())[0] + img01 = decoded[first_id]["image"] + + if img01.ndim == 2: + preview_bgr = self._gray01_to_bgr(img01) + else: + preview_bgr = self._rgb01_to_bgr(img01) + + return preview_bgr, img01.copy(), first_id + + def _frame_to_float01(self, cam_id, img, role): + if img is None: + return None + + arr = img + + if arr.dtype == np.uint8: + arr01 = arr.astype(np.float32) / 255.0 + elif arr.dtype == np.uint16: + arr01 = arr.astype(np.float32) / 65535.0 + else: + arr01 = arr.astype(np.float32) + if arr01.max() > 1.5: + arr01 = arr01 / 255.0 + + arr01 = np.clip(arr01, 0.0, 1.0) + + if role == "rgb": + # DepthAI/OpenCV entrega BGR HWC. Calibradores esperam RGB HWC. + if arr01.ndim == 3 and arr01.shape[2] == 3: + arr01 = cv2.cvtColor((arr01 * 255).astype(np.uint8), cv2.COLOR_BGR2RGB).astype(np.float32) / 255.0 + elif arr01.ndim == 2: + arr01 = np.stack([arr01, arr01, arr01], axis=2) + + return arr01 + + # Espectrais devem virar mono HW. + if arr01.ndim == 3: + arr01 = cv2.cvtColor((arr01 * 255).astype(np.uint8), cv2.COLOR_BGR2GRAY).astype(np.float32) / 255.0 + + return arr01 + + @staticmethod + def _rgb01_to_bgr(rgb01): + rgb_u8 = np.clip(rgb01 * 255.0, 0, 255).astype(np.uint8) + return cv2.cvtColor(rgb_u8, cv2.COLOR_RGB2BGR) + + @staticmethod + def _gray01_to_bgr(gray01): + g = np.clip(gray01 * 255.0, 0, 255).astype(np.uint8) + return cv2.cvtColor(g, cv2.COLOR_GRAY2BGR) \ No newline at end of file diff --git a/Python/OAK/datasets/oak-fcc-3/oak_fcc3_manager.py b/Python/OAK/datasets/oak-fcc-3/oak_fcc3_manager.py new file mode 100644 index 000000000..c1d7dcf58 --- /dev/null +++ b/Python/OAK/datasets/oak-fcc-3/oak_fcc3_manager.py @@ -0,0 +1,363 @@ +import time +from collections import deque +import depthai as dai +import numpy as np + + +class OakFcc3Manager: + def __init__( + self, + fps=30, + width=640, + height=400, + frame_type="RAW_BRUTO", + output_dtype="uint8", + capture_mode="AUTO", + raw_policy="allow_single", + roles=None, + sync_mode="best", + sync_tolerance_ms=12.0, + buffer_size=8, + ): + self.fps = fps + self.width = width + self.height = height + self.size = (width, height) + + self.frame_type = frame_type + self.output_dtype = output_dtype + self.capture_mode = capture_mode + self.raw_policy = raw_policy + + self.roles = roles or { + "CAM_A": "rgb", + "CAM_B": "re", + "CAM_C": "nir", + } + + self.sync_mode = sync_mode + self.sync_tolerance_ms = sync_tolerance_ms + self.buffer_size = buffer_size + + self.device = None + self.pipeline = None + self.queues = {} + self.buffers = {} + self.camera_info = {} + + self.running = False + self.frame_id = 0 + self.applied_camera_controls = {} + + def __enter__(self): + self.start() + return self + + def __exit__(self, exc_type, exc, tb): + self.stop() + + def list_cameras(self): + with dai.Device() as dev: + result = [] + for f in dev.getConnectedCameraFeatures(): + result.append({ + "socket": f.socket.name, + "sensor": f.sensorName, + "role": self.roles.get(f.socket.name, "unknown"), + }) + return result + + def start(self): + if self.running: + return + + self.device = dai.Device() + self.pipeline = dai.Pipeline(self.device) + + features = self.device.getConnectedCameraFeatures() + + self.queues.clear() + self.buffers.clear() + self.camera_info.clear() + + for f in features: + socket = f.socket + socket_name = socket.name + role = self.roles.get(socket_name, "unknown") + + print(f"[OAK] Criando câmera {socket_name} sensor={f.sensorName} role={role}") + + cam = self.pipeline.create(dai.node.Camera).build(socket) + + out = cam.requestOutput( + self.size, + fps=self.fps + ) + + q = out.createOutputQueue() + cam_id = self._socket_to_cam_id(socket_name) + + self.queues[cam_id] = q + self.buffers[cam_id] = deque(maxlen=self.buffer_size) + + self.camera_info[cam_id] = { + "id": cam_id, + "socket": socket_name, + "sensor": f.sensorName, + "role": role, + } + + self._validate_capture_mode() + + self.pipeline.start() + self.running = True + + def stop(self): + if not self.running: + return + + try: + if self.pipeline is not None: + self.pipeline.stop() + except Exception: + pass + + try: + if self.device is not None: + self.device.close() + except Exception: + pass + + self.pipeline = None + self.device = None + self.queues.clear() + self.buffers.clear() + self.camera_info.clear() + self.running = False + + def get_status(self): + return { + "backend": "oak_fcc3", + "running": self.running, + "fps": self.fps, + "width": self.width, + "height": self.height, + "frame_type": self.frame_type, + "output_dtype": self.output_dtype, + "capture_mode": self.capture_mode, + "raw_policy": self.raw_policy, + "sync_tolerance_ms": self.sync_tolerance_ms, + "buffer_size": self.buffer_size, + "cameras": list(self.camera_info.values()), + } + + def get_next_frame(self, timeout=1.0): + if not self.running: + raise RuntimeError("OakFcc3Manager não está rodando. Chame start() primeiro.") + + t0 = time.time() + + while time.time() - t0 < timeout: + self._drain_queues_to_buffers() + + synced = self._try_get_synced_packet() + + if synced is not None: + frames, timestamps, sync_dt_ms, sync_ok = synced + + self.frame_id += 1 + meta = self._build_meta(frames, timestamps, sync_dt_ms, sync_ok) + + return frames, meta + + time.sleep(0.001) + + raise TimeoutError( + f"Timeout aguardando pacote sincronizado do OAK-FFC-3. " + f"Tolerância atual={self.sync_tolerance_ms} ms. " + f"Tente aumentar para 25 ou 35 ms para diagnóstico." + ) + + def get_next_decoded(self, timeout=1.0, update_radiometry=True): + frame, meta = self.get_next_frame(timeout=timeout) + + decoded = { + "frames": frame, + "meta": meta, + } + + return frame, meta, decoded + + def build_preview_from_raw_payload(self, frame, meta): + if not isinstance(frame, dict) or not frame: + raise RuntimeError("Payload inválido para preview.") + + rgb_cam_id = None + + for cam_id, info in self.camera_info.items(): + if info.get("role") == "rgb" and cam_id in frame: + rgb_cam_id = cam_id + break + + preview_source_id = rgb_cam_id or list(frame.keys())[0] + preview = frame[preview_source_id] + + if preview.ndim == 2: + import cv2 + preview_bgr = cv2.cvtColor(preview, cv2.COLOR_GRAY2BGR) + else: + preview_bgr = preview.copy() + + preview_float = preview_bgr.astype(np.float32) / 255.0 + + return preview_bgr, preview_float, preview_source_id + + def _drain_queues_to_buffers(self): + for cam_id, q in self.queues.items(): + while q.has(): + msg = q.get() + + try: + ts = msg.getTimestamp().total_seconds() + except Exception: + ts = time.time() + + frame = msg.getCvFrame() + + self.buffers[cam_id].append({ + "frame": frame, + "timestamp": ts, + }) + + def _try_get_synced_packet(self): + required_cam_ids = self._get_required_cam_ids() + + for cam_id in required_cam_ids: + if cam_id not in self.buffers or len(self.buffers[cam_id]) == 0: + return None + + # Usa o timestamp mais antigo da câmera com menor buffer como referência. + ref_cam_id = min(required_cam_ids, key=lambda cid: len(self.buffers[cid])) + ref_item = self.buffers[ref_cam_id][0] + ref_ts = ref_item["timestamp"] + + selected = {} + + for cam_id in required_cam_ids: + best_item = None + best_dt = None + + for item in self.buffers[cam_id]: + dt = abs(item["timestamp"] - ref_ts) + if best_dt is None or dt < best_dt: + best_dt = dt + best_item = item + + if best_item is None: + return None + + selected[cam_id] = best_item + + timestamps = { + cam_id: item["timestamp"] + for cam_id, item in selected.items() + } + + ts_values = list(timestamps.values()) + sync_dt_ms = (max(ts_values) - min(ts_values)) * 1000.0 if len(ts_values) >= 2 else 0.0 + sync_ok = sync_dt_ms <= self.sync_tolerance_ms + + if not sync_ok and self.sync_mode == "strict": + oldest_cam_id = min(timestamps, key=timestamps.get) + if len(self.buffers[oldest_cam_id]) > 0: + self.buffers[oldest_cam_id].popleft() + return None + + frames = { + cam_id: item["frame"] + for cam_id, item in selected.items() + } + + # Remove dos buffers tudo até os frames usados. + for cam_id, used_item in selected.items(): + while len(self.buffers[cam_id]) > 0: + item = self.buffers[cam_id].popleft() + if item is used_item: + break + + return frames, timestamps, sync_dt_ms, sync_ok + + def _get_required_cam_ids(self): + available = list(self.queues.keys()) + + if self.capture_mode == "SINGLE": + return available[:1] + + if self.capture_mode == "DOUBLE": + return available[:2] + + if self.capture_mode == "TRIPLE": + return available[:3] + + if self.capture_mode == "AUTO": + if self.raw_policy == "require_triple": + return available[:3] + return available + + return available + + def _build_meta(self, frames, timestamps, sync_dt_ms, sync_ok): + payload_sources = list(frames.keys()) + + shapes = { + cam_id: list(arr.shape) + for cam_id, arr in frames.items() + } + + dtypes = { + cam_id: str(arr.dtype) + for cam_id, arr in frames.items() + } + + return { + "frame_id": self.frame_id, + "backend": "oak_fcc3", + "frame_type": self.frame_type, + "capture_mode": self.capture_mode, + "output_dtype": self.output_dtype, + "dtype": self.output_dtype, + "output_layout": "dict_by_camera", + "payload_sources": payload_sources, + "camera_info": self.camera_info, + "timestamps": timestamps, + "sync_dt_ms": sync_dt_ms, + "sync_ok": sync_ok, + "sync_tolerance_ms": self.sync_tolerance_ms, + "shapes": shapes, + "dtypes": dtypes, + "codec_name": "none", + "codec_family": "none", + "dt_comp": 0.0, + "dt_send_payload_prev": 0.0, + } + + def _validate_capture_mode(self): + n = len(self.queues) + + if self.capture_mode == "TRIPLE" and n < 3: + raise RuntimeError(f"CaptureMode TRIPLE exige 3 câmeras, mas detectou {n}.") + + if self.capture_mode == "DOUBLE" and n < 2: + raise RuntimeError(f"CaptureMode DOUBLE exige 2 câmeras, mas detectou {n}.") + + if self.raw_policy == "require_triple" and n < 3: + raise RuntimeError(f"raw_policy=require_triple exige 3 câmeras, mas detectou {n}.") + + @staticmethod + def _socket_to_cam_id(socket_name): + mapping = { + "CAM_A": "cam2", # RGB + "CAM_B": "cam0", # RE + "CAM_C": "cam1", # NIR + } + return mapping.get(socket_name, socket_name.lower()) \ No newline at end of file diff --git a/Python/OAK/datasets/oak-fcc-3/oak_fcc3_service.py b/Python/OAK/datasets/oak-fcc-3/oak_fcc3_service.py new file mode 100644 index 000000000..72fc38389 --- /dev/null +++ b/Python/OAK/datasets/oak-fcc-3/oak_fcc3_service.py @@ -0,0 +1,133 @@ +import time +from oak_fcc3_manager import OakFcc3Manager + + +class OakFcc3Service: + def __init__(self, timeout=10, **kwargs): + self.timeout = timeout + self.manager = OakFcc3Manager(**kwargs) + self.connected = False + + def connect(self): + self.connected = True + return {"ok": True, "backend": "oak_fcc3", "connected": True} + + def disconnect(self): + self.stop() + self.connected = False + return {"ok": True, "connected": False} + + def ping(self): + return { + "ok": True, + "backend": "oak_fcc3", + "msg": "pong", + "ts": time.time(), + } + + def get_status(self): + status = self.manager.get_status() + active_ids = [c["id"] for c in status.get("cameras", [])] + + status.update({ + "ok": True, + "connected": self.connected, + "active_camera_ids": active_ids, + "camera_count_active": len(active_ids), + }) + + return status + + def get_config(self): + return { + "ok": True, + "fps": self.manager.fps, + "width": self.manager.width, + "height": self.manager.height, + "frame_type": self.manager.frame_type, + "output_dtype": self.manager.output_dtype, + "capture_mode": self.manager.capture_mode, + "raw_policy": self.manager.raw_policy, + "sync_mode": getattr(self.manager, "sync_mode", "best"), + "sync_tolerance_ms": self.manager.sync_tolerance_ms, + } + + def set_fps(self, fps): + self._ensure_stopped_for_config() + self.manager.fps = int(fps) + return {"ok": True, "fps": self.manager.fps} + + def set_resolution(self, width, height): + self._ensure_stopped_for_config() + self.manager.width = int(width) + self.manager.height = int(height) + self.manager.size = (self.manager.width, self.manager.height) + return { + "ok": True, + "width": self.manager.width, + "height": self.manager.height, + } + + def set_capture_mode(self, mode): + self._ensure_stopped_for_config() + mode = str(mode).upper() + if mode not in ("AUTO", "SINGLE", "DOUBLE", "TRIPLE"): + raise ValueError(f"capture_mode inválido: {mode}") + + self.manager.capture_mode = mode + return {"ok": True, "capture_mode": self.manager.capture_mode} + + def set_frame_type(self, frame_type): + self._ensure_stopped_for_config() + frame_type = str(frame_type).upper() + if frame_type not in ("RAW_BRUTO", "RGB", "MULTISPEC"): + raise ValueError(f"frame_type inválido: {frame_type}") + + self.manager.frame_type = frame_type + return {"ok": True, "frame_type": self.manager.frame_type} + + def set_output_dtype(self, dtype): + self._ensure_stopped_for_config() + dtype = str(dtype).lower() + if dtype not in ("uint8", "uint16", "float32"): + raise ValueError(f"output_dtype inválido: {dtype}") + + self.manager.output_dtype = dtype + return {"ok": True, "output_dtype": self.manager.output_dtype} + + def begin(self, frame_type=None, output_dtype=None, capture_mode=None): + if frame_type is not None: + self.set_frame_type(frame_type) + + if output_dtype is not None: + self.set_output_dtype(output_dtype) + + if capture_mode is not None: + self.set_capture_mode(capture_mode) + + self.manager.start() + + return { + "ok": True, + "started": True, + "status": self.get_status(), + } + + def capture_frame(self, timeout=None): + if timeout is None: + timeout = self.timeout + + frame, meta = self.manager.get_next_frame(timeout=timeout) + + return frame, meta + + def stop(self): + self.manager.stop() + return {"ok": True, "stopped": True} + + def _ensure_stopped_for_config(self): + if self.manager.running: + raise RuntimeError( + "Configuração estrutural só pode ser alterada com o manager parado. " + "Chame stop() antes." + ) \ No newline at end of file diff --git a/Python/OAK/datasets/oak-fcc-3/sensor_calibration_tool.py b/Python/OAK/datasets/oak-fcc-3/sensor_calibration_tool.py new file mode 100644 index 000000000..b22b57cf5 --- /dev/null +++ b/Python/OAK/datasets/oak-fcc-3/sensor_calibration_tool.py @@ -0,0 +1,1409 @@ +import os +import json +import time +import argparse +from datetime import datetime + +import cv2 +import numpy as np + +from cam_3.multispectral_client import MultiSpectralClient + +# ============================================================ +# Helpers +# ============================================================ + +def now_str() -> str: + return datetime.now().strftime("%Y-%m-%d %H:%M:%S") + + +def ensure_dir(path: str): + os.makedirs(path, exist_ok=True) + + +def overlay_hud( + img_bgr, + lines, + x=12, + y=22, + area_h=None, + max_font_scale=None, + min_font_scale=None, + max_line_step=None, + min_line_step=None, + bottom_margin=12, +): + h, w = img_bgr.shape[:2] + + if area_h is None: + area_h = h - y - bottom_margin + + scale = max(1.0, min(1.45, area_h / 480.0)) + + if max_font_scale is None: + max_font_scale = 0.62 * scale + if min_font_scale is None: + min_font_scale = 0.34 * scale + if max_line_step is None: + max_line_step = int(22 * scale) + if min_line_step is None: + min_line_step = int(13 * scale) + + available_h = max(1, area_h - bottom_margin) + n = max(1, len(lines)) + + font_scale = max_font_scale + line_step = max_line_step + + needed_h = n * line_step + if needed_h > available_h: + shrink = available_h / float(needed_h) + font_scale = max(min_font_scale, max_font_scale * shrink) + line_step = max(min_line_step, int(max_line_step * shrink)) + + yy = y + for s in lines: + if yy > y + area_h - bottom_margin: + break + + cv2.putText(img_bgr, s, (x, yy), cv2.FONT_HERSHEY_SIMPLEX, + font_scale, (0, 0, 0), 3, cv2.LINE_AA) + cv2.putText(img_bgr, s, (x, yy), cv2.FONT_HERSHEY_SIMPLEX, + font_scale, (255, 255, 255), 1, cv2.LINE_AA) + yy += line_step + + +def to_bgr_u8_from_rgb01(rgb01: np.ndarray) -> np.ndarray: + rgb_u8 = np.clip(rgb01 * 255.0, 0, 255).astype(np.uint8) + return cv2.cvtColor(rgb_u8, cv2.COLOR_RGB2BGR) + + +def gray_to_bgr_u8(gray01: np.ndarray) -> np.ndarray: + g = np.clip(gray01 * 255.0, 0, 255).astype(np.uint8) + return cv2.cvtColor(g, cv2.COLOR_GRAY2BGR) + + +def resize_if_needed(img: np.ndarray, target_hw: tuple[int, int]) -> np.ndarray: + target_h, target_w = target_hw + if img.shape[:2] == (target_h, target_w): + return img + return cv2.resize(img, (target_w, target_h), interpolation=cv2.INTER_LINEAR) + + +def validate_module_ready(status: dict, frame_type: str, raw_policy: str, capture_mode: str): + if not status.get("ok", True): + raise RuntimeError(f"Status inválido retornado pelo módulo: {status}") + + active_ids = list(status.get("active_camera_ids", [])) + active_count = int(status.get("camera_count_active", 0)) + + if frame_type == "RAW_BRUTO": + if raw_policy == "require_triple": + missing = [cid for cid in ("cam0", "cam1", "cam2") if cid not in active_ids] + if missing: + raise RuntimeError( + f"RAW_BRUTO com política require_triple exige três câmeras ativas. " + f"Faltando: {missing}. Ativas atuais: {active_ids}" + ) + else: + if active_count < 1: + raise RuntimeError("RAW_BRUTO requer ao menos uma câmera ativa, mas nenhuma foi detectada.") + return + + raise RuntimeError(f"frame_type desconhecido para validação: {frame_type}") + + +def build_empty_panel(shape_hw: tuple[int, int], title: str) -> np.ndarray: + h, w = shape_hw + img = np.zeros((h, w, 3), dtype=np.uint8) + overlay_hud(img, [title, "sem frame disponivel"]) + return img + + +def color_for_index(idx: int) -> tuple[int, int, int]: + palette = [ + (0, 255, 255), + (0, 255, 0), + (255, 255, 0), + (255, 0, 255), + (255, 128, 0), + (128, 255, 0), + (0, 128, 255), + (200, 200, 255), + ] + return palette[idx % len(palette)] + + +def compute_stats_from_roi(img01: np.ndarray, rect: tuple[int, int, int, int]) -> dict: + x0, y0, x1, y1 = rect + x0, x1 = sorted((int(x0), int(x1))) + y0, y1 = sorted((int(y0), int(y1))) + + roi = img01[y0:y1, x0:x1] + if roi.size == 0: + return { + "valid": False, + "mean": 0.0, + "std": 0.0, + "min": 0.0, + "max": 0.0, + "p05": 0.0, + "p95": 0.0, + "pct_saturated": 0.0, + "pct_dark": 0.0, + "pixels": 0, + } + + arr = roi.astype(np.float32).reshape(-1) + return { + "valid": True, + "mean": float(arr.mean()), + "std": float(arr.std()), + "min": float(arr.min()), + "max": float(arr.max()), + "p05": float(np.percentile(arr, 5)), + "p95": float(np.percentile(arr, 95)), + "pct_saturated": float((arr >= 0.98).mean() * 100.0), + "pct_dark": float((arr <= 0.02).mean() * 100.0), + "pixels": int(arr.size), + } + + +def compute_scene_health(img01: np.ndarray) -> dict: + arr = img01.astype(np.float32).reshape(-1) + mean = float(arr.mean()) + std = float(arr.std()) + pct_sat = float((arr >= 0.98).mean() * 100.0) + pct_dark = float((arr <= 0.02).mean() * 100.0) + p05 = float(np.percentile(arr, 5)) + p95 = float(np.percentile(arr, 95)) + + comments = [] + if pct_sat > 5.0: + comments.append("saturando") + if pct_dark > 40.0: + comments.append("muito escuro") + if std < 0.05: + comments.append("baixo contraste") + if not comments: + comments.append("ok") + + return { + "mean": mean, + "std": std, + "p05": p05, + "p95": p95, + "pct_saturated": pct_sat, + "pct_dark": pct_dark, + "comment": ", ".join(comments), + } + + +def draw_rois(panel_bgr: np.ndarray, rois: list[dict]): + for idx, roi in enumerate(rois): + color = roi.get("color", color_for_index(idx)) + label = roi.get("name", f"roi_{idx+1}") + + if roi.get("type") == "polygon": + pts = np.array(roi.get("points", []), dtype=np.int32) + if len(pts) >= 2: + cv2.polylines(panel_bgr, [pts], isClosed=True, color=color, thickness=2) + if len(pts) >= 1: + x, y = pts[0] + cv2.putText( + panel_bgr, + label, + (int(x) + 4, max(18, int(y) - 6)), + cv2.FONT_HERSHEY_SIMPLEX, + 0.5, + color, + 2, + cv2.LINE_AA, + ) + continue + + rect = roi.get("rect") + if rect is None: + continue + + x0, y0, x1, y1 = rect + cv2.rectangle(panel_bgr, (x0, y0), (x1, y1), color, 2) + cv2.putText( + panel_bgr, + label, + (x0 + 4, max(18, y0 - 6)), + cv2.FONT_HERSHEY_SIMPLEX, + 0.5, + color, + 2, + cv2.LINE_AA, + ) + + +def compute_stats_from_polygon_roi(img01: np.ndarray, points: list) -> dict: + h, w = img01.shape[:2] + + if len(points) < 3: + return compute_stats_from_roi(img01, (0, 0, 0, 0)) + + pts = np.array(points, dtype=np.int32) + mask = np.zeros((h, w), dtype=np.uint8) + cv2.fillPoly(mask, [pts], 255) + + arr = img01[mask > 0].astype(np.float32).reshape(-1) + + if arr.size == 0: + return { + "valid": False, "mean": 0.0, "std": 0.0, + "min": 0.0, "max": 0.0, + "p05": 0.0, "p95": 0.0, + "pct_saturated": 0.0, + "pct_dark": 0.0, + "pixels": 0, + } + + return { + "valid": True, + "mean": float(arr.mean()), + "std": float(arr.std()), + "min": float(arr.min()), + "max": float(arr.max()), + "p05": float(np.percentile(arr, 5)), + "p95": float(np.percentile(arr, 95)), + "pct_saturated": float((arr >= 0.98).mean() * 100.0), + "pct_dark": float((arr <= 0.02).mean() * 100.0), + "pixels": int(arr.size), + } + + +def compute_stats_for_roi(img01: np.ndarray, roi: dict) -> dict: + if roi.get("type") == "polygon": + return compute_stats_from_polygon_roi(img01, roi.get("points", [])) + + return compute_stats_from_roi(img01, roi.get("rect", (0, 0, 0, 0))) + + +def draw_current_polygon(panel_bgr: np.ndarray, points: list): + if not points: + return + + pts = np.array(points, dtype=np.int32) + + for p in pts: + cv2.circle(panel_bgr, tuple(p), 4, (0, 255, 255), -1) + + if len(pts) >= 2: + cv2.polylines(panel_bgr, [pts], isClosed=False, color=(0, 255, 255), thickness=1) + + +# ============================================================ +# MOCK +# ============================================================ + +def load_mock_image_rgb(path: str, fallback_shape=(480, 640)): + if not path: + h, w = fallback_shape + img = np.zeros((h, w, 3), dtype=np.float32) + return img + + bgr = cv2.imread(path, cv2.IMREAD_COLOR) + if bgr is None: + raise RuntimeError(f"Falha ao carregar mock RGB: {path}") + + rgb = bgr[:, :, ::-1].astype(np.float32) / 255.0 + return rgb + + +def load_mock_image_gray(path: str, fallback_shape=(480, 640)): + if not path: + h, w = fallback_shape + return np.zeros((h, w), dtype=np.float32) + + gray = cv2.imread(path, cv2.IMREAD_GRAYSCALE) + if gray is None: + raise RuntimeError(f"Falha ao carregar mock mono: {path}") + + return gray.astype(np.float32) / 255.0 + + +def build_mock_decoded(args): + shape = (args.height, args.width) + + cam2 = load_mock_image_rgb(args.mock_cam2, fallback_shape=shape) + cam0 = load_mock_image_gray(args.mock_cam0, fallback_shape=shape) + cam1 = load_mock_image_gray(args.mock_cam1, fallback_shape=shape) + + return { + "cam2": {"name": "RGB", "image": cam2, "meta": {"mock": True}}, + "cam0": {"name": "RE", "image": cam0, "meta": {"mock": True}}, + "cam1": {"name": "NIR", "image": cam1, "meta": {"mock": True}}, + } + + +# ============================================================ +# Análise de dados offline +# ============================================================ + +def ts_name() -> str: + return datetime.now().strftime("%Y%m%d_%H%M%S_%f")[:-3] + + +def save_offline_sample( + base_dir: str, + preview_bgr: np.ndarray, + meta: dict, + packed_raw_by_camera: dict, +): + os.makedirs(base_dir, exist_ok=True) + name = ts_name() + + png_path = os.path.join(base_dir, f"{name}.png") + json_path = os.path.join(base_dir, f"{name}.json") + + payload_files = {} + payload_shapes = {} + payload_dtypes = {} + + for cam_id, arr in packed_raw_by_camera.items(): + path = os.path.join(base_dir, f"{name}_{cam_id}.bin") + arr.tofile(path) + + payload_files[cam_id] = os.path.basename(path) + payload_shapes[cam_id] = list(arr.shape) + payload_dtypes[cam_id] = str(arr.dtype) + + meta_save = dict(meta) + meta_save["saved_payload_type"] = "raw_native_multi" + meta_save["saved_payload_paths"] = payload_files + meta_save["saved_payload_shapes"] = payload_shapes + meta_save["saved_payload_dtypes"] = payload_dtypes + meta_save["saved_preview_path"] = os.path.basename(png_path) + + cv2.imwrite(png_path, preview_bgr) + + with open(json_path, "w", encoding="utf-8") as f: + json.dump(meta_save, f, ensure_ascii=False, indent=2) + + return png_path, json_path + + +def load_offline_sample_decoded(json_path: str, cam: MultiSpectralClient): + if not os.path.isfile(json_path): + raise FileNotFoundError(f"Sample offline não encontrado: {json_path}") + + with open(json_path, "r", encoding="utf-8") as f: + meta = json.load(f) + + base_dir = os.path.dirname(json_path) + payload_paths = meta.get("saved_payload_paths") or {} + payload_shapes = meta.get("saved_payload_shapes") or {} + payload_dtypes = meta.get("saved_payload_dtypes") or {} + + if not payload_paths: + raise RuntimeError("Sample offline inválido: saved_payload_paths ausente") + + frame = {} + + for cam_id, rel_path in payload_paths.items(): + bin_path = os.path.join(base_dir, rel_path) + if not os.path.isfile(bin_path): + raise FileNotFoundError(f"Payload não encontrado para {cam_id}: {bin_path}") + + dtype_str = payload_dtypes.get(cam_id, "uint8") + shape = payload_shapes.get(cam_id) + + if shape is None: + raise RuntimeError(f"Shape ausente para {cam_id}") + + arr = np.fromfile(bin_path, dtype=np.dtype(dtype_str)).reshape(tuple(shape)) + frame[cam_id] = arr + + stream_meta = meta.get("stream_meta") or meta + + # Garante campos mínimos usados pelo decoder. + stream_meta.setdefault("camera_frames", meta.get("camera_frames", {})) + stream_meta.setdefault("frame_type", "RAW_BRUTO") + + decoded = cam.core.decode_stream_cameras(frame, stream_meta) + + preview_path = meta.get("saved_preview_path") + preview_bgr = None + if preview_path: + preview_full = os.path.join(base_dir, preview_path) + if os.path.isfile(preview_full): + preview_bgr = cv2.imread(preview_full, cv2.IMREAD_COLOR) + + return decoded, stream_meta, frame, preview_bgr + + +# ============================================================ +# Persistência dos parâmetros/snapshots +# ============================================================ + +def default_payload(args, effective_capture_mode: str): + return { + "schema": "manual_sensor_calibration_v1", + "saved_at": now_str(), + "pi_host": args.pi_host, + "pc_host": args.pc_host, + "stream_port": args.stream_port, + "frame_type": "RAW_BRUTO", + "capture_mode_requested": args.capture_mode, + "capture_mode_effective": effective_capture_mode, + "raw_policy": args.raw_policy, + "sensor_width": args.width, + "sensor_height": args.height, + "bayer_pattern": args.bayer, + "notes": args.notes or "", + "camera_settings": { + "cam0": {}, + "cam1": {}, + "cam2": {}, + }, + "snapshots": [], + "calibration_guidance_log": [], + } + + +def load_payload(path: str, args, effective_capture_mode: str): + if not path or not os.path.isfile(path): + return default_payload(args, effective_capture_mode) + + with open(path, "r", encoding="utf-8") as f: + data = json.load(f) + + data.setdefault("schema", "manual_sensor_calibration_v1") + data.setdefault("camera_settings", {"cam0": {}, "cam1": {}, "cam2": {}}) + data.setdefault("snapshots", []) + data.setdefault("calibration_guidance_log", []) + return data + + +def save_payload(path: str, data: dict): + ensure_dir(os.path.dirname(path) or ".") + data = dict(data) + data["saved_at"] = now_str() + with open(path, "w", encoding="utf-8") as f: + json.dump(data, f, ensure_ascii=False, indent=2) + + +def build_camera_params_payload(args, effective_capture_mode, camera_controls, rois=None, snapshots=None, guidance_log=None): + return { + "schema": "multispec_camera_params_v1", + "saved_at": now_str(), + "pi_host": args.pi_host, + "pc_host": args.pc_host, + "stream_port": args.stream_port, + "frame_type": "RAW_BRUTO", + "capture_mode_requested": args.capture_mode, + "capture_mode_effective": effective_capture_mode, + "raw_policy": args.raw_policy, + "sensor_width": args.width, + "sensor_height": args.height, + "bayer_pattern": args.bayer, + "camera_settings": json.loads(json.dumps(camera_controls)), + "rois": rois or {}, + "snapshots": snapshots or [], + "notes": args.notes or "", + "calibration_guidance_log": guidance_log or [], + } + + +# ============================================================ +# Guia automática de ajustes dos controles +# ============================================================ + +def normalize_class_name(name: str) -> str: + s = (name or "").strip().lower() + if s.startswith("cana"): + return "cana" + if s.startswith("erva"): + return "erva" + if s.startswith("solo") or s.startswith("chao") or s.startswith("chão"): + return "solo" + if s.startswith("palha"): + return "palha" + return s + + +def collect_roi_metrics_by_class(img01: np.ndarray, rois_for_cam: list[dict]) -> dict: + grouped = {} + + for roi in rois_for_cam: + cls = normalize_class_name(roi.get("name", "")) + if not cls: + continue + + stats = compute_stats_for_roi(img01, roi) + if not stats.get("valid"): + continue + + grouped.setdefault(cls, []).append(stats) + + summary = {} + for cls, items in grouped.items(): + summary[cls] = { + "count": len(items), + "mean": float(np.mean([x["mean"] for x in items])), + "std": float(np.mean([x["std"] for x in items])), + "p05": float(np.mean([x["p05"] for x in items])), + "p95": float(np.mean([x["p95"] for x in items])), + "pct_saturated": float(np.mean([x["pct_saturated"] for x in items])), + "pct_dark": float(np.mean([x["pct_dark"] for x in items])), + "pixels": int(sum(x["pixels"] for x in items)), + } + + return summary + + +def mean_of_classes(summary: dict, classes: list[str], key: str = "mean"): + vals = [summary[c][key] for c in classes if c in summary] + if not vals: + return None + return float(np.mean(vals)) + + +def analyze_spectral_guidance(selected_cam: str, img01: np.ndarray, rois_for_cam: list[dict], ctrl: dict, exp_step: int, gain_step: float) -> dict: + summary = collect_roi_metrics_by_class(img01, rois_for_cam) + + veg_mean = mean_of_classes(summary, ["cana", "erva"], "mean") + veg_p95 = mean_of_classes(summary, ["cana", "erva"], "p95") + veg_sat = mean_of_classes(summary, ["cana", "erva"], "pct_saturated") + solo_mean = mean_of_classes(summary, ["solo", "palha"], "mean") + + before = json.loads(json.dumps(ctrl)) + new_ctrl = json.loads(json.dumps(ctrl)) + action = "keep" + status = "ok" + reason = "Parametros parecem aceitaveis." + + if veg_mean is None: + return { + "status": "need_rois", + "action": "none", + "reason": "Crie pelo menos uma ROI de cana ou erva para analisar canal espectral.", + "class_metrics": summary, + "before_settings": before, + "after_settings": new_ctrl, + } + + separation = None + if solo_mean is not None: + separation = float(veg_mean - solo_mean) + + exp = new_ctrl.get("exposure_time_us") + gain = new_ctrl.get("analogue_gain") + + if exp is None: + exp = 15000 + if gain is None: + gain = 1.0 + + new_ctrl["ae_enable"] = False + new_ctrl["awb_enable"] = False + + # 1) Proteção contra estouro + MIN_EXP_US = 100 + MIN_GAIN = 1.0 + + if veg_sat is not None and veg_sat > 1.0: + if exp > MIN_EXP_US: + new_ctrl["exposure_time_us"] = int(max(exp - exp_step, MIN_EXP_US)) + action = "decrease_exposure" + status = "adjust" + reason = f"Vegetacao saturando ({veg_sat:.2f}%). Reduzir exposicao." + + elif gain > MIN_GAIN: + new_ctrl["analogue_gain"] = float(max(gain / (1.0 + gain_step), MIN_GAIN)) + action = "decrease_gain" + status = "adjust" + reason = ( + f"Vegetacao saturando ({veg_sat:.2f}%), mas exposicao ja esta no minimo. " + "Reduzir ganho." + ) + + else: + action = "keep" + status = "limit" + reason = ( + f"Vegetacao saturando ({veg_sat:.2f}%), mas exposicao e ganho ja estao no minimo. " + "Nao ha ajuste possivel por software." + ) + + # 2) Vegetação pouco iluminada + elif veg_p95 is not None and veg_p95 < 0.75: + new_ctrl["exposure_time_us"] = int(min(exp + exp_step, 200000)) + action = "increase_exposure" + status = "adjust" + reason = f"p95 da vegetacao baixo ({veg_p95:.3f}). Aumentar exposicao." + + # 3) Vegetação muito perto do teto + elif veg_p95 is not None and veg_p95 > 0.96: + new_ctrl["exposure_time_us"] = int(max(exp - exp_step, 100)) + action = "decrease_exposure" + status = "adjust" + reason = f"p95 da vegetacao alto ({veg_p95:.3f}). Reduzir exposicao." + + # 4) Separação ruim + elif separation is not None and separation < 0.25: + if veg_p95 is not None and veg_p95 < 0.90: + new_ctrl["exposure_time_us"] = int(min(exp + exp_step, 200000)) + action = "increase_exposure" + status = "adjust" + reason = f"Separacao baixa ({separation:.3f}) e ha margem no p95. Aumentar exposicao." + else: + new_ctrl["analogue_gain"] = float(min(gain * (1.0 + gain_step), 32.0)) + action = "increase_gain" + status = "adjust" + reason = f"Separacao baixa ({separation:.3f}) sem muita margem de exposicao. Aumentar ganho levemente." + + return { + "status": status, + "action": action, + "reason": reason, + "channel": selected_cam, + "class_metrics": summary, + "veg_mean": veg_mean, + "solo_mean": solo_mean, + "separation": separation, + "veg_p95": veg_p95, + "veg_sat": veg_sat, + "before_settings": before, + "after_settings": new_ctrl, + } + + +def analyze_rgb_guidance(selected_cam: str, img01: np.ndarray, rois_for_cam: list[dict], ctrl: dict, exp_step: int, gain_step: float) -> dict: + scene = compute_scene_health(img01) + summary = collect_roi_metrics_by_class(img01, rois_for_cam) + + before = json.loads(json.dumps(ctrl)) + new_ctrl = json.loads(json.dumps(ctrl)) + action = "keep" + status = "ok" + reason = "RGB parece aceitável." + + exp = new_ctrl.get("exposure_time_us") + gain = new_ctrl.get("analogue_gain") + + if exp is None: + exp = 15000 + if gain is None: + gain = 1.0 + + # Para RGB calibrado fixo: desligar AE/AWB quando for aplicar preset final. + new_ctrl["ae_enable"] = False + new_ctrl["awb_enable"] = False + + if scene["pct_saturated"] > 2.0 or scene["p95"] > 0.97: + new_ctrl["exposure_time_us"] = int(max(exp - exp_step, 100)) + action = "decrease_exposure" + status = "adjust" + reason = f"RGB muito próximo de saturar. sat={scene['pct_saturated']:.2f}%, p95={scene['p95']:.3f}." + + elif scene["pct_dark"] > 20.0 and scene["p95"] < 0.85: + new_ctrl["exposure_time_us"] = int(min(exp + exp_step, 200000)) + action = "increase_exposure" + status = "adjust" + reason = f"RGB escuro. dark={scene['pct_dark']:.2f}%, p95={scene['p95']:.3f}." + + elif scene["std"] < 0.08: + new_ctrl["analogue_gain"] = float(min(gain * (1.0 + gain_step), 32.0)) + action = "increase_gain" + status = "adjust" + reason = f"RGB com baixo contraste global. std={scene['std']:.3f}." + + return { + "status": status, + "action": action, + "reason": reason, + "channel": selected_cam, + "scene_health": scene, + "class_metrics": summary, + "before_settings": before, + "after_settings": new_ctrl, + } + + +def run_guidance_analysis(selected_cam: str, img01: np.ndarray, rois_for_cam: list[dict], ctrl: dict, exp_step: int, gain_step: float) -> dict: + if img01 is None: + return { + "status": "error", + "action": "none", + "reason": "Sem imagem ativa para análise.", + "before_settings": json.loads(json.dumps(ctrl)), + "after_settings": json.loads(json.dumps(ctrl)), + } + + if selected_cam == "cam2": + return analyze_rgb_guidance(selected_cam, img01, rois_for_cam, ctrl, exp_step, gain_step) + + return analyze_spectral_guidance(selected_cam, img01, rois_for_cam, ctrl, exp_step, gain_step) + + +# ============================================================ +# Main +# ============================================================ + +def main(): + parser = argparse.ArgumentParser( + description="Ferramenta de calibração dos sensores RGB/RE/NIR com controle manual e ROIs em tempo real.", + formatter_class=argparse.ArgumentDefaultsHelpFormatter, + ) + parser.add_argument("--pi_host", default="192.168.105.6") + parser.add_argument("--pc_host", default="192.168.105.5") + parser.add_argument("--stream_port", type=int, default=6001) + parser.add_argument("--server_port", type=int, default=5000) + parser.add_argument("--fps", type=int, default=20) + parser.add_argument("--width", type=int, default=640) + parser.add_argument("--height", type=int, default=480) + parser.add_argument("--bayer", default="GBRG", choices=["GBRG", "GRBG", "RGGB", "BGGR"]) + parser.add_argument("--capture_mode", default="AUTO", choices=["AUTO", "SINGLE", "DOUBLE", "TRIPLE"]) + parser.add_argument("--raw_policy", default="allow_single", choices=["allow_single", "require_triple"]) + parser.add_argument("--preview_scale", type=float, default=1.0) + parser.add_argument("--exp_step", type=int, default=1000, help="Passo de exposição em us") + parser.add_argument("--gain_step", type=float, default=0.10, help="Passo multiplicativo do ganho") + parser.add_argument("--out_json", default="calibration/sensor_calibration.json") + parser.add_argument("--load_json", default="") + parser.add_argument("--notes", default="") + parser.add_argument("--mock", action="store_true") + parser.add_argument("--mock_cam0", default="", help="Imagem mock para cam0 / RE") + parser.add_argument("--mock_cam1", default="", help="Imagem mock para cam1 / NIR") + parser.add_argument("--mock_cam2", default="", help="Imagem mock para cam2 / RGB") + parser.add_argument("--offline_sample_json", default="", help="JSON de sample salvo para análise offline") + parser.add_argument("--offline_save_dir", default="calibration/offline_samples", help="Pasta para salvar frames brutos offline") + args = parser.parse_args() + + cam = MultiSpectralClient( + pi_host=args.pi_host, + pc_host=args.pc_host, + server_port=args.server_port, + stream_port=args.stream_port, + width=args.width, + height=args.height, + bayer=args.bayer, + fps=args.fps, + frame_type="RAW_BRUTO", + output_dtype="uint8", + capture_mode=args.capture_mode, + raw_policy=args.raw_policy, + module_calibration_json=None, + ) + + offline_mode = bool(args.offline_sample_json) + live_mode = not args.mock and not offline_mode + + effective_capture_mode = args.capture_mode + + data_payload = load_payload(args.load_json, args, effective_capture_mode) + + selected_cam = "cam2" + last_msg = "" + last_msg_t = 0.0 + last_frame_id = -1 + fps_view = 0.0 + fps_stream = 0.0 + t_view_fps = time.time() + t_stream_fps = time.time() + view_frames = 0 + stream_frames_accum = 0 + last_stream_frame_id = None + + decoded_last = {} + last_meta_stream = None + last_raw_frame = None + last_preview_bgr = None + + roi_name_input_active = False + roi_name_buffer = "" + roi_name_points_pending = [] + + guidance_log = data_payload.get("calibration_guidance_log", []) + last_guidance = guidance_log[-1]["result"] if guidance_log else None + + window_name = "Sensor Calibration Tool" + cv2.namedWindow(window_name, cv2.WINDOW_NORMAL) + + panel_rects = { + "cam2": None, + "cam0": None, + "cam1": None, + "data": None, + } + + # Controle de câmera + camera_controls = { + "cam0": { + "ae_enable": False, + "awb_enable": False, + "exposure_time_us": 15000, + "analogue_gain": 1.0, + "colour_gains": None, + }, + "cam1": { + "ae_enable": False, + "awb_enable": False, + "exposure_time_us": 15000, + "analogue_gain": 1.0, + "colour_gains": None, + }, + "cam2": { + "ae_enable": True, + "awb_enable": True, + "exposure_time_us": 15000, + "analogue_gain": 1.0, + "colour_gains": [1.0, 1.0], + }, + } + + rois = { + "cam2": [], + "cam0": [], + "cam1": [], + } + + current_polygon_points = [] + + def get_active_rect_for_mouse(): + return panel_rects.get(selected_cam) + + def on_mouse(event, x, y, flags, param): + nonlocal current_polygon_points, last_msg, last_msg_t + + rect = get_active_rect_for_mouse() + if rect is None: + return + + x0, y0, x1, y1 = rect + inside = (x0 <= x < x1 and y0 <= y < y1) + if not inside: + return + + lx = int(x - x0) + ly = int(y - y0) + + if event == cv2.EVENT_LBUTTONDOWN: + current_polygon_points.append((lx, ly)) + last_msg = f"{selected_cam}: ponto #{len(current_polygon_points)} adicionado" + last_msg_t = time.time() + + cv2.setMouseCallback(window_name, on_mouse) + + if args.mock: + decoded_last = build_mock_decoded(args) + + if offline_mode: + decoded_last, last_meta_stream, last_raw_frame, last_preview_bgr = load_offline_sample_decoded(args.offline_sample_json, cam) + + def apply_controls_to_selected_cam(): + nonlocal cam, last_msg, last_msg_t + ctrl = camera_controls[selected_cam] + + try: + resp = cam.svc.set_ae_enable(selected_cam, bool(ctrl["ae_enable"])) + ctrl["ae_enable"] = bool(resp.get("ae_enable", ctrl["ae_enable"])) + + if selected_cam == "cam2": + resp = cam.svc.set_awb_enable(selected_cam, bool(ctrl["awb_enable"])) + ctrl["awb_enable"] = bool(resp.get("awb_enable", ctrl["awb_enable"])) + + if not ctrl["ae_enable"]: + if ctrl["exposure_time_us"] is not None: + resp = cam.svc.set_exposure_time(selected_cam, int(ctrl["exposure_time_us"])) + exp_val = resp.get("exposure_time_us", ctrl["exposure_time_us"]) + ctrl["exposure_time_us"] = int(exp_val) if exp_val is not None else None + + if ctrl["analogue_gain"] is not None: + resp = cam.svc.set_analogue_gain(selected_cam, float(ctrl["analogue_gain"])) + gain_val = resp.get("analogue_gain", ctrl["analogue_gain"]) + ctrl["analogue_gain"] = float(gain_val) if gain_val is not None else None + + last_msg = f"Controles aplicados em {selected_cam}" + last_msg_t = time.time() + + except Exception as e: + last_msg = f"Falha ao aplicar controles: {e}" + last_msg_t = time.time() + + def snapshot_current_state(): + active_img = None + if selected_cam in decoded_last: + active_img = decoded_last[selected_cam]["image"] + + if active_img is None: + return None + + roi_entries = [] + for roi in rois[selected_cam]: + stats = compute_stats_for_roi(active_img, roi) + + entry = { + "name": roi["name"], + "type": roi.get("type", "rect"), + "metrics": stats, + } + + if roi.get("type") == "polygon": + entry["points"] = [[int(x), int(y)] for x, y in roi.get("points", [])] + else: + entry["rect"] = list(map(int, roi["rect"])) + + roi_entries.append(entry) + + snap = { + "timestamp": now_str(), + "camera": selected_cam, + "camera_settings": json.loads(json.dumps(camera_controls[selected_cam])), + "scene_health": compute_scene_health(active_img), + "rois": roi_entries, + } + return snap + + def sync_camera_controls_from_pi(): + nonlocal cam, camera_controls + + if not live_mode: + return + + for cam_id in camera_controls.keys(): + try: + initial_ctrl = cam.svc.get_camera_controls(cam_id) + + camera_controls[cam_id]["ae_enable"] = bool( + initial_ctrl.get("ae_enable", camera_controls[cam_id]["ae_enable"]) + ) + + camera_controls[cam_id]["awb_enable"] = bool( + initial_ctrl.get("awb_enable", camera_controls[cam_id]["awb_enable"]) + ) + + exp_val = initial_ctrl.get("exposure_time_us", camera_controls[cam_id]["exposure_time_us"]) + camera_controls[cam_id]["exposure_time_us"] = int(exp_val) if exp_val is not None else None + + gain_val = initial_ctrl.get("analogue_gain", camera_controls[cam_id]["analogue_gain"]) + camera_controls[cam_id]["analogue_gain"] = float(gain_val) if gain_val is not None else None + + camera_controls[cam_id]["colour_gains"] = initial_ctrl.get( + "colour_gains", + camera_controls[cam_id]["colour_gains"] + ) + + except Exception as e: + print(f"[WARN] Falha ao ler controles iniciais de {cam_id}: {e}") + + try: + if live_mode: + cam.start(print_debug=True) + sync_camera_controls_from_pi() + else: + last_msg = "MODO OFFLINE ativo" if offline_mode else "MODO MOCK ativo" + last_msg_t = time.time() + + while True: + t0 = time.time() + + if live_mode: + frame, meta, decoded = cam.get_next_decoded(timeout=2.0) + + if meta is not None and frame is not None and meta.get("frame_id") != last_frame_id: + last_frame_id = meta["frame_id"] + + if not isinstance(frame, dict): + raise RuntimeError("Este calibrador espera RAW_BRUTO multi-payload como dict de câmeras.") + + decoded_last = decoded + last_meta_stream = dict(meta) + last_raw_frame = {cam_id: arr.copy() for cam_id, arr in frame.items()} + + curr_frame_id = meta.get("frame_id") + if curr_frame_id is not None and last_stream_frame_id != curr_frame_id: + stream_frames_accum += 1 + last_stream_frame_id = curr_frame_id + + dt_stream = time.time() - t_stream_fps + if dt_stream >= 1.0: + fps_stream = stream_frames_accum / dt_stream + stream_frames_accum = 0 + t_stream_fps = time.time() + + view_frames += 1 + dt_view = time.time() - t_view_fps + if dt_view >= 1.0: + fps_view = view_frames / dt_view + view_frames = 0 + t_view_fps = time.time() + else: + fps_stream = 0.0 + fps_view = 0.0 + + if decoded_last: + rgb01 = decoded_last.get("cam2", {}).get("image") + re01 = decoded_last.get("cam0", {}).get("image") + nir01 = decoded_last.get("cam1", {}).get("image") + + if rgb01 is None: + rgb_panel = build_empty_panel((args.height, args.width), "RGB") + base_h, base_w = args.height, args.width + else: + rgb_panel = to_bgr_u8_from_rgb01(rgb01) + base_h, base_w = rgb01.shape[:2] + + re_panel = gray_to_bgr_u8(resize_if_needed(re01, (base_h, base_w))) if re01 is not None else build_empty_panel((base_h, base_w), "RE") + nir_panel = gray_to_bgr_u8(resize_if_needed(nir01, (base_h, base_w))) if nir01 is not None else build_empty_panel((base_h, base_w), "NIR") + + draw_rois(rgb_panel, rois["cam2"]) + draw_rois(re_panel, rois["cam0"]) + draw_rois(nir_panel, rois["cam1"]) + + active_panel = {"cam2": rgb_panel, "cam0": re_panel, "cam1": nir_panel}.get(selected_cam) + if active_panel is not None: + draw_current_polygon(active_panel, current_polygon_points) + + overlay_hud(rgb_panel, ["RGB (cam2)", f"ativo={selected_cam == 'cam2'}"]) + overlay_hud(re_panel, ["RE (cam0)", f"ativo={selected_cam == 'cam0'}"]) + overlay_hud(nir_panel, ["NIR (cam1)", f"ativo={selected_cam == 'cam1'}"]) + + ph = max(rgb_panel.shape[0], re_panel.shape[0], nir_panel.shape[0], base_h) + pw = max(rgb_panel.shape[1], re_panel.shape[1], nir_panel.shape[1], base_w) + + def fit_panel(img): + if img.shape[:2] != (ph, pw): + return cv2.resize(img, (pw, ph), interpolation=cv2.INTER_NEAREST) + return img + + rgb_panel = fit_panel(rgb_panel) + re_panel = fit_panel(re_panel) + nir_panel = fit_panel(nir_panel) + if rgb01 is not None: + last_preview_bgr = to_bgr_u8_from_rgb01(rgb01) + else: + last_preview_bgr = rgb_panel.copy() + + data_panel = np.zeros((ph, pw, 3), dtype=np.uint8) + panel_rects["cam2"] = (0, 0, pw, ph) + panel_rects["cam0"] = (pw, 0, pw * 2, ph) + panel_rects["cam1"] = (0, ph, pw, ph * 2) + panel_rects["data"] = (pw, ph, pw * 2, ph * 2) + + top = np.hstack([rgb_panel, re_panel]) + bottom = np.hstack([nir_panel, data_panel]) + board = np.vstack([top, bottom]) + + active_img = decoded_last.get(selected_cam, {}).get("image") + global_stats = compute_scene_health(active_img) if active_img is not None else None + ctrl = camera_controls[selected_cam] + lines = [ + f"CAM ATIVA: {selected_cam}", + f"AE={'ON' if ctrl['ae_enable'] else 'OFF'} | AWB={'ON' if ctrl['awb_enable'] else 'OFF'}", + f"EXP={ctrl['exposure_time_us']} us", + f"GAIN={ctrl['analogue_gain']:.2f}", + f"fps_stream={fps_stream:.1f} | fps_view={fps_view:.1f}", + ] + + if global_stats is not None: + lines.extend([ + f"mean={global_stats['mean']:.3f} | std={global_stats['std']:.3f}", + f"p05={global_stats['p05']:.3f} | p95={global_stats['p95']:.3f}", + f"sat={global_stats['pct_saturated']:.2f}% | dark={global_stats['pct_dark']:.2f}%", + f"scene={global_stats['comment']}", + ]) + else: + lines.append("sem stats da cena") + + if last_guidance is not None: + lines.extend([ + "-", + f"GUIDE: {last_guidance.get('status')} | {last_guidance.get('action')}", + f"{last_guidance.get('reason', '')[:46]}", + ]) + + sep = last_guidance.get("separation") + if sep is not None: + lines.append(f"sep_veg_solo={sep:.3f}") + + if roi_name_input_active: + lines.extend([ + "-", + "NOME DA ROI:", + f"> {roi_name_buffer}_", + "ENTER confirma | ESC cancela | BACKSPACE apaga", + ]) + + lines.append("-") + lines.append(f"ROIs: {len(rois[selected_cam])}") + for idx, roi in enumerate(rois[selected_cam][:6]): + if active_img is None: + break + stats = compute_stats_for_roi(active_img, roi) + lines.append(f"{roi['name']}: mean={stats['mean']:.3f} std={stats['std']:.3f}") + lines.append(f" p95={stats['p95']:.3f} sat={stats['pct_saturated']:.1f}% dark={stats['pct_dark']:.1f}%") + + lines.extend([ + "-", + "1=RGB | 2=RE | 3=NIR | E=AE | B=AWB", + "I/K exp +/- | O/L gain +/- | G guia | A aplica", + "mouse: clique pontos | ENTER fecha ROI | U desfaz ponto/ROI | X limpa poligono", + "F salva frame bruto | SPACE salva PARAMS | S snapshot | Q sai", + ]) + + x0, y0, x1, y1 = panel_rects["data"] + overlay_hud( + board, + lines, + x=x0 + 12, + y=y0 + 22, + area_h=(y1 - y0) - 22, + ) + + if last_msg and (time.time() - last_msg_t) < 2.5: + cv2.putText(board, last_msg, (12, board.shape[0] - 16), cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 255, 0), 2, cv2.LINE_AA) + + if args.preview_scale != 1.0: + board = cv2.resize( + board, + (int(board.shape[1] * args.preview_scale), int(board.shape[0] * args.preview_scale)), + interpolation=cv2.INTER_NEAREST, + ) + + cv2.imshow(window_name, board) + else: + blank = np.zeros((720, 1280, 3), dtype=np.uint8) + overlay_hud(blank, ["Aguardando frames do módulo..."], x=40, y=80) + cv2.imshow(window_name, blank) + + k = cv2.waitKey(1) & 0xFF + + if roi_name_input_active: + if k in (13, 10): # ENTER + name = roi_name_buffer.strip() + if not name: + name = f"roi_{len(rois[selected_cam]) + 1}" + + roi = { + "name": name, + "type": "polygon", + "points": list(roi_name_points_pending), + "color": color_for_index(len(rois[selected_cam])), + } + + rois[selected_cam].append(roi) + + current_polygon_points = [] + roi_name_points_pending = [] + roi_name_buffer = "" + roi_name_input_active = False + + last_msg = f"ROI criada em {selected_cam}: {name}" + last_msg_t = time.time() + + elif k in (27,): # ESC + roi_name_input_active = False + roi_name_buffer = "" + roi_name_points_pending = [] + last_msg = "Criacao de ROI cancelada" + last_msg_t = time.time() + + elif k in (8, 127): # BACKSPACE + roi_name_buffer = roi_name_buffer[:-1] + + elif 32 <= k <= 126: + roi_name_buffer += chr(k) + + continue + + if k in (ord("q"), ord("Q"), 27): + break + elif k == ord("1"): + selected_cam = "cam2" + last_msg = "Selecionada: cam2 / RGB" + last_msg_t = time.time() + elif k == ord("2"): + selected_cam = "cam0" + last_msg = "Selecionada: cam0 / RE" + last_msg_t = time.time() + elif k == ord("3"): + selected_cam = "cam1" + last_msg = "Selecionada: cam1 / NIR" + last_msg_t = time.time() + elif k in (ord("e"), ord("E")): + camera_controls[selected_cam]["ae_enable"] = not camera_controls[selected_cam]["ae_enable"] + last_msg = f"AE {selected_cam} -> {'ON' if camera_controls[selected_cam]['ae_enable'] else 'OFF'}" + last_msg_t = time.time() + elif k in (ord("b"), ord("B")): + if selected_cam == "cam2": + camera_controls[selected_cam]["awb_enable"] = not camera_controls[selected_cam]["awb_enable"] + last_msg = f"AWB {selected_cam} -> {'ON' if camera_controls[selected_cam]['awb_enable'] else 'OFF'}" + else: + last_msg = "AWB só se aplica ao RGB" + last_msg_t = time.time() + elif k in (ord("i"), ord("I")): + if camera_controls[selected_cam]["exposure_time_us"] is None: + camera_controls[selected_cam]["exposure_time_us"] = 15000 + else: + camera_controls[selected_cam]["exposure_time_us"] = int( + min(camera_controls[selected_cam]["exposure_time_us"] + args.exp_step, 200000) + ) + last_msg = f"EXP {selected_cam} -> {camera_controls[selected_cam]['exposure_time_us']} us" + last_msg_t = time.time() + elif k in (ord("k"), ord("K")): + if camera_controls[selected_cam]["exposure_time_us"] is None: + camera_controls[selected_cam]["exposure_time_us"] = 15000 + else: + camera_controls[selected_cam]["exposure_time_us"] = int( + max(camera_controls[selected_cam]["exposure_time_us"] - args.exp_step, 100) + ) + last_msg = f"EXP {selected_cam} -> {camera_controls[selected_cam]['exposure_time_us']} us" + last_msg_t = time.time() + elif k in (ord("o"), ord("O")): + if camera_controls[selected_cam]["analogue_gain"] is None: + camera_controls[selected_cam]["analogue_gain"] = 1.0 + else: + camera_controls[selected_cam]["analogue_gain"] = float( + min(camera_controls[selected_cam]["analogue_gain"] * (1.0 + args.gain_step), 32.0) + ) + last_msg = f"GAIN {selected_cam} -> {camera_controls[selected_cam]['analogue_gain']:.2f}" + last_msg_t = time.time() + elif k in (ord("l"), ord("L")): + if camera_controls[selected_cam]["analogue_gain"] is None: + camera_controls[selected_cam]["analogue_gain"] = 1.0 + else: + camera_controls[selected_cam]["analogue_gain"] = float( + max(camera_controls[selected_cam]["analogue_gain"] / (1.0 + args.gain_step), 1.0) + ) + last_msg = f"GAIN {selected_cam} -> {camera_controls[selected_cam]['analogue_gain']:.2f}" + last_msg_t = time.time() + elif k in (ord("g"), ord("G")): + active_img = decoded_last.get(selected_cam, {}).get("image") + ctrl = camera_controls[selected_cam] + + result = run_guidance_analysis( + selected_cam=selected_cam, + img01=active_img, + rois_for_cam=rois[selected_cam], + ctrl=ctrl, + exp_step=args.exp_step, + gain_step=args.gain_step, + ) + + last_guidance = result + + guidance_entry = { + "timestamp": now_str(), + "camera": selected_cam, + "result": result, + } + + guidance_log.append(guidance_entry) + data_payload.setdefault("calibration_guidance_log", []).append(guidance_entry) + + after = result.get("after_settings") + if isinstance(after, dict): + camera_controls[selected_cam].update(after) + + last_msg = f"GUIDE {selected_cam}: {result.get('action')} | {result.get('status')}" + last_msg_t = time.time() + elif k in (ord("a"), ord("A")): + if live_mode: + apply_controls_to_selected_cam() + else: + last_msg = "Controles só aplicam no modo ao vivo" + last_msg_t = time.time() + elif k in (ord("u"), ord("U")): + if current_polygon_points: + current_polygon_points.pop() + last_msg = f"Ponto removido | restantes={len(current_polygon_points)}" + last_msg_t = time.time() + elif rois[selected_cam]: + removed = rois[selected_cam].pop() + last_msg = f"ROI removida: {removed['name']}" + last_msg_t = time.time() + elif k in (ord("x"), ord("X")): + current_polygon_points = [] + last_msg = "Polígono atual limpo" + last_msg_t = time.time() + elif k in (ord("c"), ord("C")): + rois[selected_cam] = [] + last_msg = f"ROIs limpas em {selected_cam}" + last_msg_t = time.time() + elif k in (ord("s"), ord("S")): + snap = snapshot_current_state() + if snap is not None: + data_payload.setdefault("snapshots", []).append(snap) + last_msg = f"Snapshot salvo: {selected_cam} | rois={len(snap['rois'])}" + else: + last_msg = "Sem frame ativo para snapshot" + last_msg_t = time.time() + elif k in (ord("f"), ord("F")): + if not live_mode: + last_msg = "Salvar frame bruto só faz sentido no modo ao vivo" + last_msg_t = time.time() + elif last_raw_frame is None or last_meta_stream is None: + last_msg = "Sem frame bruto atual para salvar" + last_msg_t = time.time() + else: + preview_to_save = last_preview_bgr + if preview_to_save is None: + preview_to_save = np.zeros((args.height, args.width, 3), dtype=np.uint8) + + meta_save = { + "ts": datetime.now().isoformat(timespec="milliseconds"), + "schema": "multispec_offline_sample_v1", + "frame_type": "RAW_BRUTO", + "sensor_width": args.width, + "sensor_height": args.height, + "bayer_pattern": args.bayer, + "capture_mode_requested": args.capture_mode, + "capture_mode_effective": effective_capture_mode, + "raw_policy": args.raw_policy, + "stream_meta": last_meta_stream, + "camera_settings": json.loads(json.dumps(camera_controls)), + "note": "offline_sample_from_sensor_calibration_tool", + } + + png_path, json_path = save_offline_sample( + base_dir=args.offline_save_dir, + preview_bgr=preview_to_save, + meta=meta_save, + packed_raw_by_camera=last_raw_frame, + ) + + last_msg = f"FRAME salvo offline: {os.path.basename(json_path)}" + last_msg_t = time.time() + elif k == 32: # SPACE + payload_to_save = build_camera_params_payload( + args=args, + effective_capture_mode=effective_capture_mode, + camera_controls=camera_controls, + rois=rois, + snapshots=data_payload.get("snapshots", []), + guidance_log=guidance_log, + ) + + save_payload(args.out_json, payload_to_save) + data_payload = payload_to_save + last_msg = f"PARAMS salvos em: {args.out_json}" + last_msg_t = time.time() + elif k == 13: # ENTER + if len(current_polygon_points) < 3: + last_msg = "ROI poligonal precisa de pelo menos 3 pontos" + last_msg_t = time.time() + else: + roi_name_input_active = True + roi_name_buffer = "" + roi_name_points_pending = list(current_polygon_points) + last_msg = "Digite o nome da ROI na tela" + last_msg_t = time.time() + + dt_loop = time.time() - t0 + if dt_loop < 0.001: + time.sleep(0.001) + + finally: + if live_mode: + cam.stop() + cv2.destroyAllWindows() + print("Fim da calibração dos sensores.") + + +if __name__ == "__main__": + main() diff --git a/Python/OAK/datasets/oak-fcc-3/test_captude_mode.py b/Python/OAK/datasets/oak-fcc-3/test_captude_mode.py new file mode 100644 index 000000000..313bbbfc6 --- /dev/null +++ b/Python/OAK/datasets/oak-fcc-3/test_captude_mode.py @@ -0,0 +1,23 @@ +from oak_fcc3_client import OakFcc3Client + +for frame_type in ["RAW_BRUTO", "RGB", "MULTISPEC"]: + print("\nTESTANDO:", frame_type) + + with OakFcc3Client( + fps=15, + width=640, + height=400, + frame_type=frame_type, + output_dtype="float32", + capture_mode="AUTO", + raw_policy="allow_single", + sync_mode="best", + sync_tolerance_ms=25.0, + ) as cam: + frame, meta, decoded = cam.get_next_decoded(timeout=2.0) + + print("frame_type:", meta.get("frame_type")) + print("layout:", meta.get("output_layout")) + print("channels:", meta.get("channels")) + print("shape:", getattr(frame, "shape", None)) + print("dtype:", getattr(frame, "dtype", None)) \ No newline at end of file diff --git a/Python/OAK/datasets/oak-fcc-3/test_manager.py b/Python/OAK/datasets/oak-fcc-3/test_manager.py new file mode 100644 index 000000000..48b1251cc --- /dev/null +++ b/Python/OAK/datasets/oak-fcc-3/test_manager.py @@ -0,0 +1,35 @@ +import cv2 +from oak_fcc3_manager import OakFcc3Manager + +with OakFcc3Manager( + fps=30, + width=640, + height=400, + capture_mode="AUTO", + raw_policy="allow_single", + sync_mode="best", + sync_tolerance_ms=10.0, + buffer_size=12, +) as cam: + + print(cam.get_status()) + + while True: + frame, meta, decoded = cam.get_next_decoded(timeout=1.0) + + print( + "frame_id:", meta["frame_id"], + "sources:", meta["payload_sources"], + "sync_ms:", f"{meta['sync_dt_ms']:.2f}", + "sync_ok:", meta["sync_ok"] + ) + + for cam_id, img in frame.items(): + info = meta["camera_info"].get(cam_id, {}) + title = f"{cam_id} | {info.get('socket')} | {info.get('role')}" + cv2.imshow(title, img) + + if cv2.waitKey(1) in (27, ord("q")): + break + +cv2.destroyAllWindows() \ No newline at end of file diff --git a/Python/OAK/datasets/oak-fcc-3/test_oak_fcc3_client.py b/Python/OAK/datasets/oak-fcc-3/test_oak_fcc3_client.py new file mode 100644 index 000000000..95c83a456 --- /dev/null +++ b/Python/OAK/datasets/oak-fcc-3/test_oak_fcc3_client.py @@ -0,0 +1,48 @@ +import cv2 + +from oak_fcc3_client import OakFcc3Client + + +with OakFcc3Client( + fps=15, + width=640, + height=400, + frame_type="RAW_BRUTO", + output_dtype="uint8", + capture_mode="AUTO", + raw_policy="allow_single", + sync_mode="best", + sync_tolerance_ms=25.0, + module_calibration_json="calibration/module_params.json", +) as cam: + + print("STATUS:", cam.get_status()) + + while True: + frame, meta, decoded = cam.get_next_decoded(timeout=2.0) + + print( + "frame_id:", meta["frame_id"], + "sources:", meta["payload_sources"], + "decoded:", list(decoded.keys()), + "sync_ms:", f"{meta.get('sync_dt_ms', 0):.2f}", + "sync_ok:", meta.get("sync_ok"), + ) + + if "cam2" in decoded: + rgb01 = decoded["cam2"]["image"] + rgb_bgr = cv2.cvtColor((rgb01 * 255).astype("uint8"), cv2.COLOR_RGB2BGR) + cv2.imshow("cam2 RGB decoded", rgb_bgr) + + if "cam0" in decoded: + re01 = decoded["cam0"]["image"] + cv2.imshow("cam0 RE decoded", (re01 * 255).astype("uint8")) + + if "cam1" in decoded: + nir01 = decoded["cam1"]["image"] + cv2.imshow("cam1 NIR decoded", (nir01 * 255).astype("uint8")) + + if cv2.waitKey(1) in (27, ord("q")): + break + +cv2.destroyAllWindows() \ No newline at end of file diff --git a/Python/OAK/datasets/oak-fcc-3/test_oak_fcc3_service.py b/Python/OAK/datasets/oak-fcc-3/test_oak_fcc3_service.py new file mode 100644 index 000000000..d1df5f1ab --- /dev/null +++ b/Python/OAK/datasets/oak-fcc-3/test_oak_fcc3_service.py @@ -0,0 +1,74 @@ +import time +import cv2 +import numpy as np + +from oak_fcc3_service import OakFcc3Service + + +svc = OakFcc3Service(timeout=10) + +last_frame = None +last_meta = None + +try: + print("CONNECT:", svc.connect()) + + print("PING:", svc.ping()) + print("STATUS:", svc.get_status()) + + print("SET FPS:", svc.set_fps(15)) + print("SET RES:", svc.set_resolution(640, 400)) + print("SET CAPTURE MODE:", svc.set_capture_mode("AUTO")) + print("SET FRAME TYPE:", svc.set_frame_type("RAW_BRUTO")) + print("SET OUTPUT DTYPE:", svc.set_output_dtype("uint8")) + + print("BEGIN:", svc.begin( + frame_type="RAW_BRUTO", + output_dtype="uint8", + capture_mode="AUTO", + )) + + for i in range(1, 6): + t0 = time.time() + frame, meta = svc.capture_frame() + tempo = time.time() - t0 + + last_frame = frame + last_meta = meta + + print( + f"CAPTURE {i}: OK, Tempo={tempo:.4f}s, " + f"type={type(frame)}, frame_type={meta.get('frame_type')}, " + f"sources={meta.get('payload_sources')}, " + f"sync_ms={meta.get('sync_dt_ms', 0):.2f}, " + f"sync_ok={meta.get('sync_ok')}" + ) + + print("CONFIG:", svc.get_config()) + print("STATUS FINAL:", svc.get_status()) + +finally: + try: + print("STOP:", svc.stop()) + except Exception as e: + print("STOP ERRO:", e) + + try: + print("DISCONNECT:", svc.disconnect()) + except Exception as e: + print("DISCONNECT ERRO:", e) + + +if last_frame is not None: + if isinstance(last_frame, dict) and "cam2" in last_frame: + cv2.imwrite("calibration/capture_cam2.jpg", last_frame["cam2"]) + print("[OK] Salvo: calibration/capture_cam2.jpg") + + elif isinstance(last_frame, dict): + first_id = list(last_frame.keys())[0] + cv2.imwrite(f"calibration/capture_{first_id}.jpg", last_frame[first_id]) + print(f"[OK] Salvo: calibration/capture_{first_id}.jpg") + + elif isinstance(last_frame, np.ndarray): + cv2.imwrite("calibration/capture.jpg", last_frame) + print("[OK] Salvo: calibration/capture.jpg") \ No newline at end of file