agrobot_base/Python/raspi/manual_fusion_calibrator.py

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 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 = cam.get_next_frame(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 = cam.core.decode_stream_cameras(frame, meta)
                    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()