896 lines
32 KiB
Python
896 lines
32 KiB
Python
import os
|
|
import json
|
|
import time
|
|
import argparse
|
|
from datetime import datetime
|
|
from collections import deque
|
|
|
|
import cv2
|
|
import numpy as np
|
|
|
|
from core.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):
|
|
if path:
|
|
os.makedirs(path, exist_ok=True)
|
|
|
|
|
|
def overlay_hud(
|
|
img_bgr,
|
|
lines,
|
|
x=12,
|
|
y=24,
|
|
font_scale=0.58,
|
|
line_step=22,
|
|
color=(255, 255, 255),
|
|
shadow=True,
|
|
):
|
|
yy = y
|
|
h, _ = img_bgr.shape[:2]
|
|
for s in lines:
|
|
if yy > h - 8:
|
|
break
|
|
if shadow:
|
|
cv2.putText(img_bgr, str(s), (x, yy), cv2.FONT_HERSHEY_SIMPLEX,
|
|
font_scale, (0, 0, 0), 3, cv2.LINE_AA)
|
|
cv2.putText(img_bgr, str(s), (x, yy), cv2.FONT_HERSHEY_SIMPLEX,
|
|
font_scale, color, 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 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 = str(color_name).upper()
|
|
if color_name == "RE":
|
|
rgb = np.stack([g, z, z], axis=2)
|
|
elif color_name == "NIR":
|
|
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 resize_if_needed(img: np.ndarray, target_hw: tuple[int, int]) -> np.ndarray:
|
|
if img is None:
|
|
return None
|
|
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 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"], x=18, y=44, font_scale=0.8, line_step=32)
|
|
return img
|
|
|
|
|
|
def get_decoded_by_role(decoded: dict, role: str):
|
|
role = str(role).lower()
|
|
for cam_id, item in decoded.items():
|
|
if str(item.get("role", "")).lower() == role:
|
|
return cam_id, item
|
|
return None, None
|
|
|
|
|
|
def get_image_by_role(decoded: dict, role: str):
|
|
cam_id, item = get_decoded_by_role(decoded, role)
|
|
if item is None:
|
|
return cam_id, None
|
|
return cam_id, item.get("image")
|
|
|
|
|
|
def get_preview_panel_by_role(previews: dict, meta: dict, role: str):
|
|
camera_info = (meta or {}).get("camera_info", {}) or {}
|
|
for cam_id, preview in (previews or {}).items():
|
|
info = camera_info.get(cam_id, {}) or {}
|
|
if str(info.get("role", "")).lower() == role:
|
|
return preview
|
|
return None
|
|
|
|
|
|
def validate_module_ready(status: dict, frame_type: str, raw_policy: str):
|
|
if not status.get("ok", True):
|
|
raise RuntimeError(f"Status inválido retornado pelo módulo: {status}")
|
|
|
|
active_roles = status.get("active_roles", {}) or {}
|
|
active_count = int(status.get("camera_count_active", 0))
|
|
|
|
if frame_type == "RAW_BRUTO":
|
|
if raw_policy == "require_triple":
|
|
missing = [role for role in ("rgb", "nir", "re") if role not in active_roles]
|
|
if missing:
|
|
raise RuntimeError(
|
|
"RAW_BRUTO com require_triple exige rgb/nir/re ativas. "
|
|
f"Faltando: {missing}. Ativas: {active_roles}"
|
|
)
|
|
elif active_count < 1:
|
|
raise RuntimeError("RAW_BRUTO requer ao menos uma câmera ativa.")
|
|
return
|
|
|
|
raise RuntimeError(f"frame_type desconhecido para validação: {frame_type}")
|
|
|
|
|
|
def normalize_gray01(img01: np.ndarray) -> np.ndarray:
|
|
"""
|
|
Converte RGB/mono float 0..1 para mono float 0..1.
|
|
Para foco, o objetivo é medir borda, então usamos luminância no RGB.
|
|
"""
|
|
if img01 is None:
|
|
return None
|
|
|
|
arr = img01.astype(np.float32)
|
|
|
|
if arr.ndim == 3:
|
|
# img01 vem em RGB, não BGR.
|
|
r = arr[:, :, 0]
|
|
g = arr[:, :, 1]
|
|
b = arr[:, :, 2]
|
|
gray = 0.299 * r + 0.587 * g + 0.114 * b
|
|
else:
|
|
gray = arr
|
|
|
|
gray = np.nan_to_num(gray, nan=0.0, posinf=1.0, neginf=0.0)
|
|
return np.clip(gray, 0.0, 1.0)
|
|
|
|
|
|
def crop_rect(img: np.ndarray, rect):
|
|
if img is None:
|
|
return None
|
|
|
|
h, w = img.shape[:2]
|
|
x0, y0, x1, y1 = rect
|
|
x0, x1 = sorted((int(x0), int(x1)))
|
|
y0, y1 = sorted((int(y0), int(y1)))
|
|
x0 = max(0, min(w - 1, x0))
|
|
x1 = max(0, min(w, x1))
|
|
y0 = max(0, min(h - 1, y0))
|
|
y1 = max(0, min(h, y1))
|
|
|
|
if x1 <= x0 or y1 <= y0:
|
|
return None
|
|
|
|
return img[y0:y1, x0:x1]
|
|
|
|
|
|
def default_roi_for_shape(shape_hw, frac=0.42):
|
|
h, w = shape_hw
|
|
rw = int(w * frac)
|
|
rh = int(h * frac)
|
|
x0 = (w - rw) // 2
|
|
y0 = (h - rh) // 2
|
|
return (x0, y0, x0 + rw, y0 + rh)
|
|
|
|
|
|
# ============================================================
|
|
# Métricas de foco
|
|
# ============================================================
|
|
|
|
def preprocess_focus_gray(gray01: np.ndarray, equalize=False, blur_ksize=0) -> np.ndarray:
|
|
g = np.clip(gray01 * 255.0, 0, 255).astype(np.uint8)
|
|
|
|
if equalize:
|
|
g = cv2.equalizeHist(g)
|
|
|
|
if blur_ksize and blur_ksize >= 3:
|
|
if blur_ksize % 2 == 0:
|
|
blur_ksize += 1
|
|
g = cv2.GaussianBlur(g, (blur_ksize, blur_ksize), 0)
|
|
|
|
return g
|
|
|
|
|
|
def focus_laplacian_var(gray_u8: np.ndarray) -> float:
|
|
lap = cv2.Laplacian(gray_u8, cv2.CV_64F, ksize=3)
|
|
return float(lap.var())
|
|
|
|
|
|
def focus_tenengrad(gray_u8: np.ndarray) -> float:
|
|
sx = cv2.Sobel(gray_u8, cv2.CV_64F, 1, 0, ksize=3)
|
|
sy = cv2.Sobel(gray_u8, cv2.CV_64F, 0, 1, ksize=3)
|
|
mag2 = sx * sx + sy * sy
|
|
return float(np.mean(mag2))
|
|
|
|
|
|
def focus_brenner(gray_u8: np.ndarray) -> float:
|
|
arr = gray_u8.astype(np.float32)
|
|
if arr.shape[1] < 3:
|
|
return 0.0
|
|
diff = arr[:, 2:] - arr[:, :-2]
|
|
return float(np.mean(diff * diff))
|
|
|
|
|
|
def compute_focus_metrics(img01: np.ndarray, roi_rect, equalize=False) -> dict:
|
|
gray01 = normalize_gray01(img01)
|
|
roi = crop_rect(gray01, roi_rect)
|
|
|
|
if roi is None or roi.size < 64:
|
|
return {
|
|
"valid": False,
|
|
"laplacian": 0.0,
|
|
"tenengrad": 0.0,
|
|
"brenner": 0.0,
|
|
"mean": 0.0,
|
|
"std": 0.0,
|
|
"p95": 0.0,
|
|
"pct_saturated": 0.0,
|
|
"pct_dark": 0.0,
|
|
"pixels": 0,
|
|
}
|
|
|
|
gray_u8 = preprocess_focus_gray(roi, equalize=equalize)
|
|
|
|
arr = roi.astype(np.float32).reshape(-1)
|
|
return {
|
|
"valid": True,
|
|
"laplacian": focus_laplacian_var(gray_u8),
|
|
"tenengrad": focus_tenengrad(gray_u8),
|
|
"brenner": focus_brenner(gray_u8),
|
|
"mean": float(arr.mean()),
|
|
"std": float(arr.std()),
|
|
"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 metric_value(metrics: dict, method: str) -> float:
|
|
return float(metrics.get(method, 0.0) or 0.0)
|
|
|
|
|
|
def smooth_score(history, window: int) -> float:
|
|
if not history:
|
|
return 0.0
|
|
vals = [float(x["score"]) for x in list(history)[-max(1, window):]]
|
|
return float(np.mean(vals))
|
|
|
|
|
|
def analyze_trend(history, best_score, direction_name: str, drop_warn_pct=3.0) -> dict:
|
|
if len(history) < 6:
|
|
return {
|
|
"status": "coletando",
|
|
"instruction": "gire devagar e observe o grafico",
|
|
"delta": 0.0,
|
|
"pct_of_best": 0.0,
|
|
}
|
|
|
|
recent = [float(x["smooth"]) for x in list(history)[-5:]]
|
|
old = [float(x["smooth"]) for x in list(history)[-12:-7]] if len(history) >= 12 else [float(x["smooth"]) for x in list(history)[:5]]
|
|
|
|
recent_mean = float(np.mean(recent))
|
|
old_mean = float(np.mean(old))
|
|
delta = recent_mean - old_mean
|
|
|
|
pct_of_best = 0.0 if best_score <= 0 else (recent_mean / best_score) * 100.0
|
|
drop_from_best = 100.0 - pct_of_best
|
|
|
|
if best_score > 0 and drop_from_best >= drop_warn_pct:
|
|
return {
|
|
"status": "passou_do_pico",
|
|
"instruction": f"volte um pouco no sentido contrario de {direction_name}",
|
|
"delta": delta,
|
|
"pct_of_best": pct_of_best,
|
|
}
|
|
|
|
# Faixa morta para evitar feedback nervoso.
|
|
eps = max(best_score * 0.002, 1e-6)
|
|
|
|
if delta > eps:
|
|
return {
|
|
"status": "melhorando",
|
|
"instruction": f"continue {direction_name}",
|
|
"delta": delta,
|
|
"pct_of_best": pct_of_best,
|
|
}
|
|
|
|
if delta < -eps:
|
|
return {
|
|
"status": "piorando",
|
|
"instruction": f"inverta o sentido: contrario de {direction_name}",
|
|
"delta": delta,
|
|
"pct_of_best": pct_of_best,
|
|
}
|
|
|
|
return {
|
|
"status": "estavel",
|
|
"instruction": "ajuste bem fino ou trave a lente",
|
|
"delta": delta,
|
|
"pct_of_best": pct_of_best,
|
|
}
|
|
|
|
|
|
# ============================================================
|
|
# Desenho
|
|
# ============================================================
|
|
|
|
def draw_roi(panel: np.ndarray, rect, active=False):
|
|
if rect is None:
|
|
return
|
|
x0, y0, x1, y1 = map(int, rect)
|
|
color = (0, 255, 255) if active else (0, 180, 255)
|
|
cv2.rectangle(panel, (x0, y0), (x1, y1), color, 2)
|
|
cv2.putText(panel, "FOCUS ROI", (x0 + 6, max(20, y0 - 8)),
|
|
cv2.FONT_HERSHEY_SIMPLEX, 0.55, color, 2, cv2.LINE_AA)
|
|
|
|
|
|
def draw_crosshair(panel: np.ndarray):
|
|
h, w = panel.shape[:2]
|
|
cv2.line(panel, (w // 2 - 18, h // 2), (w // 2 + 18, h // 2), (255, 255, 255), 1, cv2.LINE_AA)
|
|
cv2.line(panel, (w // 2, h // 2 - 18), (w // 2, h // 2 + 18), (255, 255, 255), 1, cv2.LINE_AA)
|
|
|
|
|
|
def draw_score_bar(panel: np.ndarray, pct: float, x: int, y: int, w: int, h: int, label: str):
|
|
pct = float(max(0.0, min(100.0, pct)))
|
|
cv2.rectangle(panel, (x, y), (x + w, y + h), (80, 80, 80), 1)
|
|
fill_w = int((pct / 100.0) * w)
|
|
cv2.rectangle(panel, (x, y), (x + fill_w, y + h), (230, 230, 230), -1)
|
|
cv2.rectangle(panel, (x, y), (x + w, y + h), (180, 180, 180), 1)
|
|
cv2.putText(panel, f"{label}: {pct:5.1f}%", (x, y - 8),
|
|
cv2.FONT_HERSHEY_SIMPLEX, 0.55, (255, 255, 255), 1, cv2.LINE_AA)
|
|
|
|
|
|
def draw_history_graph(panel: np.ndarray, history, x: int, y: int, w: int, h: int, best_score: float):
|
|
cv2.rectangle(panel, (x, y), (x + w, y + h), (35, 35, 35), -1)
|
|
cv2.rectangle(panel, (x, y), (x + w, y + h), (120, 120, 120), 1)
|
|
|
|
if len(history) < 2:
|
|
cv2.putText(panel, "grafico aguardando historico...", (x + 10, y + h // 2),
|
|
cv2.FONT_HERSHEY_SIMPLEX, 0.55, (180, 180, 180), 1, cv2.LINE_AA)
|
|
return
|
|
|
|
vals = np.array([float(item["smooth"]) for item in history], dtype=np.float32)
|
|
vals = vals[-w:] # no máximo um ponto por pixel horizontal
|
|
|
|
max_val = max(float(np.max(vals)), float(best_score), 1e-6)
|
|
min_val = min(float(np.min(vals)), max_val * 0.90)
|
|
span = max(max_val - min_val, 1e-6)
|
|
|
|
pts = []
|
|
for i, v in enumerate(vals):
|
|
px = x + int((i / max(1, len(vals) - 1)) * (w - 1))
|
|
py = y + h - 1 - int(((float(v) - min_val) / span) * (h - 1))
|
|
pts.append((px, py))
|
|
|
|
for p0, p1 in zip(pts[:-1], pts[1:]):
|
|
cv2.line(panel, p0, p1, (255, 255, 255), 2, cv2.LINE_AA)
|
|
|
|
if best_score > 0:
|
|
by = y + h - 1 - int(((best_score - min_val) / span) * (h - 1))
|
|
by = max(y, min(y + h - 1, by))
|
|
cv2.line(panel, (x, by), (x + w, by), (0, 255, 255), 1, cv2.LINE_AA)
|
|
cv2.putText(panel, "best", (x + 6, max(y + 16, by - 4)),
|
|
cv2.FONT_HERSHEY_SIMPLEX, 0.45, (0, 255, 255), 1, cv2.LINE_AA)
|
|
|
|
|
|
def make_data_panel(
|
|
shape_hw,
|
|
selected_role,
|
|
method,
|
|
metrics,
|
|
score,
|
|
smooth,
|
|
best_score,
|
|
best_pct,
|
|
trend,
|
|
fps_stream,
|
|
fps_view,
|
|
direction_name,
|
|
history,
|
|
roi_rect,
|
|
roi_locked,
|
|
equalize,
|
|
):
|
|
h, w = shape_hw
|
|
panel = np.zeros((h, w, 3), dtype=np.uint8)
|
|
|
|
score_pct = 0.0 if best_score <= 0 else (smooth / best_score) * 100.0
|
|
score_pct = max(0.0, min(120.0, score_pct))
|
|
|
|
status = trend.get("status", "coletando")
|
|
instruction = trend.get("instruction", "gire devagar")
|
|
|
|
lines = [
|
|
"FOCUS CALIBRATION TOOL",
|
|
f"camera ativa: {selected_role.upper()} | metodo={method}",
|
|
f"score={score:.1f} | smooth={smooth:.1f}",
|
|
f"best={best_score:.1f} | atual/best={score_pct:.1f}%",
|
|
f"status={status}",
|
|
f"acao: {instruction}",
|
|
f"sentido atual: {direction_name}",
|
|
f"fps_stream={fps_stream:.1f} | fps_view={fps_view:.1f}",
|
|
f"roi={'travada' if roi_locked else 'editavel'} | equalize={'ON' if equalize else 'OFF'}",
|
|
]
|
|
|
|
if metrics and metrics.get("valid"):
|
|
lines.extend([
|
|
"-",
|
|
f"mean={metrics['mean']:.3f} std={metrics['std']:.3f} p95={metrics['p95']:.3f}",
|
|
f"sat={metrics['pct_saturated']:.2f}% dark={metrics['pct_dark']:.2f}% pixels={metrics['pixels']}",
|
|
])
|
|
|
|
overlay_hud(panel, lines, x=14, y=28, font_scale=0.58, line_step=23)
|
|
|
|
bar_y = min(h - 170, 310)
|
|
draw_score_bar(panel, min(100.0, score_pct), 18, bar_y, max(80, w - 36), 24, "nitidez relativa")
|
|
|
|
graph_y = bar_y + 52
|
|
graph_h = max(90, h - graph_y - 78)
|
|
draw_history_graph(panel, history, 18, graph_y, max(100, w - 36), graph_h, best_score)
|
|
|
|
help_lines = [
|
|
"1=RGB | 2=RE | 3=NIR | M troca metrica | D troca sentido",
|
|
"mouse arrasta ROI | C centraliza ROI | L trava ROI | E equalize",
|
|
"R reset score | S snapshot JSON | SPACE salva resultado | Q sai",
|
|
]
|
|
overlay_hud(panel, help_lines, x=14, y=h - 56, font_scale=0.48, line_step=18)
|
|
|
|
return panel
|
|
|
|
|
|
def fit_panel(img, target_hw):
|
|
th, tw = target_hw
|
|
if img.shape[:2] == (th, tw):
|
|
return img
|
|
return cv2.resize(img, (tw, th), interpolation=cv2.INTER_NEAREST)
|
|
|
|
|
|
def draw_panel_title(panel, title, selected=False):
|
|
color = (0, 255, 255) if selected else (255, 255, 255)
|
|
overlay_hud(panel, [title], x=12, y=24, font_scale=0.65, line_step=24, color=color)
|
|
|
|
|
|
# ============================================================
|
|
# Persistência
|
|
# ============================================================
|
|
|
|
def build_result_payload(args, results_by_role, snapshots):
|
|
return {
|
|
"schema": "multispec_focus_calibration_v1",
|
|
"saved_at": now_str(),
|
|
"frame_type": "RAW_BRUTO",
|
|
"capture_mode_requested": args.capture_mode,
|
|
"raw_policy": args.raw_policy,
|
|
"sensor_width": args.width,
|
|
"sensor_height": args.height,
|
|
"bayer_pattern": args.bayer,
|
|
"focus_method_default": args.method,
|
|
"notes": args.notes or "",
|
|
"results_by_role": results_by_role,
|
|
"snapshots": snapshots,
|
|
}
|
|
|
|
|
|
def save_json(path, payload):
|
|
ensure_dir(os.path.dirname(path) or ".")
|
|
payload = dict(payload)
|
|
payload["saved_at"] = now_str()
|
|
with open(path, "w", encoding="utf-8") as f:
|
|
json.dump(payload, f, ensure_ascii=False, indent=2)
|
|
|
|
|
|
# ============================================================
|
|
# Main
|
|
# ============================================================
|
|
|
|
def main():
|
|
parser = argparse.ArgumentParser(
|
|
description="Ferramenta de auxílio para foco manual das câmeras RGB/RE/NIR do módulo multiespectral.",
|
|
formatter_class=argparse.ArgumentDefaultsHelpFormatter,
|
|
)
|
|
|
|
parser.add_argument("--fps", type=int, default=20)
|
|
parser.add_argument("--width", type=int, default=1280)
|
|
parser.add_argument("--height", type=int, default=800)
|
|
parser.add_argument("--bayer", default="RGGB", 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("--module_calibration_json", default="calibration/module_params.json")
|
|
parser.add_argument("--out_json", default="calibration/focus_calibration.json")
|
|
parser.add_argument("--method", default="laplacian", choices=["laplacian", "tenengrad", "brenner"])
|
|
parser.add_argument("--history", type=int, default=260)
|
|
parser.add_argument("--smooth_window", type=int, default=5)
|
|
parser.add_argument("--drop_warn_pct", type=float, default=3.0)
|
|
parser.add_argument("--equalize", action="store_true", help="Equaliza histograma da ROI antes de medir foco")
|
|
parser.add_argument("--only_camera", default=None, choices=["CAM_A", "CAM_B", "CAM_C"])
|
|
parser.add_argument("--notes", default="")
|
|
args = parser.parse_args()
|
|
|
|
selected_role = "rgb"
|
|
method = args.method
|
|
equalize = bool(args.equalize)
|
|
direction_idx = 0
|
|
direction_names = ["rosqueando", "desrosqueando"]
|
|
|
|
decoded_last = {}
|
|
previews_last = {}
|
|
meta_last = None
|
|
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
|
|
|
|
panel_rects = {"rgb": None, "re": None, "nir": None, "data": None}
|
|
roi_rects = {"rgb": None, "re": None, "nir": None}
|
|
roi_locked = False
|
|
dragging_roi = False
|
|
drag_start = None
|
|
|
|
history_by_role = {role: deque(maxlen=args.history) for role in ("rgb", "re", "nir")}
|
|
best_by_role = {
|
|
role: {"score": 0.0, "smooth": 0.0, "metrics": None, "timestamp": None, "roi": None, "method": method}
|
|
for role in ("rgb", "re", "nir")
|
|
}
|
|
snapshots = []
|
|
last_msg = ""
|
|
last_msg_t = 0.0
|
|
|
|
window_name = "Focus Calibration Tool - Multispec"
|
|
cv2.namedWindow(window_name, cv2.WINDOW_NORMAL)
|
|
|
|
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 local_from_rect(rect, px, py):
|
|
x0, y0, _, _ = rect
|
|
return int(px - x0), int(py - y0)
|
|
|
|
def on_mouse(event, x, y, flags, param):
|
|
nonlocal dragging_roi, drag_start, last_msg, last_msg_t
|
|
|
|
if roi_locked:
|
|
return
|
|
|
|
active_rect = panel_rects.get(selected_role)
|
|
if active_rect is None or not inside(active_rect, x, y):
|
|
return
|
|
|
|
lx, ly = local_from_rect(active_rect, x, y)
|
|
|
|
if event == cv2.EVENT_LBUTTONDOWN:
|
|
dragging_roi = True
|
|
drag_start = (lx, ly)
|
|
roi_rects[selected_role] = (lx, ly, lx + 1, ly + 1)
|
|
|
|
elif event == cv2.EVENT_MOUSEMOVE and dragging_roi and drag_start is not None:
|
|
x0, y0 = drag_start
|
|
roi_rects[selected_role] = (x0, y0, lx, ly)
|
|
|
|
elif event == cv2.EVENT_LBUTTONUP and dragging_roi and drag_start is not None:
|
|
x0, y0 = drag_start
|
|
roi_rects[selected_role] = (x0, y0, lx, ly)
|
|
dragging_roi = False
|
|
drag_start = None
|
|
last_msg = f"ROI atualizada para {selected_role.upper()}"
|
|
last_msg_t = time.time()
|
|
|
|
cv2.setMouseCallback(window_name, on_mouse)
|
|
|
|
try:
|
|
with MultiSpectralClient(
|
|
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=args.module_calibration_json,
|
|
only_camera=args.only_camera,
|
|
) as cam:
|
|
|
|
validate_module_ready(cam.get_status(), "RAW_BRUTO", args.raw_policy)
|
|
|
|
while True:
|
|
t0 = time.time()
|
|
|
|
frame, meta, decoded = cam.get_next_decoded(timeout=2.0)
|
|
if frame is not None and meta is not None:
|
|
try:
|
|
previews_last = cam.build_visual_preview_from_raw(frame, meta)
|
|
except Exception:
|
|
previews_last = {}
|
|
|
|
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
|
|
meta_last = meta
|
|
|
|
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:
|
|
rgb_id, rgb01 = get_image_by_role(decoded_last, "rgb")
|
|
re_id, re01 = get_image_by_role(decoded_last, "re")
|
|
nir_id, nir01 = get_image_by_role(decoded_last, "nir")
|
|
|
|
# Base de escala visual.
|
|
if rgb01 is not None:
|
|
base_h, base_w = rgb01.shape[:2]
|
|
elif re01 is not None:
|
|
base_h, base_w = re01.shape[:2]
|
|
elif nir01 is not None:
|
|
base_h, base_w = nir01.shape[:2]
|
|
else:
|
|
base_h, base_w = args.height, args.width
|
|
|
|
for role in ("rgb", "re", "nir"):
|
|
if roi_rects[role] is None:
|
|
roi_rects[role] = default_roi_for_shape((base_h, base_w), frac=0.42)
|
|
|
|
re01 = resize_if_needed(re01, (base_h, base_w))
|
|
nir01 = resize_if_needed(nir01, (base_h, base_w))
|
|
|
|
rgb_panel = get_preview_panel_by_role(previews_last, meta_last, "rgb")
|
|
re_panel = get_preview_panel_by_role(previews_last, meta_last, "re")
|
|
nir_panel = get_preview_panel_by_role(previews_last, meta_last, "nir")
|
|
|
|
if rgb_panel is None:
|
|
rgb_panel = to_bgr_u8_from_rgb01(rgb01) if rgb01 is not None else build_empty_panel((base_h, base_w), "RGB")
|
|
if re_panel is None:
|
|
re_panel = gray_to_color_bgr(re01, "RE") if re01 is not None else build_empty_panel((base_h, base_w), "RE")
|
|
if nir_panel is None:
|
|
nir_panel = gray_to_color_bgr(nir01, "NIR") if nir01 is not None else build_empty_panel((base_h, base_w), "NIR")
|
|
|
|
# Garante que painéis e ROIs estão na mesma resolução visual.
|
|
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)
|
|
|
|
rgb_panel = fit_panel(rgb_panel, (ph, pw))
|
|
re_panel = fit_panel(re_panel, (ph, pw))
|
|
nir_panel = fit_panel(nir_panel, (ph, pw))
|
|
|
|
# Se a resolução visual mudou em relação ao decoded, escalamos a ROI para desenhar corretamente.
|
|
sx = pw / float(base_w)
|
|
sy = ph / float(base_h)
|
|
|
|
def scaled_roi(role):
|
|
r = roi_rects[role]
|
|
return (int(r[0] * sx), int(r[1] * sy), int(r[2] * sx), int(r[3] * sy))
|
|
|
|
active_img_map = {"rgb": rgb01, "re": re01, "nir": nir01}
|
|
active_img = active_img_map.get(selected_role)
|
|
active_roi = roi_rects[selected_role]
|
|
|
|
metrics = compute_focus_metrics(active_img, active_roi, equalize=equalize)
|
|
score = metric_value(metrics, method) if metrics.get("valid") else 0.0
|
|
|
|
hist = history_by_role[selected_role]
|
|
smooth_tmp = score
|
|
hist.append({
|
|
"t": time.time(),
|
|
"score": score,
|
|
"smooth": smooth_tmp,
|
|
"method": method,
|
|
})
|
|
smooth = smooth_score(hist, args.smooth_window)
|
|
hist[-1]["smooth"] = smooth
|
|
|
|
best = best_by_role[selected_role]
|
|
if smooth > best["smooth"]:
|
|
best.update({
|
|
"score": score,
|
|
"smooth": smooth,
|
|
"metrics": metrics,
|
|
"timestamp": now_str(),
|
|
"roi": list(map(int, active_roi)),
|
|
"method": method,
|
|
})
|
|
|
|
trend = analyze_trend(
|
|
hist,
|
|
best["smooth"],
|
|
direction_names[direction_idx],
|
|
drop_warn_pct=args.drop_warn_pct,
|
|
)
|
|
|
|
# Painéis com ROI
|
|
draw_roi(rgb_panel, scaled_roi("rgb"), active=(selected_role == "rgb"))
|
|
draw_roi(re_panel, scaled_roi("re"), active=(selected_role == "re"))
|
|
draw_roi(nir_panel, scaled_roi("nir"), active=(selected_role == "nir"))
|
|
|
|
draw_crosshair(rgb_panel)
|
|
draw_crosshair(re_panel)
|
|
draw_crosshair(nir_panel)
|
|
|
|
draw_panel_title(rgb_panel, f"RGB ({rgb_id}) | 1 seleciona", selected_role == "rgb")
|
|
draw_panel_title(re_panel, f"RE ({re_id}) | 2 seleciona", selected_role == "re")
|
|
draw_panel_title(nir_panel, f"NIR ({nir_id}) | 3 seleciona", selected_role == "nir")
|
|
|
|
data_panel = make_data_panel(
|
|
(ph, pw),
|
|
selected_role,
|
|
method,
|
|
metrics,
|
|
score,
|
|
smooth,
|
|
best["smooth"],
|
|
trend.get("pct_of_best", 0.0),
|
|
trend,
|
|
fps_stream,
|
|
fps_view,
|
|
direction_names[direction_idx],
|
|
hist,
|
|
active_roi,
|
|
roi_locked,
|
|
equalize,
|
|
)
|
|
|
|
panel_rects["rgb"] = (0, 0, pw, ph)
|
|
panel_rects["re"] = (pw, 0, pw * 2, ph)
|
|
panel_rects["nir"] = (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])
|
|
|
|
if last_msg and (time.time() - last_msg_t) < 2.5:
|
|
cv2.putText(board, last_msg, (16, board.shape[0] - 76),
|
|
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 modulo..."], 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 == ord("1"):
|
|
selected_role = "rgb"
|
|
last_msg = "Selecionada: RGB"
|
|
last_msg_t = time.time()
|
|
|
|
elif k == ord("2"):
|
|
selected_role = "re"
|
|
last_msg = "Selecionada: RE"
|
|
last_msg_t = time.time()
|
|
|
|
elif k == ord("3"):
|
|
selected_role = "nir"
|
|
last_msg = "Selecionada: NIR"
|
|
last_msg_t = time.time()
|
|
|
|
elif k in (ord("m"), ord("M")):
|
|
methods = ["laplacian", "tenengrad", "brenner"]
|
|
method = methods[(methods.index(method) + 1) % len(methods)]
|
|
last_msg = f"Metrica -> {method}"
|
|
last_msg_t = time.time()
|
|
|
|
elif k in (ord("d"), ord("D")):
|
|
direction_idx = 1 - direction_idx
|
|
last_msg = f"Sentido informado -> {direction_names[direction_idx]}"
|
|
last_msg_t = time.time()
|
|
|
|
elif k in (ord("e"), ord("E")):
|
|
equalize = not equalize
|
|
last_msg = f"Equalize -> {'ON' if equalize else 'OFF'}"
|
|
last_msg_t = time.time()
|
|
|
|
elif k in (ord("l"), ord("L")):
|
|
roi_locked = not roi_locked
|
|
last_msg = f"ROI -> {'travada' if roi_locked else 'editavel'}"
|
|
last_msg_t = time.time()
|
|
|
|
elif k in (ord("c"), ord("C")):
|
|
# Centraliza ROI da câmera ativa usando a resolução do último frame ativo.
|
|
active_img = {"rgb": get_image_by_role(decoded_last, "rgb")[1],
|
|
"re": get_image_by_role(decoded_last, "re")[1],
|
|
"nir": get_image_by_role(decoded_last, "nir")[1]}.get(selected_role)
|
|
if active_img is not None:
|
|
roi_rects[selected_role] = default_roi_for_shape(active_img.shape[:2], frac=0.42)
|
|
last_msg = f"ROI centralizada em {selected_role.upper()}"
|
|
else:
|
|
last_msg = "Sem imagem ativa para centralizar ROI"
|
|
last_msg_t = time.time()
|
|
|
|
elif k in (ord("r"), ord("R")):
|
|
history_by_role[selected_role].clear()
|
|
best_by_role[selected_role] = {
|
|
"score": 0.0,
|
|
"smooth": 0.0,
|
|
"metrics": None,
|
|
"timestamp": None,
|
|
"roi": list(map(int, roi_rects[selected_role])) if roi_rects[selected_role] else None,
|
|
"method": method,
|
|
}
|
|
last_msg = f"Score resetado: {selected_role.upper()}"
|
|
last_msg_t = time.time()
|
|
|
|
elif k in (ord("s"), ord("S")):
|
|
best = best_by_role[selected_role]
|
|
snap = {
|
|
"timestamp": now_str(),
|
|
"role": selected_role,
|
|
"method": method,
|
|
"roi": list(map(int, roi_rects[selected_role])) if roi_rects[selected_role] else None,
|
|
"current_best": json.loads(json.dumps(best)),
|
|
"direction_name": direction_names[direction_idx],
|
|
"equalize": equalize,
|
|
}
|
|
snapshots.append(snap)
|
|
last_msg = f"Snapshot salvo em memoria: {selected_role.upper()}"
|
|
last_msg_t = time.time()
|
|
|
|
elif k == 32:
|
|
payload = build_result_payload(args, best_by_role, snapshots)
|
|
save_json(args.out_json, payload)
|
|
last_msg = f"Resultado salvo em: {args.out_json}"
|
|
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 de foco.")
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|