829 lines
32 KiB
Python
829 lines
32 KiB
Python
|
import os
|
||
|
|
import json
|
||
|
|
import time
|
||
|
|
import argparse
|
||
|
|
from datetime import datetime
|
||
|
|
|
||
|
|
import cv2
|
||
|
|
import numpy as np
|
||
|
|
|
||
|
|
from cam_3.multispectral_service import MultiSpectralService
|
||
|
|
from cam_3.stream_receiver import StreamReceiver
|
||
|
|
from cam_3.pi.raw_processor_core import RawProcessorCore
|
||
|
|
|
||
|
|
|
||
|
|
STREAM_PORT = 6001
|
||
|
|
PI_HOST = "192.168.105.6"
|
||
|
|
PC_HOST = "192.168.105.5"
|
||
|
|
|
||
|
|
|
||
|
|
# ============================================================
|
||
|
|
# 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: np.ndarray,
|
||
|
|
lines: list[str],
|
||
|
|
x: int = 12,
|
||
|
|
y: int = 22,
|
||
|
|
font_scale: float = 0.55,
|
||
|
|
line_step: int = 22,
|
||
|
|
):
|
||
|
|
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 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 resolve_effective_capture_mode(requested_mode: str) -> str:
|
||
|
|
if requested_mode in ("SINGLE", "DOUBLE", "TRIPLE"):
|
||
|
|
return requested_mode
|
||
|
|
return "AUTO"
|
||
|
|
|
||
|
|
|
||
|
|
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=40, font_scale=0.8, line_step=34)
|
||
|
|
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):
|
||
|
|
rect = roi.get("rect")
|
||
|
|
if rect is None:
|
||
|
|
continue
|
||
|
|
x0, y0, x1, y1 = rect
|
||
|
|
color = roi.get("color", color_for_index(idx))
|
||
|
|
cv2.rectangle(panel_bgr, (x0, y0), (x1, y1), color, 2)
|
||
|
|
label = roi.get("name", f"roi_{idx+1}")
|
||
|
|
cv2.putText(panel_bgr, label, (x0 + 4, max(18, y0 - 6)), cv2.FONT_HERSHEY_SIMPLEX, 0.5, color, 2, cv2.LINE_AA)
|
||
|
|
|
||
|
|
|
||
|
|
# ============================================================
|
||
|
|
# Decodificação do stream RAW_BRUTO
|
||
|
|
# ============================================================
|
||
|
|
|
||
|
|
class StreamDecoder:
|
||
|
|
def __init__(self, sensor_width: int, sensor_height: int, bayer_pattern: str = "GBRG"):
|
||
|
|
self.sensor_width = sensor_width
|
||
|
|
self.sensor_height = sensor_height
|
||
|
|
self.bayer_pattern = bayer_pattern
|
||
|
|
|
||
|
|
def decode_stream_cameras(self, frame, meta):
|
||
|
|
if not isinstance(frame, dict):
|
||
|
|
raise RuntimeError("RAW_BRUTO esperado como dict de câmeras no modo multi")
|
||
|
|
|
||
|
|
camera_frames = meta.get("camera_frames", {}) or {}
|
||
|
|
decoded = {}
|
||
|
|
|
||
|
|
if "cam2" in frame:
|
||
|
|
rgb_bgr = frame["cam2"]
|
||
|
|
if rgb_bgr.ndim != 3 or rgb_bgr.shape[2] != 3:
|
||
|
|
raise RuntimeError(f"cam2 RGB inválida: shape={rgb_bgr.shape}")
|
||
|
|
|
||
|
|
rgb = rgb_bgr[:, :, ::-1].astype(np.float32) / 255.0
|
||
|
|
decoded["cam2"] = {
|
||
|
|
"name": "RGB",
|
||
|
|
"image": rgb,
|
||
|
|
"meta": camera_frames.get("cam2", {}),
|
||
|
|
}
|
||
|
|
|
||
|
|
for cam_id, spec_name in (("cam0", "RE"), ("cam1", "NIR")):
|
||
|
|
if cam_id not in frame:
|
||
|
|
continue
|
||
|
|
|
||
|
|
packed = frame[cam_id]
|
||
|
|
if packed.ndim == 3 and packed.shape[2] == 1:
|
||
|
|
packed = packed[:, :, 0]
|
||
|
|
|
||
|
|
cam_meta = camera_frames.get(cam_id, {})
|
||
|
|
packed_width = int(cam_meta.get("width", packed.shape[1]))
|
||
|
|
height = int(cam_meta.get("height", packed.shape[0]))
|
||
|
|
bayer = cam_meta.get("bayer_pattern", self.bayer_pattern)
|
||
|
|
bit_depth = int(cam_meta.get("bit_depth", 10))
|
||
|
|
|
||
|
|
real_width = int((packed_width * 8) / 10) if bit_depth == 10 else packed_width
|
||
|
|
|
||
|
|
rp = RawProcessorCore(
|
||
|
|
sensor_width=real_width,
|
||
|
|
sensor_height=height,
|
||
|
|
bayer_pattern=bayer,
|
||
|
|
)
|
||
|
|
|
||
|
|
raw16 = rp.unpack_raw10_packed(packed)
|
||
|
|
max_val = float((1 << bit_depth) - 1)
|
||
|
|
single = np.clip(raw16.astype(np.float32) / max_val, 0.0, 1.0)
|
||
|
|
|
||
|
|
decoded[cam_id] = {
|
||
|
|
"name": spec_name,
|
||
|
|
"image": single,
|
||
|
|
"meta": cam_meta,
|
||
|
|
}
|
||
|
|
|
||
|
|
return decoded
|
||
|
|
|
||
|
|
|
||
|
|
# ============================================================
|
||
|
|
# 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": [],
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
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", [])
|
||
|
|
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)
|
||
|
|
|
||
|
|
|
||
|
|
# ============================================================
|
||
|
|
# 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=PI_HOST)
|
||
|
|
parser.add_argument("--pc_host", default=PC_HOST)
|
||
|
|
parser.add_argument("--stream_port", type=int, default=STREAM_PORT)
|
||
|
|
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="")
|
||
|
|
args = parser.parse_args()
|
||
|
|
|
||
|
|
effective_capture_mode = resolve_effective_capture_mode(args.capture_mode)
|
||
|
|
|
||
|
|
receiver = StreamReceiver(host="0.0.0.0", port=args.stream_port)
|
||
|
|
svc = MultiSpectralService(host=args.pi_host, port=args.server_port, timeout=10)
|
||
|
|
decoder = StreamDecoder(sensor_width=args.width, sensor_height=args.height, bayer_pattern=args.bayer)
|
||
|
|
|
||
|
|
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 = {}
|
||
|
|
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": [],
|
||
|
|
}
|
||
|
|
|
||
|
|
drawing_roi = False
|
||
|
|
roi_start = None
|
||
|
|
roi_current = None
|
||
|
|
|
||
|
|
def get_active_rect_for_mouse():
|
||
|
|
return panel_rects.get(selected_cam)
|
||
|
|
|
||
|
|
def on_mouse(event, x, y, flags, param):
|
||
|
|
nonlocal drawing_roi, roi_start, roi_current, 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:
|
||
|
|
drawing_roi = True
|
||
|
|
roi_start = (lx, ly)
|
||
|
|
roi_current = (lx, ly)
|
||
|
|
|
||
|
|
elif event == cv2.EVENT_MOUSEMOVE and drawing_roi:
|
||
|
|
roi_current = (lx, ly)
|
||
|
|
|
||
|
|
elif event == cv2.EVENT_LBUTTONUP and drawing_roi:
|
||
|
|
drawing_roi = False
|
||
|
|
roi_current = (lx, ly)
|
||
|
|
|
||
|
|
if roi_start is None:
|
||
|
|
return
|
||
|
|
|
||
|
|
rx0, ry0 = roi_start
|
||
|
|
rx1, ry1 = roi_current
|
||
|
|
rx0, rx1 = sorted((rx0, rx1))
|
||
|
|
ry0, ry1 = sorted((ry0, ry1))
|
||
|
|
|
||
|
|
if (rx1 - rx0) < 8 or (ry1 - ry0) < 8:
|
||
|
|
last_msg = "ROI muito pequena, ignorada"
|
||
|
|
last_msg_t = time.time()
|
||
|
|
roi_start = None
|
||
|
|
roi_current = None
|
||
|
|
return
|
||
|
|
|
||
|
|
name = f"roi_{len(rois[selected_cam]) + 1}"
|
||
|
|
roi = {
|
||
|
|
"name": name,
|
||
|
|
"rect": (rx0, ry0, rx1, ry1),
|
||
|
|
"color": color_for_index(len(rois[selected_cam])),
|
||
|
|
}
|
||
|
|
rois[selected_cam].append(roi)
|
||
|
|
last_msg = f"ROI criada em {selected_cam}: {name}"
|
||
|
|
last_msg_t = time.time()
|
||
|
|
roi_start = None
|
||
|
|
roi_current = None
|
||
|
|
|
||
|
|
cv2.setMouseCallback(window_name, on_mouse)
|
||
|
|
|
||
|
|
def apply_controls_to_selected_cam():
|
||
|
|
nonlocal last_msg, last_msg_t
|
||
|
|
ctrl = camera_controls[selected_cam]
|
||
|
|
|
||
|
|
try:
|
||
|
|
resp = 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 = 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 = 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 = 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_from_roi(active_img, roi["rect"])
|
||
|
|
roi_entries.append({
|
||
|
|
"name": roi["name"],
|
||
|
|
"rect": list(map(int, roi["rect"])),
|
||
|
|
"metrics": stats,
|
||
|
|
})
|
||
|
|
|
||
|
|
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
|
||
|
|
|
||
|
|
try:
|
||
|
|
receiver.start()
|
||
|
|
time.sleep(0.5)
|
||
|
|
|
||
|
|
print(f"[INFO] Verificando conexão com o módulo em {args.pi_host}:{args.server_port}...")
|
||
|
|
if not svc.check_connection(2):
|
||
|
|
raise RuntimeError("Módulo não encontrado ou não respondeu ao ping.")
|
||
|
|
print("[OK] Módulo conectado e respondendo.")
|
||
|
|
|
||
|
|
svc.connect()
|
||
|
|
|
||
|
|
print("SET CAM0 RES:", svc.set_camera_resolution(0, args.width, args.height))
|
||
|
|
print("SET CAM1 RES:", svc.set_camera_resolution(1, args.width, args.height))
|
||
|
|
print("SET CAM2 RES:", svc.set_camera_resolution(2, args.width, args.height))
|
||
|
|
print("SET CAM0 BAYER:", svc.set_camera_bayer(0, args.bayer))
|
||
|
|
print("SET CAM1 BAYER:", svc.set_camera_bayer(1, args.bayer))
|
||
|
|
print("SET FPS:", svc.set_fps(args.fps))
|
||
|
|
print("SET CAPTURE MODE:", svc.set_capture_mode(effective_capture_mode))
|
||
|
|
print("SET FRAME TYPE:", svc.set_frame_type("RAW_BRUTO"))
|
||
|
|
print("SET OUTPUT DTYPE:", svc.set_output_dtype("float32"))
|
||
|
|
|
||
|
|
begin_resp = svc.begin(frame_type="RAW_BRUTO", output_dtype="float32", capture_mode=effective_capture_mode)
|
||
|
|
print("BEGIN:", begin_resp)
|
||
|
|
|
||
|
|
status = svc.get_status()
|
||
|
|
print("STATUS:", json.dumps({
|
||
|
|
"status": status.get("status"),
|
||
|
|
"detected_mode": status.get("detected_mode"),
|
||
|
|
"camera_count_active": status.get("camera_count_active"),
|
||
|
|
"active_camera_ids": status.get("active_camera_ids"),
|
||
|
|
}, ensure_ascii=False))
|
||
|
|
|
||
|
|
validate_module_ready(status, "RAW_BRUTO", args.raw_policy, effective_capture_mode)
|
||
|
|
print("START STREAM:", svc.start_stream(args.pc_host, args.stream_port, fps=args.fps))
|
||
|
|
|
||
|
|
try:
|
||
|
|
for cam_id in ("cam0", "cam1", "cam2"):
|
||
|
|
initial_ctrl = 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"])
|
||
|
|
if exp_val is not None:
|
||
|
|
exp_val = int(exp_val)
|
||
|
|
camera_controls[cam_id]["exposure_time_us"] = exp_val
|
||
|
|
|
||
|
|
gain_val = initial_ctrl.get("analogue_gain", camera_controls[cam_id]["analogue_gain"])
|
||
|
|
if gain_val is not None:
|
||
|
|
gain_val = float(gain_val)
|
||
|
|
camera_controls[cam_id]["analogue_gain"] = gain_val
|
||
|
|
|
||
|
|
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: {e}")
|
||
|
|
|
||
|
|
while True:
|
||
|
|
t0 = time.time()
|
||
|
|
|
||
|
|
meta = receiver.last_meta
|
||
|
|
frame = receiver.last_frame
|
||
|
|
|
||
|
|
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 = decoder.decode_stream_cameras(frame, 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:
|
||
|
|
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"])
|
||
|
|
|
||
|
|
if drawing_roi and roi_start is not None and roi_current is not None:
|
||
|
|
active_panel = {"cam2": rgb_panel, "cam0": re_panel, "cam1": nir_panel}.get(selected_cam)
|
||
|
|
if active_panel is not None:
|
||
|
|
color = (0, 255, 255)
|
||
|
|
cv2.rectangle(active_panel, roi_start, roi_current, color, 1)
|
||
|
|
|
||
|
|
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)
|
||
|
|
|
||
|
|
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")
|
||
|
|
|
||
|
|
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_from_roi(active_img, roi["rect"])
|
||
|
|
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 +/- | A aplica",
|
||
|
|
"mouse: arrasta ROI | U desfaz ROI | C limpa ROIs",
|
||
|
|
"SPACE salva JSON | S salva snapshot | Q sai",
|
||
|
|
])
|
||
|
|
|
||
|
|
x0, y0, _, _ = panel_rects["data"]
|
||
|
|
overlay_hud(board, lines, x=x0 + 12, y=y0 + 22, font_scale=0.52, line_step=20)
|
||
|
|
|
||
|
|
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, 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_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("a"), ord("A")):
|
||
|
|
apply_controls_to_selected_cam()
|
||
|
|
elif k in (ord("u"), ord("U")):
|
||
|
|
if rois[selected_cam]:
|
||
|
|
removed = rois[selected_cam].pop()
|
||
|
|
last_msg = f"ROI removida: {removed['name']}"
|
||
|
|
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 == 32:
|
||
|
|
data_payload["camera_settings"] = json.loads(json.dumps(camera_controls))
|
||
|
|
save_payload(args.out_json, data_payload)
|
||
|
|
last_msg = f"JSON 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:
|
||
|
|
try:
|
||
|
|
print("STOP STREAM:", svc.stop_stream())
|
||
|
|
except Exception:
|
||
|
|
pass
|
||
|
|
try:
|
||
|
|
print("STOP:", svc.stop())
|
||
|
|
except Exception:
|
||
|
|
pass
|
||
|
|
try:
|
||
|
|
svc.disconnect()
|
||
|
|
except Exception:
|
||
|
|
pass
|
||
|
|
try:
|
||
|
|
receiver.stop()
|
||
|
|
except Exception:
|
||
|
|
pass
|
||
|
|
cv2.destroyAllWindows()
|
||
|
|
print("Fim da calibração dos sensores.")
|
||
|
|
|
||
|
|
|
||
|
|
if __name__ == "__main__":
|
||
|
|
main()
|