690 lines
28 KiB
Python
690 lines
28 KiB
Python
|
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()
|