diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/camera_multispectral.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/camera_multispectral.py new file mode 100644 index 000000000..f316d2a08 --- /dev/null +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/camera_multispectral.py @@ -0,0 +1,1047 @@ +import os +import time +import threading +from pathlib import Path + +import cv2 +import numpy as np + +from camera_worker.tcp_streamer import CameraTcpStreamer +from shared.enums import StatusModulo, T_Code +from shared.contexto_global_redis import ContextoGlobalRedis +from camera_worker.oak_fcc3_core.oak_fcc3_client import OakFcc3Client +from health_worker.modulos.imu import IMUCamera + + +class CameraMultispectral: + """ + Adaptador do módulo OAK-FFC-3 multiespectral para o ecossistema do robô. + + Contrato principal: + - requisitar_tensor_multispec() -> tensor CHW float32 [R,G,B,RE,NIR] 0..1 + - requisitar_frame_rgb() -> preview BGR uint8 para stream/debug + - atualizar_saude() + - enviar_frame_tcp() + - parar() + + A classe NÃO expõe lógica radiométrica/fusão/flatfield para o worker. + Isso fica dentro do core OAK-FCC-3. + """ + + def __init__( + self, + mostrar_log, + mx_id=None, + module_calibration_json=None, + width=1280, + height=800, + fps=20, + target_size=None, + cache_max_age_s=0.15, + timeout_s=1.0, + ): + self.mostrar_log = mostrar_log + self.mx_id = str(mx_id) if mx_id else None + self.stream = None + + self.dispositivo = T_Code.Cam + self.modelo = "OAK-FFC-3 Multiespectral" + self.versao = None + + self.tem_imu = False + self.tem_depth = False + + self.iniciado = False + self.rodando = False + self.ultima_saude = {} + + self.width = int(width) + self.height = int(height) + self.fps = int(fps) + self.target_size = target_size + self.timeout_s = float(timeout_s) + + self._cache_max_age_s = float(cache_max_age_s) + self._lock = threading.Lock() + + self.timestamp_ultimo_tensor = None + self.timestamp_ultimo_frame_rgb = None + self.timestamp_ultimo_raw_multi = None + + self.ultimo_tensor_multispec = None + self.ultimo_frame_rgb = None + self.ultimo_raw_multi = None + self.ultimo_meta = None + self.ultimo_decoded = None + + self._ultimo_resultado_tensor = { + "erro": "sem tensor em cache", + "duracao": 0.0, + "frame_valido": False, + } + + self._ultimo_resultado_rgb = { + "erro": "sem frame rgb em cache", + "duracao": 0.0, + "frame_valido": False, + } + + self._ultimo_resultado_raw = { + "erro": "sem raw multi em cache", + "duracao": 0.0, + "frame_valido": False, + } + + self.parametros = {} + + if module_calibration_json is None: + module_calibration_json = self._resolver_module_params_default() + + self.module_calibration_json = module_calibration_json + + ContextoGlobalRedis.atualizar_ctx_dict( + ContextoGlobalRedis.CamKey(self.mx_id), + mx_id=self.mx_id, + versao=self.versao, + iniciando=True, + iniciado=False, + rodando=False, + parametros={}, + modelo=self.modelo, + dispositivo=self.dispositivo.value, + tem_depth=False, + tem_imu=self.tem_imu, + ) + + self.imu = None + self.q_imu = None + self.tem_imu = False + + try: + self.client = OakFcc3Client( + mx_id=self.mx_id, + width=self.width, + height=self.height, + bayer="BGGR", + fps=self.fps, + frame_type="RAW_BRUTO", + output_dtype="float32", + capture_mode="TRIPLE", + raw_policy="require_triple", + module_calibration_json=self.module_calibration_json, + sync_mode="best", + sync_tolerance_ms=25.0, + ) + + resp = self.client.start(print_debug=False) + + status = self._safe_get_status() + self.tem_imu = bool(status.get("tem_imu", False)) + + if self.tem_imu: + try: + self.q_imu = self.client.svc.manager.q_imu + if self.q_imu is not None: + self.imu = IMUCamera( + self.mx_id, + self.q_imu, + freq=100, + angulo_inicial=0.0, + ) + except Exception as e: + self.tem_imu = False + self.q_imu = None + self.imu = None + self.mostrar_log(f"[CameraMultispectral] IMU indisponível: {e}") + + # Se mx_id veio None, pega o ID real aberto pelo core, se disponível. + try: + self.mx_id = str(getattr(self.client, "mx_id", None) or self.mx_id) + except Exception: + pass + + status = self._safe_get_status() + self.versao = status.get("backend", "oak_fcc3") + + self.parametros = self._montar_parametros(status=status, start_resp=resp) + + self.ultima_saude = { + "timestamp": time.time(), + "conectado": True, + "status": StatusModulo.OPERANTE.value, + "saude": 100, + "motivos": [], + "saude_individual": [], + } + + self.iniciado = True + self.rodando = True + + porta = (ContextoGlobalRedis.get_equipamento() or {}).get("base_porta_ervas") + self.stream = CameraTcpStreamer( + porta=porta, + mx_id=self.mx_id, + mostrar_log=self.mostrar_log, + ) + + except Exception as e: + self.client = None + self.iniciado = False + self.rodando = False + self.mostrar_log(f"[CameraMultispectral] Erro ao iniciar módulo: {e}") + + ContextoGlobalRedis.atualizar_ctx_dict( + ContextoGlobalRedis.CamKey(self.mx_id), + mx_id=self.mx_id, + versao=self.versao, + modelo=self.modelo, + dispositivo=self.dispositivo.value, + tem_depth=False, + tem_imu=self.tem_imu, + parametros=self.parametros, + iniciado=self.iniciado, + iniciando=False, + iniciado_em=time.time() if self.iniciado else None, + ) + + # ============================================================ + # Resolução de configuração + # ============================================================ + + def _resolver_module_params_default(self): + candidatos = [ + "calibration/module_params.json", + "Python/OAK/datasets/oak-fcc-3/calibration/module_params.json", + "Python/Scripts/workers/camera_worker/oak_fcc3_core/calibration/module_params.json", + ] + + for c in candidatos: + if os.path.isfile(c): + return c + + # Devolve o default mesmo se não existir, para o erro ficar claro no core. + return "calibration/module_params.json" + + def _montar_parametros(self, status=None, start_resp=None): + status = status or {} + + altura_camera = 111.0 + + # Valores medidos por você anteriormente: + # FOV prático: 96 x 157 cm a 111 cm de altura. + largura_real = 157.0 + altura_real = 96.0 + + cm_por_px_x = largura_real / float(self.width) + cm_por_px_y = altura_real / float(self.height) + + parametros = { + "tipo": "multispectral", + "module_calibration_json": self.module_calibration_json, + "frame_type": "MULTISPEC", + "channels": ["R", "G", "B", "RE", "NIR"], + "output_layout": "CHW", + "dtype": "float32", + "tensor_min": 0.0, + "tensor_max": 1.0, + "rgb_width": self.width, + "rgb_height": self.height, + "tensor_width": self.width if self.target_size is None else int(self.target_size[0]), + "tensor_height": self.height if self.target_size is None else int(self.target_size[1]), + "fps": self.fps, + "altura_camera": altura_camera, + "largura_real": largura_real, + "altura_real": altura_real, + "cm_por_px_x": cm_por_px_x, + "cm_por_px_y": cm_por_px_y, + "status": status, + "start_resp": start_resp, + } + + return parametros + + # ============================================================ + # Controle de vida + # ============================================================ + + def parar(self): + self.rodando = False + self.iniciado = False + + try: + if self.imu is not None: + self.imu.parar() + except Exception: + pass + + self.imu = None + self.q_imu = None + self.tem_imu = False + + try: + if self.client is not None: + self.client.stop() + except Exception as e: + try: + self.mostrar_log(f"[CameraMultispectral] Erro ao parar client: {e}") + except Exception: + pass + + self.client = None + + try: + if self.stream is not None and hasattr(self.stream, "parar"): + self.stream.parar() + except Exception: + pass + + self.stream = None + + with self._lock: + self.ultimo_tensor_multispec = None + self.ultimo_frame_rgb = None + self.ultimo_raw_multi = None + self.ultimo_meta = None + self.ultimo_decoded = None + + self.timestamp_ultimo_tensor = None + self.timestamp_ultimo_frame_rgb = None + self.timestamp_ultimo_raw_multi = None + + self._ultimo_resultado_tensor = { + "erro": "pipeline parado", + "duracao": 0.0, + "frame_valido": False, + } + + def _is_erro_fatal_depthai(self, erro): + if erro is None: + return False + + txt = str(erro) + + sinais_fatais = [ + "X_LINK_ERROR", + "Communication exception", + "Couldn't read data from stream", + "Device already closed", + "device has been closed", + "No available devices", + "Nenhum dispositivo DepthAI", + "não encontrado", + "not found", + ] + + return any(s in txt for s in sinais_fatais) + + def _marcar_desconectada(self, motivo): + agora = time.time() + + self.iniciado = False + self.rodando = False + + self.ultima_saude = { + "timestamp": agora, + "conectado": False, + "status": StatusModulo.DESCONECTADO.value, + "saude": 0, + "motivos": [str(motivo)], + "saude_individual": [], + } + + try: + from camera_worker.manager import definir_saude_camera + + definir_saude_camera( + self.mx_id, + StatusModulo.DESCONECTADO, + 0, + [str(motivo)], + False, + { + "temperatura": 0.0, + "memoria_usada": 0.0, + "executando": False, + "velocidade": "", + "sync_dt_ms": 0.0, + }, + False, + agora, + self.dispositivo, + imu={ + "valido": False, + "timestamp": 0.0, + "roll": 0.0, + "pitch": 0.0, + "yaw": 0.0, + } + ) + except Exception: + pass + + def _falha_fatal_depthai(self, motivo): + self.mostrar_log(f"[CameraMultispectral] Falha fatal DepthAI. Fechando pipeline: {motivo}") + + self._marcar_desconectada(motivo) + + try: + self.parar() + except Exception as e: + self.mostrar_log(f"[CameraMultispectral] Erro ao parar após falha fatal: {e}") + + # ============================================================ + # Captura principal + # ============================================================ + + def requisitar_tensor_multispec(self, force: bool = False, max_age_s: float = None): + """ + Retorna: + tensor, resultado + + tensor: + np.ndarray CHW float32 [R,G,B,RE,NIR] 0..1 + + Observação: + O RawProcessorCore/OakFcc3Client já deve entregar o tensor final. + Aqui fazemos apenas cache, validação leve e métricas. + """ + try: + agora = time.time() + if max_age_s is None: + max_age_s = self._cache_max_age_s + + with self._lock: + cache_ok = ( + self.ultimo_tensor_multispec is not None + and self.timestamp_ultimo_tensor is not None + and (agora - self.timestamp_ultimo_tensor) < max_age_s + ) + + if cache_ok and not force: + return self.ultimo_tensor_multispec, dict(self._ultimo_resultado_tensor) + + if self.client is None: + raise RuntimeError("OakFcc3Client não inicializado") + + t_total0 = time.perf_counter() + + # ============================================================ + # 1) get_next_decoded + # ============================================================ + t0 = time.perf_counter() + frame, meta, decoded = self.client.get_next_decoded(timeout=self.timeout_s) + t_get_decoded_ms = (time.perf_counter() - t0) * 1000.0 + + # ============================================================ + # 2) build tensor final + # ============================================================ + t0 = time.perf_counter() + + if isinstance(frame, np.ndarray): + tensor = frame + origem_tensor = "frame_ndarray" + elif isinstance(decoded, dict): + tensor = self._build_tensor_from_decoded(decoded, meta) + origem_tensor = "decoded_dict" + elif isinstance(frame, dict): + tensor = self._build_tensor_from_decoded(frame, meta) + origem_tensor = "frame_dict" + else: + raise RuntimeError( + f"Saída inesperada de get_next_decoded(): " + f"frame={type(frame)} decoded={type(decoded)}" + ) + + t_build_ms = (time.perf_counter() - t0) * 1000.0 + + # ============================================================ + # 3) validação leve + # ============================================================ + t0 = time.perf_counter() + + if not isinstance(tensor, np.ndarray): + raise RuntimeError(f"Tensor multiespectral não é ndarray: {type(tensor)}") + + if tensor.ndim != 3: + raise RuntimeError(f"Tensor multiespectral inválido: shape={tensor.shape}") + + if tensor.shape[0] != 5: + raise RuntimeError( + f"Tensor multiespectral deveria ter 5 canais, veio shape={tensor.shape}" + ) + + t_validate_ms = (time.perf_counter() - t0) * 1000.0 + + # ============================================================ + # 4) normalização defensiva opcional, sem repetir trabalho pesado + # ============================================================ + t0 = time.perf_counter() + + # Se o core já entrega float32 contíguo e 0..1, isso vira quase zero. + if tensor.dtype != np.float32: + tensor = tensor.astype(np.float32, copy=False) + + if not tensor.flags["C_CONTIGUOUS"]: + tensor = np.ascontiguousarray(tensor) + + # Não fazer nan_to_num/clip todo frame aqui por padrão. + # Isso custa caro e o core já deve garantir o contrato. + t_post_ms = (time.perf_counter() - t0) * 1000.0 + + # ============================================================ + # 5) preview desligado no runtime + # ============================================================ + preview_bgr = None + t_preview_ms = 0.0 + + ts = time.time() + dur = time.perf_counter() - t_total0 + + resultado = { + "erro": None, + "duracao": dur, + "frame_valido": True, + "shape": list(tensor.shape), + "dtype": str(tensor.dtype), + "channels": ["R", "G", "B", "RE", "NIR"], + "sync_ok": bool(meta.get("sync_ok", True)) if isinstance(meta, dict) else True, + "sync_dt_ms": float(meta.get("sync_dt_ms", 0.0)) if isinstance(meta, dict) else 0.0, + "perf": { + "origem_tensor": origem_tensor, + "get_decoded_ms": t_get_decoded_ms, + "build_ms": t_build_ms, + "validate_ms": t_validate_ms, + "post_ms": t_post_ms, + "preview_ms": t_preview_ms, + "total_ms": dur * 1000.0, + }, + } + + with self._lock: + self.ultimo_tensor_multispec = tensor + self.ultimo_meta = meta + self.ultimo_decoded = decoded + self.timestamp_ultimo_tensor = ts + self._ultimo_resultado_tensor = resultado + + self.ultimo_frame_rgb = preview_bgr + self.timestamp_ultimo_frame_rgb = ts + self._ultimo_resultado_rgb = { + "erro": None if preview_bgr is not None else "preview desabilitado no runtime", + "duracao": 0.0, + "frame_valido": preview_bgr is not None and preview_bgr.size > 0, + "origem": "tensor_multispec", + } + + self._definir_heartbeat() + + return tensor, resultado + + except Exception as e: + erro = str(e) + + resultado = { + "erro": erro, + "duracao": 0.0, + "frame_valido": False, + } + + with self._lock: + self._ultimo_resultado_tensor = resultado + + self.mostrar_log(f"[CameraMultispectral] Erro ao requisitar tensor: {e}") + + if self._is_erro_fatal_depthai(e): + self._falha_fatal_depthai(e) + + return None, resultado + + def requisitar_frame_rgb(self, force: bool = False, max_age_s: float = None): + """ + Retorna preview BGR uint8 do tensor multiespectral. + """ + try: + agora = time.time() + if max_age_s is None: + max_age_s = self._cache_max_age_s + + with self._lock: + cache_ok = ( + self.ultimo_frame_rgb is not None + and self.timestamp_ultimo_frame_rgb is not None + and (agora - self.timestamp_ultimo_frame_rgb) < max_age_s + ) + + if cache_ok and not force: + return self.ultimo_frame_rgb, dict(self._ultimo_resultado_rgb) + + # Atualiza tensor, que por consequência atualiza preview RGB. + tensor, res = self.requisitar_tensor_multispec(force=force, max_age_s=max_age_s) + + with self._lock: + if self.ultimo_frame_rgb is not None: + return self.ultimo_frame_rgb, dict(self._ultimo_resultado_rgb) + + return None, { + "erro": res.get("erro") or "preview RGB indisponível", + "duracao": res.get("duracao", 0.0), + "frame_valido": False, + } + + except Exception as e: + self.mostrar_log(f"[CameraMultispectral] Erro ao requisitar RGB: {e}") + return None, { + "erro": str(e), + "duracao": 0.0, + "frame_valido": False, + } + + def requisitar_frame_raw_multi(self, force: bool = False, max_age_s: float = None): + """ + Debug opcional. + Retorna dict por câmera: + {"CAM_A": raw, "CAM_B": raw, "CAM_C": raw}, resultado + + O weed_worker normal não precisa usar isso. + """ + try: + agora = time.perf_counter() + if max_age_s is None: + max_age_s = self._cache_max_age_s + + with self._lock: + cache_ok = ( + self.ultimo_raw_multi is not None + and self.timestamp_ultimo_raw_multi is not None + and (agora - self.timestamp_ultimo_raw_multi) < max_age_s + ) + + if cache_ok and not force: + return self.ultimo_raw_multi, dict(self._ultimo_resultado_raw) + + if self.client is None: + raise RuntimeError("OakFcc3Client não inicializado") + + t0 = time.perf_counter() + raw_frame, raw_meta = self.client.get_next_raw_frame(timeout=self.timeout_s) + ts = time.perf_counter() + dur = ts - t0 + + resultado = { + "erro": None, + "duracao": dur, + "frame_valido": bool(raw_frame), + "cameras": list(raw_frame.keys()) if isinstance(raw_frame, dict) else [], + "sync_ok": bool(raw_meta.get("sync_ok", True)) if isinstance(raw_meta, dict) else True, + "sync_dt_ms": float(raw_meta.get("sync_dt_ms", 0.0)) if isinstance(raw_meta, dict) else 0.0, + } + + with self._lock: + self.ultimo_raw_multi = raw_frame + self.ultimo_meta = raw_meta + self.timestamp_ultimo_raw_multi = ts + self._ultimo_resultado_raw = resultado + + return raw_frame, resultado + + except Exception as e: + resultado = { + "erro": str(e), + "duracao": 0.0, + "frame_valido": False, + } + + with self._lock: + self._ultimo_resultado_raw = resultado + + self.mostrar_log(f"[CameraMultispectral] Erro ao requisitar RAW multi: {e}") + return None, resultado + + # ============================================================ + # Saúde + # ============================================================ + + def atualizar_saude(self): + #self.mostrar_log(f"[{self.mx_id}] Atualizando saude {self.dispositivo.name} multispectral...") + + if self.imu is not None: + self.imu.atualizar_saude() + + conectado_ctx = (ContextoGlobalRedis.get_cameras() or {}).get(self.mx_id) is not None + + conectado = bool(self.iniciado and self.client is not None) + motivos = [] + saude = 50 + + performance = { + "temperatura": 0.0, + "memoria_usada": 0.0, + "executando": False, + "velocidade": "", + "sync_dt_ms": 0.0 + } + imu = self.imu.get_dados() if self.imu else { + "valido": False, + "timestamp": 0.0, + "roll": 0.0, + "pitch": 0.0, + "yaw": 0.0, + } + + resultado = dict(self._ultimo_resultado_tensor) + + try: + status = self._safe_get_status() + running = bool(status.get("running", False)) + performance["executando"] = running + + # Tenta chegar no device real por baixo do service/manager. + dev = None + try: + dev = self.client.svc.manager.device + except Exception: + dev = None + + if dev is not None: + try: + temp = float(dev.getChipTemperature().average) + performance["temperatura"] = temp + + if temp >= 80: + motivos.append(f"Temperatura crítica: {temp:.1f} °C") + saude -= 30 + elif temp >= 70: + motivos.append(f"Temperatura elevada: {temp:.1f} °C") + saude -= 15 + elif temp >= 60: + motivos.append(f"Temperatura acima do ideal: {temp:.1f} °C") + saude -= 5 + except Exception: + pass + + try: + ddr = float(dev.getDdrMemoryUsage().used) / 1024.0 / 1024.0 + performance["memoria_usada"] = ddr + + if ddr >= 500: + motivos.append(f"Memória DDR crítica: {ddr:.1f} MB") + saude -= 30 + elif ddr >= 400: + motivos.append(f"Memória DDR elevada: {ddr:.1f} MB") + saude -= 15 + elif ddr >= 300: + motivos.append(f"Memória DDR acima do ideal: {ddr:.1f} MB") + saude -= 5 + except Exception: + pass + + try: + speed = dev.getUsbSpeed().name + performance["velocidade"] = speed + + if str(speed).lower() not in ["super", "superplus"]: + motivos.append(f"USB lenta: {speed}") + saude -= 15 + except Exception: + pass + + if not running: + motivos.append("Pipeline parada") + saude -= 20 + + except Exception as e: + resultado = { + "erro": str(e), + "duracao": 0.0, + "frame_valido": False, + } + + if "Communication exception" in str(e) or "X_LINK_ERROR" in str(e): + conectado = False + + agora = time.time() + ts_tensor = self.timestamp_ultimo_tensor or 0.0 + frame_recente = (agora - ts_tensor) <= 2.0 + + if resultado.get("sync_dt_ms") is not None: + try: + performance["sync_dt_ms"] = float(resultado.get("sync_dt_ms", 0.0)) + except Exception: + pass + + if not conectado and not conectado_ctx: + motivos.append("desconectado") + saude = 0 + elif resultado.get("erro"): + motivos.append(resultado["erro"]) + saude = 0 + elif not resultado.get("frame_valido", False): + motivos.append("Tensor inválido ou indisponível") + saude = 0 + else: + saude += 50 + + if not frame_recente: + motivos.append("Tensor antigo") + saude -= 20 + + dur = float(resultado.get("duracao", 0.0) or 0.0) + if dur > 1.5: + saude -= 20 + motivos.append(f"Tempo elevado para captura tensor: {dur:.2f}s") + elif dur > 0.8: + saude -= 10 + motivos.append(f"Tempo moderado para captura tensor: {dur:.2f}s") + + sync_dt = float(resultado.get("sync_dt_ms", 0.0) or 0.0) + if sync_dt > 35.0: + saude -= 10 + motivos.append(f"Sincronismo alto: {sync_dt:.1f} ms") + + saude = min(max(saude, 0), 100) + + status_mod = StatusModulo.OPERANTE + if not conectado: + status_mod = StatusModulo.DESCONECTADO + elif saude <= 0: + status_mod = StatusModulo.FALHA + elif saude < 80: + status_mod = StatusModulo.ALERTA + + self.rodando = conectado and frame_recente + + saude_geral = { + "timestamp": agora, + "conectado": conectado, + "status": status_mod.value, + "saude": saude, + "motivos": motivos, + "saude_individual": [], + } + + self.ultima_saude = saude_geral + + from camera_worker.manager import definir_saude_camera + + definir_saude_camera( + self.mx_id, + status_mod, + saude, + motivos, + self.rodando, + performance, + conectado, + agora, + self.dispositivo, + imu=imu + ) + + def _definir_heartbeat(self): + from camera_worker.manager import definir_heartbeat_camera + definir_heartbeat_camera(self.mx_id) + + # ============================================================ + # Stream/debug + # ============================================================ + + def enviar_frame_tcp(self, frame_bgr): + if frame_bgr is None: + return + + if self.stream is None: + return + + self.stream.enviar_frame_tcp(frame_bgr) + + # ============================================================ + # Utils + # ============================================================ + + def _safe_get_status(self): + try: + if self.client is None: + return {} + status = self.client.get_status() + return status if isinstance(status, dict) else {} + except Exception: + return {} + + def _resize_tensor_chw(self, tensor, target_size): + target_w, target_h = int(target_size[0]), int(target_size[1]) + hwc = np.transpose(tensor, (1, 2, 0)) + hwc = cv2.resize(hwc, (target_w, target_h), interpolation=cv2.INTER_LINEAR) + out = np.transpose(hwc, (2, 0, 1)).astype(np.float32, copy=False) + return np.ascontiguousarray(out) + + def _tensor_to_preview_bgr(self, tensor): + """ + Gera preview BGR bonito a partir dos canais RGB do tensor. + Não altera o tensor científico. + """ + if tensor is None or tensor.ndim != 3 or tensor.shape[0] < 3: + return None + + rgb = np.transpose(tensor[:3].astype(np.float32), (1, 2, 0)) + rgb = np.nan_to_num(rgb, nan=0.0, posinf=1.0, neginf=0.0) + + # Stretch só para visualização. + lo = np.percentile(rgb, 1.0) + hi = np.percentile(rgb, 99.0) + + if hi > lo: + rgb_vis = (rgb - lo) / (hi - lo) + else: + rgb_vis = rgb.copy() + + rgb_vis = np.clip(rgb_vis, 0.0, 1.0) + rgb_u8 = (rgb_vis * 255.0).astype(np.uint8) + + return cv2.cvtColor(rgb_u8, cv2.COLOR_RGB2BGR) + + def _build_tensor_from_decoded(self, decoded, meta): + t_total0 = time.perf_counter() + + t0 = time.perf_counter() + decoded_fixed = self._normalize_decoded_for_core(decoded, meta) + t_fix_ms = (time.perf_counter() - t0) * 1000.0 + + if hasattr(self.client, "build_multispec_tensor"): + t0 = time.perf_counter() + try: + out = self.client.build_multispec_tensor(decoded_fixed, meta=meta) + except TypeError: + out = self.client.build_multispec_tensor(decoded_fixed, meta) + + #t_core_build_ms = (time.perf_counter() - t0) * 1000.0 + #t_total_ms = (time.perf_counter() - t_total0) * 1000.0 + #self.mostrar_log( + # "[PERF][BUILD] " + # f"fix_decoded={t_fix_ms:.1f}ms " + # f"client_build_multispec={t_core_build_ms:.1f}ms " + # f"total={t_total_ms:.1f}ms" + #) + + return out + + if hasattr(self.client, "build_infer_tensor_from_decoded"): + t0 = time.perf_counter() + try: + out = self.client.build_infer_tensor_from_decoded( + decoded_fixed, + meta, + channels_expected=5, + ) + except TypeError: + out = self.client.build_infer_tensor_from_decoded( + decoded_fixed, + meta, + 5, + ) + #t_core_build_ms = (time.perf_counter() - t0) * 1000.0 + #t_total_ms = (time.perf_counter() - t_total0) * 1000.0 + #self.mostrar_log( + # "[PERF][BUILD] " + # f"fix_decoded={t_fix_ms:.1f}ms " + # f"client_build_infer={t_core_build_ms:.1f}ms " + # f"total={t_total_ms:.1f}ms" + #) + + return out + + core = getattr(self.client, "core", None) + if core is not None and hasattr(core, "fuse_multispec_cameras"): + t0 = time.perf_counter() + out = core.fuse_multispec_cameras(decoded_fixed, meta, 5) + + #t_core_build_ms = (time.perf_counter() - t0) * 1000.0 + #t_total_ms = (time.perf_counter() - t_total0) * 1000.0 + #self.mostrar_log( + # "[PERF][BUILD] " + # f"fix_decoded={t_fix_ms:.1f}ms " + # f"core_fuse={t_core_build_ms:.1f}ms " + # f"total={t_total_ms:.1f}ms" + #) + + return out + + raise RuntimeError("Não encontrei método compatível para montar tensor MULTISPEC.") + + def _normalize_decoded_for_core(self, decoded, meta): + """ + Garante que o decoded esteja no formato esperado pelo RawProcessorCore: + + { + "CAM_A": {"role": "rgb", "image": img, "meta": {...}}, + "CAM_B": {"role": "re", "image": img, "meta": {...}}, + "CAM_C": {"role": "nir", "image": img, "meta": {...}}, + } + + Algumas versões/caminhos do client podem devolver: + {"CAM_A": ndarray, ...} + """ + if not isinstance(decoded, dict): + raise RuntimeError(f"decoded esperado como dict, veio {type(decoded)}") + + camera_info = {} + if isinstance(meta, dict): + camera_info = meta.get("camera_info", {}) or {} + + default_roles = { + "CAM_A": "rgb", + "CAM_B": "re", + "CAM_C": "nir", + } + + out = {} + + for cam_id, item in decoded.items(): + info = camera_info.get(cam_id, {}) or {} + + role = ( + info.get("role") + or default_roles.get(cam_id) + or str(cam_id).lower() + ) + + # Já está no formato correto. + if isinstance(item, dict) and "image" in item: + fixed = dict(item) + fixed["role"] = fixed.get("role") or role + + meta_item = fixed.get("meta") + if not isinstance(meta_item, dict): + meta_item = {} + + meta_item.setdefault("cam_id", cam_id) + meta_item.setdefault("role", role) + meta_item.setdefault("socket", info.get("socket", cam_id)) + meta_item.setdefault("sensor", info.get("sensor")) + fixed["meta"] = meta_item + + out[cam_id] = fixed + continue + + # Veio direto como ndarray. + if isinstance(item, np.ndarray): + out[cam_id] = { + "name": str(role).upper(), + "role": role, + "image": item.astype(np.float32, copy=False), + "meta": { + "cam_id": cam_id, + "role": role, + "socket": info.get("socket", cam_id), + "sensor": info.get("sensor"), + "timestamp": (meta.get("timestamps") or {}).get(cam_id) if isinstance(meta, dict) else None, + "shape": list(item.shape), + "dtype": str(item.dtype), + }, + } + continue + + raise RuntimeError( + f"decoded[{cam_id}] em formato inesperado: {type(item)}" + ) + + return out + diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/camera_oak.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/camera_oak.py index 3a239faae..c67f333c3 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/camera_oak.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/camera_oak.py @@ -1,6 +1,7 @@ import numpy as np import depthai as dai import time +import threading from camera_worker.tcp_streamer import CameraTcpStreamer from health_worker.modulos.imu import IMUCamera @@ -8,10 +9,11 @@ from shared.enums import StatusModulo, T_Code from shared.contexto_global_redis import ContextoGlobalRedis class CameraOak: - def __init__(self, mostrar_log, mx_id, modelo_ia_seg=None, modelo_ia_det=None, iniciar_imu=False): + def __init__(self, mostrar_log, mx_id, modelo_ia_seg=None, modelo_ia_det=None, iniciar_imu=False, perf_monitor=None): self.mostrar_log = mostrar_log self.modelo_ia_seg = modelo_ia_seg self.modelo_ia_det = modelo_ia_det + self.perf = perf_monitor self.stream = None self.dispositivo = T_Code.Vzo @@ -27,12 +29,39 @@ class CameraOak: self.rodando = False self.iniciado = False + self._rgb_seq = None + self._depth_seq = None + self._rgb_device_ts = None + self._depth_device_ts = None + + self._cache_lock = threading.Lock() + self._cache_thread = None + + self._rgb_cache = None + self._rgb_cache_ts = 0.0 + self._rgb_cache_resultado = { + "erro": "sem frame rgb em cache", + "duracao": 0, + "frame_valido": False + } + + self._depth_cache = None + self._depth_cache_ts = 0.0 + self._depth_cache_resultado = { + "erro": "sem frame depth em cache", + "duracao": 0, + "frame_valido": False + } + + self._cache_rodando = False + + self.parametros = {} + disp_list = dai.Device.getAllAvailableDevices() disp_info = next((d for d in disp_list if d.getMxId() == mx_id), None) if not disp_info: - return - #raise RuntimeError(f"Dispositivo com mxid {mx_id} não encontrado") + raise RuntimeError(f"Dispositivo com mxid {mx_id} não encontrado") self.dev_info = disp_info @@ -47,7 +76,7 @@ class CameraOak: iniciando=True, iniciado=False, rodando=False, - parametros={} + parametros=self.parametros ) # Fase 1: detectar sensores sem pipeline @@ -70,9 +99,10 @@ class CameraOak: except Exception as e: pass self.mostrar_log(f"Falha ao detectar sensores: {e}") - + ContextoGlobalRedis.atualizar_ctx_dict( ContextoGlobalRedis.CamKey(self.mx_id), + timestamp=time.time(), modelo=self.modelo, dispositivo=self.dispositivo.value, tem_depth=self.tem_depth, @@ -90,7 +120,7 @@ class CameraOak: if self.tem_imu and iniciar_imu: self.q_imu = self.device.getOutputQueue(name="imu", maxSize=50, blocking=False) - self.imu = IMUCamera(self.q_imu, freq=100, angulo_inicial=26.3) + self.imu = IMUCamera(self.mx_id, self.q_imu, freq=100, angulo_inicial=26.3) if self.modelo_ia_seg is not None: self.q_seg = self.device.getOutputQueue(name="seg", maxSize=1, blocking=False) @@ -104,6 +134,8 @@ class CameraOak: except: pass + self._iniciar_cache_frames() + if self.dispositivo == T_Code.Snr: dadosSnr = ContextoGlobalRedis.get_operacao().get("Snr", {}) self.parametros = { @@ -152,22 +184,392 @@ class CameraOak: except Exception as e: self.mostrar_log(f"Erro ao iniciar camera: {e}") pass - + ContextoGlobalRedis.atualizar_ctx_dict( ContextoGlobalRedis.CamKey(self.mx_id), + timestamp=time.time(), parametros=self.parametros, iniciado=self.iniciado, iniciando=False, iniciado_em=time.time() if self.iniciado else None ) + def parar(self): + self._cache_rodando = False + self.rodando = False + self.iniciado = False + + try: + if self._cache_thread is not None and self._cache_thread.is_alive(): + self._cache_thread.join(timeout=1.0) + except Exception: + pass + + self._cache_thread = None + + try: + if self.imu is not None: + self.imu.parar() + except Exception: + pass + + self.imu = None + + try: + if hasattr(self, "device") and self.device is not None: + self.device.close() + except Exception: + pass + + self.device = None + self.pipeline = None + + try: + self.q_video = None + self.q_depth = None + self.q_imu = None + self.q_seg = None + self.q_det = None + self.q_det_track = None + except Exception: + pass + + with self._cache_lock: + self._rgb_cache = None + self._depth_cache = None + + self._rgb_cache_ts = 0.0 + self._depth_cache_ts = 0.0 + + self.timestamp_ultimo_frame_rgb = None + self.timestamp_ultimo_frame_depth = None + self.timestamp_ultima_deteccao = None + self.timestamp_ultima_segmentacao = None + + self._rgb_cache_resultado = { + "erro": "pipeline parado", + "duracao": 0.0, + "frame_valido": False, + } + + self._depth_cache_resultado = { + "erro": "pipeline parado", + "duracao": 0.0, + "frame_valido": False, + } + + try: + self.stream = None + except Exception: + pass + + def _is_erro_fatal_depthai(self, erro): + if erro is None: + return False + + txt = str(erro) + + sinais_fatais = [ + "X_LINK_ERROR", + "Communication exception", + "Couldn't read data from stream", + "Device already closed", + "device has been closed", + "Nenhum dispositivo DepthAI", + "No available devices", + "não encontrado", + "not found", + ] + + return any(s in txt for s in sinais_fatais) + + def esta_utilizavel(self): + if not self.iniciado: + return False + + if self.device is None: + return False + + erro_rgb = (self._rgb_cache_resultado or {}).get("erro") + erro_depth = (self._depth_cache_resultado or {}).get("erro") + + if self._is_erro_fatal_depthai(erro_rgb): + return False + + if self._is_erro_fatal_depthai(erro_depth): + return False + + try: + if hasattr(self.device, "isPipelineRunning"): + if not self.device.isPipelineRunning(): + return False + except Exception as e: + if self._is_erro_fatal_depthai(e): + return False + + return True + + def tem_frame_recente(self, timeout=5.0): + agora = time.time() + + ts_rgb = self.timestamp_ultimo_frame_rgb or 0.0 + ts_depth = self.timestamp_ultimo_frame_depth or 0.0 + + rgb_ok = (agora - ts_rgb) <= timeout + depth_ok = (agora - ts_depth) <= timeout if self.tem_depth else True + + return rgb_ok or depth_ok + + def _marcar_desconectada(self, motivo): + agora = time.time() + + self.iniciado = False + self.rodando = False + + self.ultima_saude = { + "timestamp": agora, + "conectado": False, + "status": StatusModulo.DESCONECTADO.value, + "saude": 0, + "motivos": [str(motivo)], + "saude_individual": [], + } + + try: + from camera_worker.manager import definir_saude_camera + + definir_saude_camera( + self.mx_id, + StatusModulo.DESCONECTADO, + 0, + [str(motivo)], + False, + { + "temperatura": 0.0, + "memoria_usada": 0.0, + "executando": False, + "velocidade": "", + }, + False, + agora, + self.dispositivo, + imu={ + "valido": False, + "timestamp": 0.0, + "roll": 0.0, + "pitch": 0.0, + "yaw": 0.0, + } + ) + except Exception: + pass + + def _falha_fatal_depthai(self, motivo): + self.mostrar_log(f"[CameraOak] Falha fatal DepthAI. Fechando pipeline: {motivo}") + + self._marcar_desconectada(motivo) + + try: + self.parar() + except Exception as e: + self.mostrar_log(f"[CameraOak] Erro ao parar após falha fatal: {e}") + + def _iniciar_cache_frames(self): + if self._cache_rodando: + return + + self._cache_rodando = True + + def loop(): + while self._cache_rodando: + try: + teve_pkt = False + # RGB + if hasattr(self, "q_video") and self.q_video is not None: + pkts = self.q_video.getAll() + n_pkts = len(pkts) + pkt = pkts[-1] if pkts else self.q_video.tryGet() + + if pkt is not None: + teve_pkt = True + t0 = time.time() + frame = pkt.getCvFrame() + dur = time.time() - t0 + + resultado = { + "erro": None, + "duracao": dur, + "frame_valido": frame is not None and frame.shape[0] > 0 + } + + if resultado["frame_valido"]: + self.rodando = True + + pkt_ts_device = None + try: + pkt_ts_device = pkt.getTimestamp().total_seconds() + except Exception: + pass + pkt_seq = None + try: + pkt_seq = pkt.getSequenceNum() + except Exception: + pass + seq_anterior = self._rgb_seq + seq_delta = None + frames_descartados = 0 + if seq_anterior is not None and pkt_seq is not None: + seq_delta = pkt_seq - seq_anterior + frames_descartados = max(0, seq_delta - 1) + + if hasattr(self, "perf") and self.perf is not None: + self.perf.tick( + "camera_rgb", + latencia_ms=dur * 1000.0, + frame_ts_host=self._rgb_cache_ts, + frame_ts_device=pkt_ts_device, + seq=pkt_seq, + seq_delta=seq_delta, + frames_descartados=frames_descartados, + n_pkts_getall=n_pkts, + valido=resultado["frame_valido"] + ) + + with self._cache_lock: + self._rgb_cache = frame + self._rgb_cache_ts = time.time() + self._rgb_cache_resultado = resultado + self.timestamp_ultimo_frame_rgb = self._rgb_cache_ts + self._rgb_seq = pkt_seq + self._rgb_device_ts = pkt_ts_device + + # DEPTH + if self.tem_depth and hasattr(self, "q_depth") and self.q_depth is not None: + pkts = self.q_depth.getAll() + n_pkts = len(pkts) + pkt = pkts[-1] if pkts else self.q_depth.tryGet() + + if pkt is not None: + teve_pkt = True + t0 = time.time() + frame = pkt.getFrame() + dur = time.time() - t0 + + resultado = { + "erro": None, + "duracao": dur, + "frame_valido": frame is not None and frame.shape[0] > 0 + } + + if resultado["frame_valido"]: + self.rodando = True + + pkt_ts_device = None + try: + pkt_ts_device = pkt.getTimestamp().total_seconds() + except Exception: + pass + pkt_seq = None + try: + pkt_seq = pkt.getSequenceNum() + except Exception: + pass + seq_anterior = self._depth_seq + seq_delta = None + frames_descartados = 0 + if seq_anterior is not None and pkt_seq is not None: + seq_delta = pkt_seq - seq_anterior + frames_descartados = max(0, seq_delta - 1) + + if hasattr(self, "perf") and self.perf is not None: + self.perf.tick( + "camera_depth", + latencia_ms=dur * 1000.0, + frame_ts_host=self._depth_cache_ts, + frame_ts_device=pkt_ts_device, + seq=pkt_seq, + seq_delta=seq_delta, + frames_descartados=frames_descartados, + n_pkts_getall=n_pkts, + valido=resultado["frame_valido"] + ) + + with self._cache_lock: + self._depth_cache = frame + self._depth_cache_ts = time.time() + self._depth_cache_resultado = resultado + self.timestamp_ultimo_frame_depth = self._depth_cache_ts + self.ultimo_resultado_depth = resultado + self._depth_seq = pkt_seq + self._depth_device_ts = pkt_ts_device + + if not teve_pkt: + time.sleep(0.001) + + except Exception as e: + erro = str(e) + + with self._cache_lock: + self._rgb_cache_resultado = { + "erro": erro, + "duracao": 0, + "frame_valido": False + } + + self._depth_cache_resultado = { + "erro": erro, + "duracao": 0, + "frame_valido": False + } + + if self._is_erro_fatal_depthai(e): + self.mostrar_log(f"[CameraOak] Erro fatal no cache loop: {e}") + self._cache_rodando = False + + # Não chama parar() diretamente de dentro da própria thread, + # porque parar() tenta dar join nela mesma. + self._marcar_desconectada(e) + + try: + if self.device is not None: + self.device.close() + except Exception: + pass + + self.device = None + self.pipeline = None + self.iniciado = False + self.rodando = False + break + + self._cache_thread = threading.Thread(target=loop, daemon=True) + self._cache_thread.start() + def atualizar_saude(self): #self.mostrar_log(f"[{self.mx_id}] Atualizando saude {self.dispositivo.name}...") if self.imu is not None: self.imu.atualizar_saude() - conectado = ContextoGlobalRedis.get_cameras().get(self.mx_id) is not None + agora = time.time() + + ts_rgb = self.timestamp_ultimo_frame_rgb or 0.0 + ts_depth = self.timestamp_ultimo_frame_depth or 0.0 + + frame_rgb_recente = (agora - ts_rgb) <= 5.0 + frame_depth_recente = (agora - ts_depth) <= 5.0 if self.tem_depth else True + + camera_no_ctx = (ContextoGlobalRedis.get_cameras() or {}).get(self.mx_id) is not None + + erro_cache = (self._rgb_cache_resultado or {}).get("erro") + erro_fatal = self._is_erro_fatal_depthai(erro_cache) + + conectado = bool( + self.iniciado + and self.device is not None + and not erro_fatal + and (frame_rgb_recente or camera_no_ctx) + ) saude = 50 motivos = [] @@ -175,9 +577,20 @@ class CameraOak: performance = { "temperatura": 0, "memoria_usada": 0, - "executando": False, + "executando": False, "velocidade": "" } + + imu = self.imu.get_dados() if self.imu else { + "valido": False, + "timestamp": 0.0, + "roll": 0.0, + "pitch": 0.0, + "yaw": 0.0, + } + + resultado = dict(self._rgb_cache_resultado or {}) + if conectado: try: dev = self.device @@ -223,26 +636,30 @@ class CameraOak: motivos.append(f"USB lenta: {speed}") saude -= 15 - frame, resultado = self.requisitar_frame_rgb() + resultado = self._rgb_cache_resultado except Exception as e: self.mostrar_log(f"Erro ao requisitar dados da camera para atualizar saude: {e}") + resultado = { "erro": str(e), "frame_valido": False, "duracao": 0 } - if "Communication exception" in str(e) or "X_LINK_ERROR" in str(e): + + if self._is_erro_fatal_depthai(e): conectado = False + self._falha_fatal_depthai(e) if not conectado: motivos.append("desconectado") saude = 0 - elif resultado["erro"]: - motivos.append(resultado["erro"]) + elif resultado.get("erro"): + motivos.append(resultado.get("erro")) saude = 0 - if "Communication exception" in resultado["erro"] or "X_LINK_ERROR" in resultado["erro"]: + if self._is_erro_fatal_depthai(resultado.get("erro")): conectado = False - elif not resultado["frame_valido"]: + self._falha_fatal_depthai(resultado.get("erro")) + elif not resultado.get("frame_valido", False): motivos.append("Frame inválido ou vazio") saude = 0 else: @@ -277,10 +694,23 @@ class CameraOak: timeout = 2.0 ts_depth = self.timestamp_ultimo_frame_depth or 0 ts_rgb = self.timestamp_ultimo_frame_rgb or 0 - self.rodando = ((agora - ts_depth) <= timeout or (agora - ts_rgb) <= timeout) + self.rodando = bool( + conectado + and ( + (agora - (self.timestamp_ultimo_frame_rgb or 0.0)) <= 2.0 + or ( + self.tem_depth + and (agora - (self.timestamp_ultimo_frame_depth or 0.0)) <= 2.0 + ) + ) + ) from camera_worker.manager import definir_saude_camera - definir_saude_camera(self.mx_id, status, saude, motivos, self.rodando, performance, conectado, agora, self.dispositivo) + definir_saude_camera(self.mx_id, status, saude, motivos, self.rodando, performance, conectado, agora, self.dispositivo, imu=imu) + + def _definir_heartbeat(self): + from camera_worker.manager import definir_heartbeat_camera + definir_heartbeat_camera(self.mx_id) def enviar_frame_tcp(self, frame_bgr): self.stream.enviar_frame_tcp(frame_bgr) @@ -341,30 +771,34 @@ class CameraOak: except Exception as e: self.mostrar_log(f"[WARN] Falha ao montar pipeline imu: {e}") - script = pipeline.create(dai.node.Script) + script = None if self.modelo_ia_seg is not None or self.modelo_ia_det is not None: try: + script = pipeline.create(dai.node.Script) + N_seg = (self.modelo_ia_seg or {}).get("seg_every_n", 1) N_det = (self.modelo_ia_det or {}).get("det_every_n", 1) + script.setProcessor(dai.ProcessorType.LEON_CSS) script.setScript(f""" - i = 0 - while True: - f = node.io['in'].get() - send_seg = {1 if self.modelo_ia_seg else 0} and (i % {N_seg} == 0) - send_det = {1 if self.modelo_ia_det else 0} and (i % {N_det} == 0) + i = 0 + while True: + f = node.io['in'].get() + send_seg = {1 if self.modelo_ia_seg else 0} and (i % {N_seg} == 0) + send_det = {1 if self.modelo_ia_det else 0} and (i % {N_det} == 0) - if send_seg and send_det: - node.io['toSeg'].send(f) # move o original - node.io['toDet'].send(f) # clone para a segunda rota - elif send_seg: - node.io['toSeg'].send(f) - elif send_det: - node.io['toDet'].send(f) - # se nenhum for enviar, apenas descarta 'f' - i += 1 - """) + if send_seg and send_det: + node.io['toSeg'].send(f) + node.io['toDet'].send(f) + elif send_seg: + node.io['toSeg'].send(f) + elif send_det: + node.io['toDet'].send(f) + + i += 1 + """) cam.video.link(script.inputs['in']) + except Exception as e: self.mostrar_log(f"Erro ao criar script: {e}") @@ -495,43 +929,31 @@ class CameraOak: #self.mostrar_log(f"[CALIB] {self.parametros}") def _latest_pkt(self, q): - # pega tudo que chegou e fica só com o último pkts = q.getAll() if pkts: return pkts[-1] - # fallback: tentativa não-bloqueante - pkt = q.tryGet() - if pkt is not None: - return pkt - # último recurso: uma única espera curta - return q.get() # só se não tiver nada mesmo + return q.tryGet() def requisitar_frame_rgb(self): - #print("Requisitando frame") - try: - start = time.time() - pkt = self._latest_pkt(self.q_video) - frame = pkt.getCvFrame() - self.timestamp_ultimo_frame_rgb = pkt.getTimestamp().total_seconds() # ou device ts - #frame = self.q_video.get().getCvFrame() - #self.timestamp_ultimo_frame_rgb = time.time() - dur = time.time() - start + with self._cache_lock: + resultado = dict(self._rgb_cache_resultado) - resultado = { - "erro": None, - "duracao": dur, - "frame_valido": frame is not None and frame.shape[0] > 0 - } - - return frame, resultado + if self._is_erro_fatal_depthai(resultado.get("erro")): + erro = resultado.get("erro") + else: + erro = None - except Exception as e: - print(f"Erro ao requisitar frame: {e}") - return None, { - "erro": str(e), - "duracao": 0, - "frame_valido": False - } + if self._rgb_cache is None: + frame = None + else: + frame = self._rgb_cache.copy() + + if erro: + self._falha_fatal_depthai(erro) + return None, resultado + + self._definir_heartbeat() + return frame, resultado def requisitar_frame_depth(self): if not self.tem_depth: @@ -540,27 +962,26 @@ class CameraOak: "duracao": 0, "frame_valido": False } - try: - start = time.time() - pkt = self._latest_pkt(self.q_depth) - frame = pkt.getFrame() - self.timestamp_ultimo_frame_depth = pkt.getTimestamp().total_seconds() - #frame = self.q_depth.get().getFrame() - #self.timestamp_ultimo_frame_depth = time.time() - dur = time.time() - start - return frame, { - "erro": None, - "duracao": dur, - "frame_valido": frame is not None and frame.shape[0] > 0 - } + with self._cache_lock: + resultado = dict(self._depth_cache_resultado) - except Exception as e: - return None, { - "erro": str(e), - "duracao": 0, - "frame_valido": False - } + if self._is_erro_fatal_depthai(resultado.get("erro")): + erro = resultado.get("erro") + else: + erro = None + + if self._depth_cache is None: + frame = None + else: + frame = self._depth_cache.copy() + + if erro: + self._falha_fatal_depthai(erro) + return None, resultado + + self._definir_heartbeat() + return frame, resultado def requisitar_segmentacao(self): if not hasattr(self, "q_seg"): @@ -581,7 +1002,16 @@ class CameraOak: return pred_ids, {"erro": None, "duracao": dur, "frame_valido": True} except Exception as e: dur = time.time() - start - return None, {"erro": str(e), "duracao": dur, "frame_valido": False} + erro = str(e) + + if self._is_erro_fatal_depthai(e): + self._falha_fatal_depthai(e) + + return None, { + "erro": erro, + "duracao": dur, + "frame_valido": False + } def requisitar_deteccao(self, mapear_para_fullframe: bool = False): """ @@ -750,6 +1180,28 @@ class CameraOak: except Exception as e: dur = time.time() - start - return [], {"erro": str(e), "duracao": dur, "frame_valido": False} + erro = str(e) + if self._is_erro_fatal_depthai(e): + self._falha_fatal_depthai(e) + + return [], { + "erro": erro, + "duracao": dur, + "frame_valido": False + } + + + def get_cache_stats(self): + with self._cache_lock: + return { + "rgb_ts_host": self._rgb_cache_ts, + "depth_ts_host": self._depth_cache_ts, + "rgb_ts_device": self._rgb_device_ts, + "depth_ts_device": self._depth_device_ts, + "rgb_seq": self._rgb_seq, + "depth_seq": self._depth_seq, + "rgb_resultado": dict(self._rgb_cache_resultado or {}), + "depth_resultado": dict(self._depth_cache_resultado or {}), + } \ No newline at end of file diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/manager.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/manager.py index a1db468fa..dd1b2a55f 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/manager.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/manager.py @@ -10,61 +10,180 @@ class CameraManager: self._ultimo_scan = 0 def atualizar_cameras(self): - if time.time() - self._ultimo_scan < 10: + agora = time.time() + + if agora - self._ultimo_scan < 10: return - self._ultimo_scan = time.time() - + + self._ultimo_scan = agora + + TIMEOUT_CAMERA_ABERTA = 30 # pipeline aberto, worker precisa responder saúde + TIMEOUT_CAMERA_SUMIR = 60 # remove do mapa se passou disso + + dispositivos_serializados = [] + try: dispositivos = dai.Device.getAllAvailableDevices() - dispositivos_serializados = [d.getMxId() for d in dispositivos] + + for d in dispositivos: + device_id = None + + try: + device_id = d.getMxId() + except Exception: + pass + + if not device_id: + try: + device_id = d.getDeviceId() + except Exception: + pass + + if not device_id: + device_id = getattr(d, "deviceId", None) + + if device_id: + dispositivos_serializados.append(str(device_id)) + except Exception as e: - dispositivos_serializados = [] + self.mostrar_log("[CameraManager] Erro ao listar dispositivos DepthAI:", e) try: with GalService() as cam: - if cam: - info = cam.get_device_info() - if info: - dispositivos_serializados.append(info.get("serial")) - except Exception as e: + info = cam.get_device_info() if cam else None + serial = info.get("serial") if info else None + + if serial: + dispositivos_serializados.append(str(serial)) + + except Exception: pass - - cameras_mapeadas = ContextoGlobalRedis.get_cameras() - + + dispositivos_serializados = list(set(dispositivos_serializados)) + + #self.mostrar_log("Dispositivos Serializados", dispositivos_serializados) + + cameras_mapeadas = ContextoGlobalRedis.get_cameras() or {} + + # 1) Atualiza câmeras vistas fisicamente no scan for mx_id in dispositivos_serializados: cam_existente = cameras_mapeadas.get(mx_id) + if not cam_existente: cameras_mapeadas[mx_id] = { - "timestamp": self._ultimo_scan, - "mx_id": mx_id, + "timestamp": agora, + "mx_id": mx_id, + "presente_scan": True, + "status_scan": "detectada", } else: - cam_existente["timestamp"] = self._ultimo_scan + #cam_existente["timestamp"] = agora + cam_existente["presente_scan"] = True + cam_existente["status_scan"] = "detectada" ids_para_remover = [] - for cam_id, val in cameras_mapeadas.items(): - cam = ContextoGlobalRedis.get_camera(cam_id) - if cam is not None and cam.get("saude", {}).get("status", StatusModulo.DESCONECTADO.value) != StatusModulo.DESCONECTADO.value: - if cam.get("dispositivo", T_Code.Vzo.value) == T_Code.Snr.value: - ContextoGlobalRedis.publicar_comando(CmdKey.VisualWorkerRx, { "cmd": VisualWorkerCommandType.AtualizarSaudeCamera.value } ) - elif cam.get("dispositivo", T_Code.Vzo.value) == T_Code.Cam.value: - ContextoGlobalRedis.publicar_comando(CmdKey.WeedWorkerRx, { "cmd": WeedWorkerCommandType.AtualizarSaudeCamera.value } ) - elif cam is not None and val.get("timestamp", 0) < self._ultimo_scan: + + # 2) Avalia câmeras já conhecidas + for cam_id, val in list(cameras_mapeadas.items()): + cam = ContextoGlobalRedis.get_camera(cam_id) or val + + timestamp = cam.get("timestamp", val.get("timestamp", 0)) + idade = agora - timestamp + #self.mostrar_log(f"mx_id: {cam_id}, ts: {timestamp}, idade: {idade}") + + vista_no_scan = cam_id in dispositivos_serializados + + dispositivo = cam.get("dispositivo", val.get("dispositivo")) + + # Se não apareceu no scan, pode estar com pipeline aberto. + # Então pedimos saúde, mas só se ainda estiver dentro da janela tolerável. + if not vista_no_scan and idade <= TIMEOUT_CAMERA_ABERTA: + if dispositivo == T_Code.Snr.value: + ContextoGlobalRedis.publicar_comando( + CmdKey.VisualWorkerRx, + {"cmd": VisualWorkerCommandType.AtualizarSaudeCamera.value} + ) + + elif dispositivo == T_Code.Cam.value: + ContextoGlobalRedis.publicar_comando( + CmdKey.WeedWorkerRx, + {"cmd": WeedWorkerCommandType.AtualizarSaudeCamera.value} + ) + + val["presente_scan"] = False + val["status_scan"] = "provavelmente_em_uso" + + # Se passou muito tempo sem scan e sem saúde, remove + elif not vista_no_scan and idade > TIMEOUT_CAMERA_SUMIR: ids_para_remover.append(cam_id) + # Se apareceu no scan, mantém + elif vista_no_scan: + val["presente_scan"] = True + val["status_scan"] = "detectada" + for cam_id in ids_para_remover: + self.mostrar_log(f"[CameraManager] Removendo câmera inativa: {cam_id}") cameras_mapeadas.pop(cam_id, None) + ContextoGlobalRedis.set(CtxKey.DadosCameras, cameras_mapeadas) - - visual_worker_camera_id = ContextoGlobalRedis.get_equipamento().get("camera_caminho_id") - if visual_worker_camera_id is not None and visual_worker_camera_id in cameras_mapeadas: - ContextoGlobalRedis.publicar_comando(CmdKey.VisualWorkerRx, { "cmd": VisualWorkerCommandType.IniciarCameraManager.value, "params": visual_worker_camera_id } ) - weed_worker_camera_id = ContextoGlobalRedis.get_equipamento().get("camera_solo_id") - if (weed_worker_camera_id is not None and weed_worker_camera_id in cameras_mapeadas): - ContextoGlobalRedis.publicar_comando(CmdKey.WeedWorkerRx, { "cmd": WeedWorkerCommandType.IniciarCameraManager.value, "params": weed_worker_camera_id } ) + + #self.mostrar_log("Cameras Mapeadas", cameras_mapeadas) + + equipamento = ContextoGlobalRedis.get_equipamento() or {} + + visual_worker_camera_id = equipamento.get("camera_caminho_id") + weed_worker_camera_id = equipamento.get("camera_solo_id") + + # 3) Só tenta iniciar se a câmera estiver realmente válida + if self._camera_pode_iniciar(visual_worker_camera_id, cameras_mapeadas, agora): + ContextoGlobalRedis.publicar_comando( + CmdKey.VisualWorkerRx, + { + "cmd": VisualWorkerCommandType.IniciarCameraManager.value, + "params": visual_worker_camera_id + } + ) + + if self._camera_pode_iniciar(weed_worker_camera_id, cameras_mapeadas, agora): + ContextoGlobalRedis.publicar_comando( + CmdKey.WeedWorkerRx, + { + "cmd": WeedWorkerCommandType.IniciarCameraManager.value, + "params": weed_worker_camera_id + } + ) + + def _camera_pode_iniciar(self, camera_id, cameras_mapeadas, agora): + if not camera_id: + return False + + cam = cameras_mapeadas.get(camera_id) + if not cam: + return False + + timestamp = cam.get("timestamp", 0) + idade = agora - timestamp + + presente_scan = cam.get("presente_scan", False) + status_scan = cam.get("status_scan") + + # Pode iniciar se foi vista no scan agora. + if presente_scan: + return True + + # Se não apareceu no scan porque já está em uso, NÃO manda iniciar de novo. + if status_scan == "provavelmente_em_uso": + return False + + # Segurança extra: se está velha, não inicia. + if idade > 30: + return False + + return True -def definir_saude_camera(mx_id: str, status: StatusModulo, saude: int, motivos: list, rodando: bool, performance: dict, conectado: bool, ts=None, disp: T_Code = None): +def definir_saude_camera(mx_id: str, status: StatusModulo, saude: int, motivos: list, rodando: bool, performance: dict, conectado: bool, ts=None, disp: T_Code = None, imu: dict = None): #print(f"Definindo saude da camera {mx_id}") _camera = ContextoGlobalRedis.get_camera(mx_id) or {} #print(_cameras) @@ -78,9 +197,12 @@ def definir_saude_camera(mx_id: str, status: StatusModulo, saude: int, motivos: } #if not conectado: # _cameras[mx_id] = {} + if rodando: + _camera["timestamp"] = time.time() _camera["saude"] = saude_geral _camera["rodando"] = rodando _camera["performance"] = performance + _camera["imu"] = imu if disp is not None: _camera["dispositivo"] = disp.value disp = T_Code(_camera.get("dispositivo", T_Code.Vzo.value)) @@ -90,4 +212,18 @@ def definir_saude_camera(mx_id: str, status: StatusModulo, saude: int, motivos: saude=saude_geral ) ContextoGlobalRedis.set(ContextoGlobalRedis.CamKey(mx_id), _camera) + +def definir_imu_camera(mx_id: str, dados: dict, saude: dict): + ContextoGlobalRedis.atualizar_ctx_dict( + ContextoGlobalRedis.CamImuKey(mx_id), + timestamp=time.time(), + dados=dados, + saude=saude + ) +def definir_heartbeat_camera(mx_id: str): + ContextoGlobalRedis.atualizar_ctx_dict( + ContextoGlobalRedis.CamKey(mx_id), + timestamp=time.time() + ) + diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/oak_fcc3_core/benchmark.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/oak_fcc3_core/benchmark.py new file mode 100644 index 000000000..755895987 --- /dev/null +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/oak_fcc3_core/benchmark.py @@ -0,0 +1,620 @@ +import argparse +import json +import sys +import time +from pathlib import Path + +import cv2 +import numpy as np + +try: + import torch +except Exception: + torch = None + + +# ============================================================ +# Ajuste de import local +# ============================================================ + +THIS_FILE = Path(__file__).resolve() + +# Esperado: +# .../Python/Scripts/workers/camera_worker/oak_fcc3_core/benchmark_raw_bruto_scientific.py +WORKERS_DIR = THIS_FILE.parents[2] + +if str(WORKERS_DIR) not in sys.path: + sys.path.insert(0, str(WORKERS_DIR)) + +from camera_worker.oak_fcc3_core.oak_fcc3_client import OakFcc3Client + +try: + from camera_worker.oak_fcc3_core.segformer_service import MultiSpecSegformerService +except Exception: + MultiSpecSegformerService = None + + +# ============================================================ +# Utils +# ============================================================ + +def now_ms(): + return time.perf_counter() * 1000.0 + + +def mean(xs): + return float(np.mean(xs)) if xs else 0.0 + + +def p95(xs): + return float(np.percentile(xs, 95)) if xs else 0.0 + + +def maxv(xs): + return float(np.max(xs)) if xs else 0.0 + + +def last_mean(xs, n=30): + return float(np.mean(xs[-n:])) if xs else 0.0 + + +def last_max(xs, n=30): + return float(np.max(xs[-n:])) if xs else 0.0 + + +def load_json_if_exists(path): + if not path: + return None + with open(path, "r", encoding="utf-8") as f: + return json.load(f) + + +def parse_float_list(s, expected=5, default=None): + if s is None: + return default + + vals = [float(x.strip()) for x in str(s).split(",") if x.strip() != ""] + + if len(vals) != expected: + raise ValueError(f"Esperado {expected} valores, veio {len(vals)}: {s}") + + return np.array(vals, dtype=np.float32) + + +def extract_mean_std_from_model_config(cfg): + if not isinstance(cfg, dict): + return None, None + + mean_cfg = cfg.get("mean") or cfg.get("channel_mean") or cfg.get("norm_mean") + std_cfg = cfg.get("std") or cfg.get("channel_std") or cfg.get("norm_std") + + norm = cfg.get("normalization") or cfg.get("norm") or {} + + if mean_cfg is None and isinstance(norm, dict): + mean_cfg = norm.get("mean") or norm.get("channel_mean") + + if std_cfg is None and isinstance(norm, dict): + std_cfg = norm.get("std") or norm.get("channel_std") + + if mean_cfg is None or std_cfg is None: + return None, None + + mean_arr = np.array(mean_cfg, dtype=np.float32) + std_arr = np.array(std_cfg, dtype=np.float32) + + if mean_arr.size != 5 or std_arr.size != 5: + return None, None + + return mean_arr, std_arr + + +def tensor_stats(tensor): + names = ["R", "G", "B", "RE", "NIR"] + out = {} + + for i, name in enumerate(names): + ch = tensor[i].astype(np.float32) + out[name] = { + "min": float(np.min(ch)), + "p01": float(np.percentile(ch, 1)), + "p50": float(np.percentile(ch, 50)), + "p99": float(np.percentile(ch, 99)), + "max": float(np.max(ch)), + "mean": float(np.mean(ch)), + "std": float(np.std(ch)), + } + + return out + + +def print_tensor_stats(label, tensor): + print("============================================") + print(f"[{label}] TENSOR") + print(f"shape={tensor.shape} dtype={tensor.dtype}") + + stats = tensor_stats(tensor) + for ch, s in stats.items(): + print( + f"{ch:>3} | " + f"min={s['min']:.4f} " + f"p01={s['p01']:.4f} " + f"p50={s['p50']:.4f} " + f"p99={s['p99']:.4f} " + f"max={s['max']:.4f} " + f"mean={s['mean']:.4f} " + f"std={s['std']:.4f}" + ) + + print("============================================") + + +def to_u8_01(arr): + arr = np.asarray(arr, dtype=np.float32) + arr = np.nan_to_num(arr, nan=0.0, posinf=1.0, neginf=0.0) + arr = np.clip(arr, 0.0, 1.0) + return (arr * 255.0).astype(np.uint8) + + +def make_panel(tensor): + rgb = np.stack([tensor[0], tensor[1], tensor[2]], axis=2) + rgb_bgr = cv2.cvtColor(to_u8_01(rgb), cv2.COLOR_RGB2BGR) + + re_bgr = cv2.cvtColor(to_u8_01(tensor[3]), cv2.COLOR_GRAY2BGR) + nir_bgr = cv2.cvtColor(to_u8_01(tensor[4]), cv2.COLOR_GRAY2BGR) + + false_rgb = np.stack( + [ + tensor[4], # visual R = NIR + tensor[3], # visual G = RE + tensor[0], # visual B = R real + ], + axis=2, + ) + false_bgr = cv2.cvtColor(to_u8_01(false_rgb), cv2.COLOR_RGB2BGR) + + def title(img, text): + h, w = img.shape[:2] + bar_h = 34 + bar = np.zeros((bar_h, w, 3), dtype=np.uint8) + cv2.putText( + bar, + text, + (10, 24), + cv2.FONT_HERSHEY_SIMPLEX, + 0.7, + (255, 255, 255), + 2, + cv2.LINE_AA, + ) + return np.vstack([bar, img]) + + rgb_bgr = title(rgb_bgr, "RGB") + re_bgr = title(re_bgr, "RE") + nir_bgr = title(nir_bgr, "NIR") + false_bgr = title(false_bgr, "Falso color NIR/RE/R") + + top = np.hstack([rgb_bgr, re_bgr]) + bottom = np.hstack([nir_bgr, false_bgr]) + + return np.vstack([top, bottom]) + + +# ============================================================ +# Benchmark processor +# ============================================================ + +class ScientificBenchmark: + def __init__(self, args): + self.args = args + self.target_size = (int(args.width), int(args.height)) + + self.model_cfg = load_json_if_exists(args.model_config_json) + mean_cfg, std_cfg = extract_mean_std_from_model_config(self.model_cfg) + + if args.model_mean is not None: + self.mean = parse_float_list(args.model_mean, expected=5) + elif mean_cfg is not None: + self.mean = mean_cfg + else: + self.mean = np.array([0.5, 0.5, 0.5, 0.5, 0.5], dtype=np.float32) + + if args.model_std is not None: + self.std = parse_float_list(args.model_std, expected=5) + elif std_cfg is not None: + self.std = std_cfg + else: + self.std = np.array([0.25, 0.25, 0.25, 0.25, 0.25], dtype=np.float32) + + self.mean_chw = self.mean[:, None, None].astype(np.float32) + self.std_chw = self.std[:, None, None].astype(np.float32) + + self.device = None + if torch is not None and torch.cuda.is_available(): + self.device = torch.device("cuda") + elif torch is not None: + self.device = torch.device("cpu") + + self.model_svc = None + + if args.run_model: + if MultiSpecSegformerService is None: + raise RuntimeError("MultiSpecSegformerService não pôde ser importado.") + + if not isinstance(self.model_cfg, dict): + raise RuntimeError("--run_model requer --model_config_json válido.") + + self.model_svc = MultiSpecSegformerService( + model_config=self.model_cfg, + mostrar_log=print, + ) + + dummy = np.zeros((5, args.height, args.width), dtype=np.float32) + + for _ in range(max(0, int(args.warmup_model))): + self.model_svc.infer_tensor_fast(dummy, keep_probs=False) + + print(f"[BENCH] Warmup modelo concluído: {args.warmup_model}x") + + def process_once(self, client): + """ + Mede uma iteração completa do fluxo científico. + """ + times = { + "capture_ms": 0.0, + "decode_ms": 0.0, + "controller_ms": 0.0, + "fuse_total_ms": 0.0, + "fuse_dark_ms": 0.0, + "fuse_radnorm_ms": 0.0, + "fuse_flat_ms": 0.0, + "fuse_prepare_ms": 0.0, + "fuse_warp_ms": 0.0, + "fuse_crop_resize_ms": 0.0, + "fuse_concat_ms": 0.0, + "model_norm_ms": 0.0, + "torch_ms": 0.0, + "infer_ms": 0.0, + "total_ms": 0.0, + "sync_dt_ms": 0.0, + } + + t_total0 = now_ms() + + # ======================================================== + # 1. Captura RAW_BRUTO + # ======================================================== + t0 = now_ms() + raw_frame, raw_meta = client.get_next_raw_frame(timeout=self.args.timeout) + times["capture_ms"] = now_ms() - t0 + + #cp = raw_meta.get("capture_perf", {}) + #print( + # "[CAP_ASYNC] " + # f"get_wait={cp.get('async_get_wait_ms',0):.2f}ms " + # f"age={cp.get('async_packet_age_ms',0):.2f}ms " + # f"seq={cp.get('async_packet_seq')} " + # f"thread_wait={cp.get('wait_total_ms',0):.2f}ms " + # f"sleep={cp.get('sleep_ms',0):.2f}ms/{cp.get('sleep_count',0)} " + # f"drain={cp.get('drain_total_ms',0):.2f}ms " + # f"copy={cp.get('drain_frombuffer_copy_ms',0):.2f}ms " + # f"status={cp.get('async_status',{})}" + #) + + times["sync_dt_ms"] = float(raw_meta.get("sync_dt_ms", 0.0) or 0.0) + + # ======================================================== + # 2. Decode RAW10 packed -> float científico por câmera + # ======================================================== + t0 = now_ms() + decoded = client.decode_stream_cameras(raw_frame, raw_meta) + times["decode_ms"] = now_ms() - t0 + + # ======================================================== + # 3. RadiometricController update, se ativo + # Isso NÃO é a radiometric_normalization do tensor. + # É o controlador de exposição/ganho. + # ======================================================== + t0 = now_ms() + client.update_radiometry(decoded, raw_meta) + times["controller_ms"] = now_ms() - t0 + + # ======================================================== + # 4. Fusão científica no RawProcessorCore + # dark + radnorm + flat + homografia + crop/resize + concat + # ======================================================== + t0 = now_ms() + tensor = client.build_infer_tensor_from_decoded( + decoded=decoded, + meta=raw_meta, + channels_expected=5, + target_size=self.target_size, + ) + times["fuse_total_ms"] = now_ms() - t0 + + # Pega detalhamento interno do core + try: + perf = (client.core.last_fusion_result or {}).get("perf", {}) or {} + times["fuse_dark_ms"] = float(perf.get("dark_ms", 0.0) or 0.0) + times["fuse_radnorm_ms"] = float(perf.get("radnorm_ms", 0.0) or 0.0) + times["fuse_flat_ms"] = float(perf.get("flat_ms", 0.0) or 0.0) + times["fuse_prepare_ms"] = float(perf.get("prepare_ms", 0.0) or 0.0) + times["fuse_warp_ms"] = float(perf.get("warp_total_ms", 0.0) or 0.0) + times["fuse_crop_resize_ms"] = float(perf.get("crop_resize_ms", 0.0) or 0.0) + times["fuse_concat_ms"] = float(perf.get("concat_ms", 0.0) or 0.0) + except Exception: + pass + + # ======================================================== + # 5. Normalização do modelo, opcional + # ======================================================== + if self.args.simulate_model_norm: + t0 = now_ms() + tensor = (tensor - self.mean_chw) / np.maximum(self.std_chw, 1e-6) + tensor = np.ascontiguousarray(tensor, dtype=np.float32) + times["model_norm_ms"] = now_ms() - t0 + + # ======================================================== + # 6. Transferência para torch/cuda, opcional + # ======================================================== + if self.args.to_torch: + if torch is None: + raise RuntimeError("--to_torch requer torch instalado.") + + t0 = now_ms() + x = torch.from_numpy(tensor).unsqueeze(0).to(self.device, non_blocking=True) + + if self.device is not None and self.device.type == "cuda": + torch.cuda.synchronize() + + times["torch_ms"] = now_ms() - t0 + + # ======================================================== + # 7. Inferência real, opcional + # ======================================================== + pred = None + if self.args.run_model: + t0 = now_ms() + pred = self.model_svc.infer_tensor_fast(tensor, keep_probs=False) + times["infer_ms"] = now_ms() - t0 + + times["total_ms"] = now_ms() - t_total0 + + return tensor, pred, raw_frame, raw_meta, decoded, times + + +# ============================================================ +# Main +# ============================================================ + +def main(): + ap = argparse.ArgumentParser( + description="Benchmark científico OAK-FCC-3 RAW_BRUTO -> tensor multiespectral final." + ) + + ap.add_argument( + "--module_params", + default=r"C:\ZendionInc\agrobot_base\Python\OAK\datasets\oak-fcc-3\calibration\module_params.json", + help="Caminho do module_params.json.", + ) + + ap.add_argument("--width", type=int, default=1024, help="Largura final do tensor.") + ap.add_argument("--height", type=int, default=640, help="Altura final do tensor.") + ap.add_argument("--fps", type=float, default=30.0) + ap.add_argument("--seconds", type=float, default=20.0) + ap.add_argument("--timeout", type=float, default=3.0) + ap.add_argument("--warmup", type=int, default=5) + ap.add_argument("--mx_id", default=None) + ap.add_argument("--sync_tolerance_ms", type=float, default=25.0) + ap.add_argument("--buffer_size", type=int, default=8) + + ap.add_argument("--simulate_model_norm", action="store_true") + ap.add_argument("--model_mean", default=None) + ap.add_argument("--model_std", default=None) + ap.add_argument("--to_torch", action="store_true") + + ap.add_argument("--run_model", action="store_true") + ap.add_argument("--model_config_json", default=None) + ap.add_argument("--warmup_model", type=int, default=3) + + ap.add_argument("--save_debug", action="store_true") + ap.add_argument("--debug_dir", default="raw_bruto_scientific_benchmark") + ap.add_argument("--show", action="store_true") + ap.add_argument("--display_scale", type=float, default=0.65) + + args = ap.parse_args() + + bench = ScientificBenchmark(args) + + client = OakFcc3Client( + width=args.width, + height=args.height, + fps=args.fps, + frame_type="RAW_BRUTO", + output_dtype="uint8", + capture_mode="TRIPLE", + raw_policy="require_triple", + module_calibration_json=args.module_params, + sync_tolerance_ms=args.sync_tolerance_ms, + buffer_size=args.buffer_size, + mx_id=args.mx_id, + ) + + client.core.warmup_numba_raw10_decode() + + samples = { + "capture_ms": [], + "decode_ms": [], + "controller_ms": [], + "fuse_total_ms": [], + "fuse_dark_ms": [], + "fuse_radnorm_ms": [], + "fuse_flat_ms": [], + "fuse_prepare_ms": [], + "fuse_warp_ms": [], + "fuse_crop_resize_ms": [], + "fuse_concat_ms": [], + "model_norm_ms": [], + "torch_ms": [], + "infer_ms": [], + "total_ms": [], + "sync_dt_ms": [], + } + + n_frames = 0 + t_start = time.perf_counter() + t_last_log = t_start + + last_tensor = None + last_meta = None + + try: + client.start(print_debug=True) + + # Warmup de câmera/controlador/filas + print(f"[BENCH] Warmup frames: {args.warmup}") + for _ in range(max(0, int(args.warmup))): + try: + bench.process_once(client) + except Exception as e: + print(f"[WARN] warmup falhou: {type(e).__name__}: {e}") + time.sleep(0.02) + + print("============================================") + print("[BENCH] Iniciando benchmark científico RAW_BRUTO") + print(f"target tensor : (5,{args.height},{args.width})") + print(f"duration : {args.seconds}s") + print("============================================") + + while True: + now = time.perf_counter() + elapsed = now - t_start + + if elapsed >= args.seconds: + break + + try: + tensor, pred, raw_frame, raw_meta, decoded, times = bench.process_once(client) + except TimeoutError as e: + print(f"[TIMEOUT] {e}") + continue + + n_frames += 1 + last_tensor = tensor + last_meta = raw_meta + + for k in samples: + samples[k].append(float(times.get(k, 0.0) or 0.0)) + + if now - t_last_log >= 1.0: + elapsed = now - t_start + fps = n_frames / max(elapsed, 1e-6) + + print( + "[RAW_SCI_PERF] " + f"elapsed={elapsed:.1f}s " + f"frames={n_frames} " + f"fps={fps:.2f} " + f"sync={last_mean(samples['sync_dt_ms']):.2f}ms " + f"capture={last_mean(samples['capture_ms']):.2f}ms " + f"decode={last_mean(samples['decode_ms']):.2f}ms " + f"controller={last_mean(samples['controller_ms']):.2f}ms " + f"fuse={last_mean(samples['fuse_total_ms']):.2f}ms " + f"radnorm={last_mean(samples['fuse_radnorm_ms']):.2f}ms " + f"flat={last_mean(samples['fuse_flat_ms']):.2f}ms " + f"warp={last_mean(samples['fuse_warp_ms']):.2f}ms " + f"crop_resize={last_mean(samples['fuse_crop_resize_ms']):.2f}ms " + f"concat={last_mean(samples['fuse_concat_ms']):.2f}ms " + f"model_norm={last_mean(samples['model_norm_ms']):.2f}ms " + f"torch={last_mean(samples['torch_ms']):.2f}ms " + f"infer={last_mean(samples['infer_ms']):.2f}ms " + f"total={last_mean(samples['total_ms']):.2f}ms " + f"tensor_shape={tuple(tensor.shape)}" + ) + + t_last_log = now + + elapsed_total = time.perf_counter() - t_start + fps_total = n_frames / max(elapsed_total, 1e-6) + + print("============================================") + print("RESULTADO FINAL RAW_BRUTO CIENTÍFICO") + print(f"elapsed : {elapsed_total:.2f}s") + print(f"frames : {n_frames}") + print(f"fps : {fps_total:.2f}") + print("--------------------------------------------") + + def print_metric(name): + xs = samples[name] + print( + f"{name:18s} " + f"mean={mean(xs):8.2f}ms " + f"p95={p95(xs):8.2f}ms " + f"max={maxv(xs):8.2f}ms" + ) + + for name in [ + "sync_dt_ms", + "capture_ms", + "decode_ms", + "controller_ms", + "fuse_total_ms", + "fuse_dark_ms", + "fuse_radnorm_ms", + "fuse_flat_ms", + "fuse_prepare_ms", + "fuse_warp_ms", + "fuse_crop_resize_ms", + "fuse_concat_ms", + "model_norm_ms", + "torch_ms", + "infer_ms", + "total_ms", + ]: + print_metric(name) + + print("============================================") + + if last_tensor is not None: + print_tensor_stats("LAST RAW_BRUTO SCI", last_tensor) + + if args.save_debug: + debug_dir = Path(args.debug_dir) + debug_dir.mkdir(parents=True, exist_ok=True) + + np.save(str(debug_dir / "last_tensor.npy"), last_tensor) + + stats_path = debug_dir / "last_tensor_stats.json" + with open(stats_path, "w", encoding="utf-8") as f: + json.dump(tensor_stats(last_tensor), f, indent=2, ensure_ascii=False) + + panel = make_panel(last_tensor) + cv2.imwrite(str(debug_dir / "last_tensor_panel.png"), panel) + + if last_meta is not None: + with open(debug_dir / "last_meta.json", "w", encoding="utf-8") as f: + json.dump(last_meta, f, indent=2, ensure_ascii=False) + + print(f"[SAVE] Debug salvo em: {debug_dir}") + + if args.show: + panel = make_panel(last_tensor) + + if args.display_scale and abs(args.display_scale - 1.0) > 1e-6: + new_w = max(1, int(panel.shape[1] * args.display_scale)) + new_h = max(1, int(panel.shape[0] * args.display_scale)) + panel = cv2.resize(panel, (new_w, new_h), interpolation=cv2.INTER_AREA) + + cv2.imshow("RAW_BRUTO scientific tensor", panel) + print("[INFO] Pressione qualquer tecla para fechar.") + cv2.waitKey(0) + cv2.destroyAllWindows() + + finally: + try: + client.stop() + except Exception: + pass + + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/oak_fcc3_core/benchmark_preview.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/oak_fcc3_core/benchmark_preview.py new file mode 100644 index 000000000..407a3bbbe --- /dev/null +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/oak_fcc3_core/benchmark_preview.py @@ -0,0 +1,645 @@ +import argparse +import json +import sys +import time +import threading +from pathlib import Path + +import cv2 +import numpy as np + +# ============================================================ +# Ajuste de import local +# ============================================================ +THIS_FILE = Path(__file__).resolve() +WORKERS_DIR = THIS_FILE.parents[2] +if str(WORKERS_DIR) not in sys.path: + sys.path.insert(0, str(WORKERS_DIR)) + +from camera_worker.oak_fcc3_core.oak_fcc3_client import OakFcc3Client + +try: + from camera_worker.oak_fcc3_core.segformer_service import MultiSpecSegformerService +except Exception: + MultiSpecSegformerService = None + + +# ============================================================ +# FPS / utilidades +# ============================================================ +class FpsMeter: + def __init__(self, alpha=0.15): + self.alpha = float(alpha) + self.last_ts = None + self.fps = 0.0 + + def tick(self): + now = time.perf_counter() + if self.last_ts is not None: + dt = now - self.last_ts + if dt > 1e-9: + inst = 1.0 / dt + self.fps = inst if self.fps <= 0 else (1.0 - self.alpha) * self.fps + self.alpha * inst + self.last_ts = now + return self.fps + + +def load_json_if_exists(path): + if not path: + return None + with open(path, "r", encoding="utf-8") as f: + return json.load(f) + + +def init_model_service(args): + """ + Mesmo contrato do benchmark científico: + --run_model exige --model_config_json + MultiSpecSegformerService(model_config=cfg).infer_tensor_fast(tensor, keep_probs=False) + """ + if not args.run_model: + return None + + if MultiSpecSegformerService is None: + raise RuntimeError("MultiSpecSegformerService não pôde ser importado.") + + model_cfg = load_json_if_exists(args.model_config_json) + if not isinstance(model_cfg, dict): + raise RuntimeError("--run_model requer --model_config_json válido.") + + svc = MultiSpecSegformerService( + model_config=model_cfg, + mostrar_log=print, + ) + + dummy = np.zeros((5, int(args.height), int(args.width)), dtype=np.float32) + for _ in range(max(0, int(args.warmup_model))): + svc.infer_tensor_fast(dummy, keep_probs=False) + + print(f"[MODEL] Warmup concluído: {args.warmup_model}x") + return svc + + +def to_u8_01(arr, auto_level=False): + arr = np.asarray(arr, dtype=np.float32) + arr = np.nan_to_num(arr, nan=0.0, posinf=1.0, neginf=0.0) + + if auto_level: + p1 = float(np.percentile(arr, 1)) + p99 = float(np.percentile(arr, 99)) + den = max(p99 - p1, 1e-6) + arr = (arr - p1) / den + + arr = np.clip(arr, 0.0, 1.0) + return (arr * 255.0).astype(np.uint8) + + +def ensure_bgr(img, auto_level=False): + img = np.asarray(img) + + if img.ndim == 2: + g = to_u8_01(img, auto_level=auto_level) + return cv2.cvtColor(g, cv2.COLOR_GRAY2BGR) + + if img.ndim == 3 and img.shape[2] == 3: + u8 = to_u8_01(img, auto_level=auto_level) + # decoded/tensor RGB vem em RGB; OpenCV mostra BGR + return cv2.cvtColor(u8, cv2.COLOR_RGB2BGR) + + raise RuntimeError(f"Imagem inválida para visualização: shape={img.shape}") + + +def tensor_rgb_to_bgr(tensor, auto_level=False): + rgb = np.stack([tensor[0], tensor[1], tensor[2]], axis=2) + return ensure_bgr(rgb, auto_level=auto_level) + + +def tensor_channel_to_bgr(tensor, idx, auto_level=False): + return ensure_bgr(tensor[idx], auto_level=auto_level) + + +def add_title(img, title, color=(255, 255, 255)): + out = img.copy() + h, w = out.shape[:2] + bar_h = 34 + bar = np.zeros((bar_h, w, 3), dtype=np.uint8) + cv2.putText(bar, str(title), (10, 23), cv2.FONT_HERSHEY_SIMPLEX, 0.62, color, 2, cv2.LINE_AA) + return np.vstack([bar, out]) + + +def add_hud(panel, lines): + out = panel.copy() + x, y = 12, 45 + for line in lines: + cv2.putText(out, line, (x, y), cv2.FONT_HERSHEY_SIMPLEX, 0.62, (0, 255, 255), 2, cv2.LINE_AA) + y += 24 + return out + + +def resize_tile(img, tile_w, tile_h): + return cv2.resize(img, (int(tile_w), int(tile_h)), interpolation=cv2.INTER_AREA) + + +def get_cam_by_role(decoded, role): + role = str(role).lower() + for cam_id, item in decoded.items(): + r = str(item.get("role") or item.get("meta", {}).get("role") or "").lower() + if r == role: + return cam_id + return None + + +def decoded_raw_tiles(decoded, tile_w, tile_h, auto_level=False): + """ + Retorna tiles RAW/decoded para RGB, RE, NIR antes da fusão final. + Aqui 'RAW_BRUTO' significa o conteúdo decodificado vindo das câmeras, ainda no espaço nativo. + """ + tiles = {} + + rgb_id = get_cam_by_role(decoded, "rgb") + re_id = get_cam_by_role(decoded, "re") + nir_id = get_cam_by_role(decoded, "nir") + + if rgb_id is not None: + img = decoded[rgb_id]["image"] + tiles["rgb"] = resize_tile(ensure_bgr(img, auto_level=auto_level), tile_w, tile_h) + else: + tiles["rgb"] = np.zeros((tile_h, tile_w, 3), dtype=np.uint8) + + if re_id is not None: + img = decoded[re_id]["image"] + tiles["re"] = resize_tile(ensure_bgr(img, auto_level=auto_level), tile_w, tile_h) + else: + tiles["re"] = np.zeros((tile_h, tile_w, 3), dtype=np.uint8) + + if nir_id is not None: + img = decoded[nir_id]["image"] + tiles["nir"] = resize_tile(ensure_bgr(img, auto_level=auto_level), tile_w, tile_h) + else: + tiles["nir"] = np.zeros((tile_h, tile_w, 3), dtype=np.uint8) + + return tiles + + +def tensor_tiles(tensor, tile_w, tile_h, auto_level=False): + return { + "rgb": resize_tile(tensor_rgb_to_bgr(tensor, auto_level=auto_level), tile_w, tile_h), + "re": resize_tile(tensor_channel_to_bgr(tensor, 3, auto_level=auto_level), tile_w, tile_h), + "nir": resize_tile(tensor_channel_to_bgr(tensor, 4, auto_level=auto_level), tile_w, tile_h), + } + + +def colorize_label_map(label_map, num_classes=None): + label = np.asarray(label_map) + if label.ndim == 3: + label = np.argmax(label, axis=0) + label = label.astype(np.int32) + + if num_classes is None: + num_classes = int(max(1, label.max() + 1)) + + # Paleta simples e estável. BGR. + palette = np.array([ + [40, 40, 40], + [60, 180, 60], + [60, 60, 220], + [220, 180, 60], + [180, 60, 180], + [180, 180, 60], + [60, 180, 180], + [220, 220, 220], + ], dtype=np.uint8) + + out = palette[label % len(palette)] + return out + + +def try_extract_prediction_tiles(pred, target_w, target_h): + """ + Tentativa genérica. Adapte aqui se o benchmark tiver nomes específicos das cabeças. + Retorna até 3 tiles BGR: semântica/head0/head1. + """ + if pred is None: + blank = np.zeros((target_h, target_w, 3), dtype=np.uint8) + return [blank, blank.copy(), blank.copy()], ["Pred vazio", "Head 1", "Head 2"] + + candidates = [] + names = [] + + if isinstance(pred, dict): + # nomes comuns + for key in ("mask", "pred_mask", "class_map", "semantic", "semantic_mask", "segmentation"): + if key in pred: + candidates.append(pred[key]) + names.append(key) + + heads = pred.get("heads") or pred.get("head_outputs") or pred.get("predictions") + if isinstance(heads, dict): + for k, v in heads.items(): + candidates.append(v) + names.append(str(k)) + elif isinstance(heads, (list, tuple)): + for i, v in enumerate(heads): + candidates.append(v) + names.append(f"head_{i}") + else: + candidates.append(pred) + names.append("prediction") + + tiles = [] + out_names = [] + + for name, arr in zip(names, candidates): + arr = np.asarray(arr) + + # remove batch se existir + if arr.ndim == 4 and arr.shape[0] == 1: + arr = arr[0] + + if arr.ndim == 3: + # CHW logits/probs ou HWC RGB/probs + if arr.shape[0] <= 32: + vis = colorize_label_map(np.argmax(arr, axis=0), num_classes=arr.shape[0]) + elif arr.shape[2] in (1, 3): + vis = ensure_bgr(arr[:, :, 0] if arr.shape[2] == 1 else arr, auto_level=True) + else: + vis = ensure_bgr(np.max(arr, axis=2), auto_level=True) + elif arr.ndim == 2: + # se parecer label map, colore; se parecer float, cinza auto-level + if np.issubdtype(arr.dtype, np.integer) and int(np.max(arr)) <= 64: + vis = colorize_label_map(arr) + else: + vis = ensure_bgr(arr, auto_level=True) + elif arr.ndim == 1: + # vetor de classe/score: desenha texto + vis = np.zeros((target_h, target_w, 3), dtype=np.uint8) + txt = np.array2string(arr[:8], precision=2, separator=", ") + cv2.putText(vis, txt[:80], (10, target_h // 2), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 1, cv2.LINE_AA) + else: + continue + + vis = resize_tile(vis, target_w, target_h) + tiles.append(vis) + out_names.append(name) + + if len(tiles) >= 3: + break + + while len(tiles) < 3: + tiles.append(np.zeros((target_h, target_w, 3), dtype=np.uint8)) + out_names.append(f"pred_{len(tiles)}") + + return tiles[:3], out_names[:3] + + +def try_run_model(model_svc, tensor): + """ + Inferência real, igual ao benchmark científico. + Mantém esta função isolada para adaptar fácil caso o retorno do modelo mude. + """ + if model_svc is None: + return None, "model_svc_none" + + if hasattr(model_svc, "infer_tensor_fast"): + return model_svc.infer_tensor_fast(tensor, keep_probs=False), None + + if hasattr(model_svc, "infer"): + return model_svc.infer(tensor), None + + return None, "model_svc_sem_infer" + + +def build_grid(raw_tiles, final_tiles, pred_tiles=None, pred_names=None, tile_w=420, tile_h=260): + rows = [] + row_defs = [ + ("RGB", "rgb"), + ("RE", "re"), + ("NIR", "nir"), + ] + + for i, (label, key) in enumerate(row_defs): + left = add_title(raw_tiles[key], f"RAW_BRUTO decoded {label}") + mid = add_title(final_tiles[key], f"Tensor final {label}") + + cells = [left, mid] + + if pred_tiles is not None: + name = pred_names[i] if pred_names and i < len(pred_names) else f"Pred {i}" + cells.append(add_title(pred_tiles[i], name)) + + # iguala altura após título + h_min = min(c.shape[0] for c in cells) + norm = [cv2.resize(c, (tile_w, h_min), interpolation=cv2.INTER_AREA) if c.shape[1] != tile_w or c.shape[0] != h_min else c for c in cells] + rows.append(np.hstack(norm)) + + return np.vstack(rows) + + +def get_core_perf(client): + for attr in ("raw_processor", "processor", "core", "raw_processor_core"): + obj = getattr(client, attr, None) + if obj is not None and getattr(obj, "last_fusion_result", None) is not None: + return obj.last_fusion_result.get("perf", {}) or {} + return {} + + + +# ============================================================ +# Estado compartilhado / workers assíncronos +# ============================================================ +class SharedState: + def __init__(self): + self.lock = threading.Lock() + self.running = True + self.latest_decoded = None + self.latest_meta = None + self.latest_tensor = None + self.latest_perf = {} + self.latest_pred = None + self.latest_model_warn = None + self.latest_model_ms = 0.0 + self.latest_error = None + self.tensor_fps = FpsMeter() + self.model_fps = FpsMeter() + self.preview_fps = FpsMeter() + self.tensor_seq = 0 + self.model_seq = 0 + + def stop(self): + with self.lock: + self.running = False + + def is_running(self): + with self.lock: + return bool(self.running) + + +def tensor_worker(client, state, args): + """ + Roda no talo: captura RAW_BRUTO, monta tensor final e atualiza cache. + Não depende do FPS da janela. + """ + while state.is_running(): + try: + frame, meta, decoded = client.get_next_decoded(timeout=args.timeout) + if not isinstance(frame, dict): + raise RuntimeError(f"RAW_BRUTO esperado como dict. Veio {type(frame)}") + + tensor = client.build_infer_tensor_from_decoded( + decoded=decoded, + meta=meta, + channels_expected=5, + target_size=(args.width, args.height), + ) + tensor = np.ascontiguousarray(tensor.astype(np.float32, copy=False)) + perf = get_core_perf(client) + fps = state.tensor_fps.tick() + + with state.lock: + state.latest_decoded = decoded + state.latest_meta = meta + state.latest_tensor = tensor + state.latest_perf = dict(perf or {}) + state.tensor_seq += 1 + state.latest_error = None + + except Exception as e: + with state.lock: + state.latest_error = f"tensor_worker: {type(e).__name__}: {e}" + time.sleep(0.02) + + +def model_worker(model_svc, state, args): + """ + Opcional: roda inferência no último tensor disponível. + Não bloqueia o worker de tensor nem a janela. + """ + last_seq = -1 + + while state.is_running(): + with state.lock: + tensor = None if state.latest_tensor is None else state.latest_tensor.copy() + seq = state.tensor_seq + + if tensor is None or seq == last_seq: + time.sleep(0.005) + continue + + last_seq = seq + + try: + t0_model = time.perf_counter() + pred, warn = try_run_model(model_svc, tensor) + model_ms = (time.perf_counter() - t0_model) * 1000.0 + if warn is None: + state.model_fps.tick() + + with state.lock: + state.latest_pred = pred + state.latest_model_warn = warn + state.latest_model_ms = float(model_ms) + state.model_seq += 1 + + except Exception as e: + with state.lock: + state.latest_model_warn = f"model_worker: {type(e).__name__}: {e}" + time.sleep(0.02) + + +def snapshot_state(state): + """ + Copia referências do cache para desenhar. A janela só roda na cadência do preview. + """ + with state.lock: + return { + "decoded": state.latest_decoded, + "meta": state.latest_meta, + "tensor": state.latest_tensor, + "perf": dict(state.latest_perf or {}), + "pred": state.latest_pred, + "model_warn": state.latest_model_warn, + "model_ms": state.latest_model_ms, + "error": state.latest_error, + "tensor_fps": state.tensor_fps.fps, + "model_fps": state.model_fps.fps, + "tensor_seq": state.tensor_seq, + "model_seq": state.model_seq, + } + + +# ============================================================ +# Main loop assíncrono +# ============================================================ +def main(): + ap = argparse.ArgumentParser(description="Preview assíncrono RAW_BRUTO decoded vs tensor final multispectral.") + ap.add_argument("--module_params", default=r"C:\ZendionInc\agrobot_base\Python\OAK\datasets\oak-fcc-3\calibration\module_params.json") + ap.add_argument("--width", type=int, default=1024, help="Largura final do tensor.") + ap.add_argument("--height", type=int, default=640, help="Altura final do tensor.") + ap.add_argument("--fps", type=float, default=40.0, help="FPS alvo da câmera.") + ap.add_argument("--preview_fps", type=float, default=5.0, help="FPS da janela OpenCV apenas.") + ap.add_argument("--timeout", type=float, default=2.0) + ap.add_argument("--warmup", type=int, default=5) + ap.add_argument("--mx_id", default=None) + ap.add_argument("--display_scale", type=float, default=0.75) + ap.add_argument("--tile_w", type=int, default=420) + ap.add_argument("--tile_h", type=int, default=260) + ap.add_argument("--auto_level_raw", action="store_true") + ap.add_argument("--auto_level_tensor", action="store_true") + ap.add_argument("--run_model", action="store_true", help="Roda inferência em thread separada usando o último tensor cacheado.") + ap.add_argument("--model_config_json", default=None, help="JSON de configuração do modelo SegFormer, igual ao benchmark.") + ap.add_argument("--warmup_model", type=int, default=3, help="Número de inferências dummy para aquecer o modelo.") + ap.add_argument("--save_last", default=None) + args = ap.parse_args() + + client = OakFcc3Client( + width=args.width, + height=args.height, + fps=args.fps, + frame_type="RAW_BRUTO", + capture_mode="TRIPLE", + raw_policy="require_triple", + module_calibration_json=args.module_params, + mx_id=args.mx_id, + ) + + model_svc = init_model_service(args) if args.run_model else None + + state = SharedState() + last_panel = None + last_warn_ts = 0.0 + last_seq_drawn = -1 + + try: + client.start(print_debug=True) + + for _ in range(max(0, int(args.warmup))): + try: + client.get_next_decoded(timeout=args.timeout) + except Exception: + pass + time.sleep(0.03) + + tw = threading.Thread(target=tensor_worker, args=(client, state, args), daemon=True) + tw.start() + + mw = None + if args.run_model: + mw = threading.Thread(target=model_worker, args=(model_svc, state, args), daemon=True) + mw.start() + + min_period = 1.0 / max(float(args.preview_fps), 0.1) + next_draw_ts = 0.0 + + print("[INFO] Preview assíncrono iniciado. Pressione Q ou ESC para sair.") + print("[INFO] Tensor FPS = geração real do tensor. Preview FPS = janela. Model FPS = inferência, se habilitada.") + + while True: + now = time.perf_counter() + if now < next_draw_ts: + time.sleep(min(0.005, next_draw_ts - now)) + key = cv2.waitKey(1) & 0xFF + if key in (27, ord('q'), ord('Q')): + break + continue + + next_draw_ts = now + min_period + snap = snapshot_state(state) + + if snap["tensor"] is None or snap["decoded"] is None: + blank = np.zeros((360, 900, 3), dtype=np.uint8) + cv2.putText(blank, "Aguardando primeiro tensor...", (30, 180), cv2.FONT_HERSHEY_SIMPLEX, 0.9, (255,255,255), 2, cv2.LINE_AA) + cv2.imshow("OAK RAW_BRUTO vs Tensor Final", blank) + key = cv2.waitKey(1) & 0xFF + if key in (27, ord('q'), ord('Q')): + break + continue + + # Desenha só usando o cache. Não captura nem monta tensor aqui. + t_draw0 = time.perf_counter() + raw_tiles = decoded_raw_tiles( + snap["decoded"], + tile_w=args.tile_w, + tile_h=args.tile_h, + auto_level=args.auto_level_raw, + ) + final_tiles = tensor_tiles( + snap["tensor"], + tile_w=args.tile_w, + tile_h=args.tile_h, + auto_level=args.auto_level_tensor, + ) + + pred_tiles = None + pred_names = None + if args.run_model: + pred_tiles, pred_names = try_extract_prediction_tiles( + snap["pred"], + target_w=args.tile_w, + target_h=args.tile_h, + ) + if snap["model_warn"]: + tnow = time.time() + if tnow - last_warn_ts > 2.0: + print(f"[WARN][MODEL] {snap['model_warn']}") + last_warn_ts = tnow + + panel = build_grid( + raw_tiles=raw_tiles, + final_tiles=final_tiles, + pred_tiles=pred_tiles, + pred_names=pred_names, + tile_w=args.tile_w, + tile_h=args.tile_h, + ) + + preview_fps = state.preview_fps.tick() + draw_ms = (time.perf_counter() - t_draw0) * 1000.0 + perf = snap["perf"] + meta = snap["meta"] or {} + tensor_seq = int(snap["tensor_seq"]) + dropped_for_preview = max(0, tensor_seq - last_seq_drawn - 1) if last_seq_drawn >= 0 else 0 + last_seq_drawn = tensor_seq + + hud = [ + f"Preview FPS: {preview_fps:.1f} | Tensor FPS: {snap['tensor_fps']:.1f} | Model FPS: {snap['model_fps']:.1f} | infer={snap.get('model_ms', 0.0):.1f}ms", + f"draw={draw_ms:.1f}ms flat={perf.get('flat_ms', 0):.1f} warp={perf.get('warp_total_ms', 0):.1f} crop={perf.get('crop_resize_ms', 0):.1f} fuse={perf.get('total_ms', 0):.1f}", + f"seq={tensor_seq} skipped_preview={dropped_for_preview} frame_type={meta.get('frame_type')} run_model={args.run_model}", + ] + if snap["error"]: + hud.append(str(snap["error"])[:120]) + + panel = add_hud(panel, hud) + last_panel = panel + + disp = panel + if args.display_scale and abs(args.display_scale - 1.0) > 1e-6: + disp = cv2.resize( + disp, + (max(1, int(disp.shape[1] * args.display_scale)), max(1, int(disp.shape[0] * args.display_scale))), + interpolation=cv2.INTER_AREA, + ) + + cv2.imshow("OAK RAW_BRUTO vs Tensor Final", disp) + key = cv2.waitKey(1) & 0xFF + if key in (27, ord('q'), ord('Q')): + break + + finally: + state.stop() + time.sleep(0.05) + + if args.save_last and last_panel is not None: + out_path = Path(args.save_last) + out_path.parent.mkdir(parents=True, exist_ok=True) + cv2.imwrite(str(out_path), last_panel) + print(f"[SAVE] {out_path}") + + try: + client.stop() + except Exception: + pass + cv2.destroyAllWindows() + + +if __name__ == "__main__": + main() diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/oak_fcc3_core/oak_fcc3_client.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/oak_fcc3_core/oak_fcc3_client.py new file mode 100644 index 000000000..b9e515dd7 --- /dev/null +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/oak_fcc3_core/oak_fcc3_client.py @@ -0,0 +1,672 @@ +import numpy as np +import cv2 +import json +import os + +from .oak_fcc3_service import OakFcc3Service +from .raw_processor_core import RawProcessorCore +from .raw_processor_preview import RawProcessorPreview +from .radiometric_controller import RadiometricController + + +class OakFcc3Client: + def __init__( + self, + width=640, + height=400, + bayer="BGGR", + fps=30, + frame_type="RAW_BRUTO", + output_dtype="uint8", + capture_mode="AUTO", + raw_policy="allow_single", + module_calibration_json=None, + sync_mode="best", + sync_tolerance_ms=25.0, + mx_id=None, + **kwargs, + ): + self.width = width + self.height = height + self.bayer = bayer + self.fps = fps + self.frame_type = frame_type + self.output_dtype = output_dtype + self.capture_mode = capture_mode + self.raw_policy = raw_policy + self.module_calibration_json = module_calibration_json + self.module_params = self._load_module_params(module_calibration_json) + self.fusion_config = self.module_params.get("fusion_config", {}) or {} + + self.mx_id = str(mx_id) if mx_id else None + + self.svc = OakFcc3Service( + timeout=10, + fps=fps, + width=width, + height=height, + frame_type=frame_type, + output_dtype=output_dtype, + capture_mode=capture_mode, + raw_policy=raw_policy, + sync_mode=sync_mode, + sync_tolerance_ms=sync_tolerance_ms, + mx_id=self.mx_id, + module_calibration_json=module_calibration_json, + **kwargs, + ) + + self.applied_camera_controls = {} + self.radiometric_controller = None + self.core = RawProcessorCore( + sensor_width=width, + sensor_height=height, + bayer_pattern=bayer, + calibration_json_path=module_calibration_json, + ) + self.preview = RawProcessorPreview( + sensor_width=width, + sensor_height=height, + bayer_pattern=bayer, + ) + + def __enter__(self): + self.start() + return self + + def __exit__(self, exc_type, exc, tb): + self.stop() + + def _load_module_params(self, path): + if not path or not os.path.isfile(path): + return {} + + with open(path, "r", encoding="utf-8") as f: + return json.load(f) + + def apply_module_camera_settings(self): + camera_settings = self.module_params.get("camera_settings", {}) or {} + + applied = {} + + for role, settings in camera_settings.items(): + if not isinstance(settings, dict): + continue + + try: + resp = self.svc.apply_camera_controls( + role=role, + controls=settings, + ) + applied[role] = resp + + except Exception as e: + applied[role] = { + "ok": False, + "error": str(e), + "requested": settings, + } + + self.applied_camera_controls = applied + return applied + + def enable_radiometric_controller(self): + self.radiometric_controller = RadiometricController( + client=self, + config_json_path=self.module_calibration_json, + ) + + self.radiometric_controller.sync_from_camera_controls(self.applied_camera_controls) + self.radiometric_controller.sync_from_actual_camera_controls() + + return self.radiometric_controller + + def update_radiometry(self, decoded, meta=None): + if self.radiometric_controller is None: + return None + + return self.radiometric_controller.update(decoded, meta) + + def get_current_camera_controls(self): + controls = {} + + for role in ("rgb", "re", "nir"): + try: + controls[role] = self.svc.get_camera_controls(role=role) + except Exception as e: + controls[role] = { + "ok": False, + "role": role, + "error": str(e), + } + + return controls + + def get_radiometric_last_result(self): + if self.radiometric_controller is None: + return None + + return self.radiometric_controller.last_result + + def start(self, print_debug=False): + self.svc.connect() + + resp = self.svc.begin( + frame_type=self.frame_type, + output_dtype=self.output_dtype, + capture_mode=self.capture_mode, + ) + + try: + self.mx_id = self.svc.manager.mx_id + except Exception: + pass + + applied = self.apply_module_camera_settings() + + if print_debug: + print("[OAK CLIENT] START:", resp) + print("[OAK CLIENT] APPLIED CAMERA SETTINGS:", applied) + + self.enable_radiometric_controller() + + return resp + + def stop(self): + try: + self.svc.stop() + finally: + self.svc.disconnect() + + def get_status(self): + return self.svc.get_status() + + def get_next_raw_frame(self, timeout=1.0): + return self.svc.capture_frame(timeout=timeout) + + def get_next_frame(self, timeout=2.0): + frame, meta, _ = self.get_next_decoded(timeout=timeout) + return frame, meta + + def get_next_decoded(self, timeout=2.0): + raw_frame, raw_meta = self.get_next_raw_frame(timeout=timeout) + + frame_type = str(raw_meta.get("frame_type", self.frame_type)).upper() + meta = dict(raw_meta) + + if frame_type == "RAW_BRUTO": + decoded = self.decode_stream_cameras(raw_frame, raw_meta) + + self.update_radiometry(decoded, raw_meta) + + frame = raw_frame + return frame, meta, decoded + + elif frame_type == "RGB": + decoded = self.decode_stream_cameras(raw_frame, raw_meta) + + self.update_radiometry(decoded, raw_meta) + + frame = self.build_rgb_tensor(decoded) + meta["output_layout"] = "CHW" + meta["channels"] = ["R", "G", "B"] + meta["shape"] = list(frame.shape) + meta["dtype"] = str(frame.dtype) + + return frame, meta, decoded + + elif frame_type == "MULTISPEC": + decoded = self.decode_oak_aligned_multispec(raw_frame, raw_meta) + + self.update_radiometry(decoded, raw_meta) + + # Versão inicial segura: + # não chama core.fuse_multispec_cameras(), porque ali teria homografia de novo. + frame = self.build_multispec_tensor_from_oak_aligned(decoded, meta=raw_meta) + + meta["output_layout"] = "CHW" + meta["channels"] = ["R", "G", "B", "RE", "NIR"] + meta["shape"] = list(frame.shape) + meta["dtype"] = str(frame.dtype) + meta["aligned_by_oak"] = True + meta["geometry_stage"] = "oak" + + return frame, meta, decoded + + elif frame_type == "PREVIEW": + decoded = self.decode_stream_cameras(raw_frame, raw_meta) + frame = raw_frame + return frame, meta, decoded + + else: + raise RuntimeError(f"frame_type não suportado: {frame_type}") + + def get_next_tensor_preview(self, timeout=2.0): + frame, meta, decoded = self.get_next_decoded(timeout=timeout) + + frame_type = str(meta.get("frame_type", self.frame_type)).upper() + + if frame_type == "RGB": + rgb_hwc = np.transpose(frame[:3], (1, 2, 0)) + preview = self._rgb01_to_bgr(rgb_hwc) + return {"rgb_tensor": preview}, meta + + if frame_type == "MULTISPEC": + rgb_hwc = np.transpose(frame[:3], (1, 2, 0)) + re01 = frame[3] + nir01 = frame[4] + + return { + "rgb_tensor": self._rgb01_to_bgr(rgb_hwc), + "re_tensor": self._gray01_to_bgr(re01), + "nir_tensor": self._gray01_to_bgr(nir01), + }, meta + + else: + return self.build_visual_preview_from_raw(frame, meta), meta + + raise RuntimeError(f"frame_type não suportado para preview: {frame_type}") + + def get_next_preview(self, timeout=2.0): + raw_frame, raw_meta = self.get_next_raw_frame(timeout=timeout) + + meta = dict(raw_meta) + + previews = self.build_visual_preview_from_raw(raw_frame, meta) + + return previews, meta + + def get_last_patch_normalization_result(self): + try: + return self.core.last_patch_normalization_result + except Exception: + return None + + def get_last_radiometric_normalization_result(self): + try: + return self.core.get_last_radiometric_normalization_result() + except Exception: + return None + + def build_infer_tensor(self, frame, meta, channels_expected, target_size=None): + return self.core.build_infer_tensor_from_stream( + frame, + meta, + channels_expected=channels_expected, + target_size=target_size, + ) + + def build_infer_tensor_from_decoded(self, decoded, meta, channels_expected, target_size=None): + tensor = self.core.fuse_multispec_cameras(decoded, meta, channels_expected) + return self.core.resize_tensor_chw(tensor, target_size=target_size) + + def decode_stream_cameras(self, frame, meta): + if str(meta.get("frame_type", self.frame_type)).upper() == "PREVIEW": + decoded = {} + + camera_info = meta.get("camera_info", {}) or {} + + for cam_id, img in frame.items(): + info = camera_info.get(cam_id, {}) or {} + role = info.get("role", cam_id) + img01 = self._frame_to_float01(cam_id, img, role) + + decoded[cam_id] = { + "name": role.upper(), + "role": role, + "image": img01, + "meta": { + "cam_id": cam_id, + "role": role, + "socket": info.get("socket"), + "sensor": info.get("sensor"), + "timestamp": (meta.get("timestamps") or {}).get(cam_id), + "shape": list(img.shape), + "dtype": str(img.dtype), + }, + } + + return decoded + + return self.core.decode_stream_cameras(frame, meta) + + def build_rgb_tensor(self, decoded): + cam_id, item = self._find_decoded_by_role(decoded, "rgb") + + rgb01 = item["image"] + + if rgb01.ndim != 3 or rgb01.shape[2] != 3: + raise RuntimeError(f"{cam_id} RGB inválida: shape={rgb01.shape}") + + tensor = np.transpose(rgb01.astype(np.float32), (2, 0, 1)) + return np.ascontiguousarray(tensor.astype(np.float32, copy=False)) + + def build_multispec_tensor(self, decoded, meta=None): + tensor = self.build_infer_tensor_from_decoded( + decoded=decoded, + meta=meta, + channels_expected=5, + ) + return np.ascontiguousarray(tensor.astype(np.float32, copy=False)) + + def build_preview_from_raw_payload(self, frame, meta): + """ + Gera preview priorizando a câmera com role='rgb'. + Retorna: + preview_bgr: imagem BGR uint8 para OpenCV + payload_float_preview: tensor CHW float32 [0..1] + preview_source_id: cam_id usado + """ + decoded = self.decode_stream_cameras(frame, meta) + + if not decoded: + raise RuntimeError("Nenhum frame decodificado disponível para preview.") + + try: + cam_id, item = self._find_decoded_by_role(decoded, "rgb") + rgb01 = item["image"] + + if rgb01.ndim != 3 or rgb01.shape[2] != 3: + raise RuntimeError(f"{cam_id} decodificada inválida para preview RGB: shape={rgb01.shape}") + + preview_bgr = self._rgb01_to_bgr(rgb01) + payload_float = np.transpose(rgb01.astype(np.float32), (2, 0, 1)) + + return preview_bgr, np.ascontiguousarray(payload_float), cam_id + + except RuntimeError: + pass + + first_id = list(decoded.keys())[0] + img01 = decoded[first_id]["image"] + + if img01.ndim == 2: + preview_bgr = self._gray01_to_bgr(img01) + payload_float = np.stack([img01, img01, img01], axis=0).astype(np.float32) + + elif img01.ndim == 3 and img01.shape[2] == 3: + preview_bgr = self._rgb01_to_bgr(img01) + payload_float = np.transpose(img01.astype(np.float32), (2, 0, 1)) + + else: + raise RuntimeError(f"Frame decodificado inválido para preview: cam={first_id}, shape={img01.shape}") + + return preview_bgr, np.ascontiguousarray(payload_float), first_id + + def build_visual_preview_from_raw(self, frame, meta): + camera_info = meta.get("camera_info", {}) or {} + previews = {} + + for cam_id, arr in frame.items(): + info = camera_info.get(cam_id, {}) or {} + role = info.get("role", cam_id) + bit_depth = int(info.get("bit_depth", 10)) + + if arr.ndim == 3 and arr.shape[2] == 1: + arr = arr[:, :, 0] + + if bit_depth == 10 and arr.ndim == 2: + raw16 = self.core.unpack_raw10_packed( + arr, + sensor_width=int(info.get("width", self.width)), + sensor_height=int(info.get("height", self.height)), + ) + + if role == "rgb": + previews[cam_id] = self.preview.raw16_to_preview_bgr( + raw16, + bit_depth=bit_depth, + apply_wb=True, + apply_contrast=True, + ) + else: + vis8 = self.preview.raw16_to_vis8( + raw16, + bit_depth=bit_depth, + gamma=2.2, + ) + previews[cam_id] = cv2.cvtColor(vis8, cv2.COLOR_GRAY2BGR) + + else: + decoded = self.decode_stream_cameras({cam_id: arr}, {"camera_info": {cam_id: info}}) + img01 = decoded[cam_id]["image"] + + if img01.ndim == 2: + previews[cam_id] = self._gray01_to_bgr(img01) + else: + previews[cam_id] = self._rgb01_to_bgr(img01) + + return previews + + def build_save_preview_from_cam_a( + self, + packed_raw_by_camera: dict | None, + meta_stream: dict, + sensor_width: int, + sensor_height: int, + bayer_pattern: str, + ) -> np.ndarray | None: + """ + Gera o preview salvo no mesmo padrão do 'CAM_A reconstruido'. + + Usa apenas CAM_A do RAW_BRUTO: + CAM_A packed RAW10 + -> unpack_raw10_packed + -> RawProcessorPreview.raw16_to_preview_bgr + + Retorna BGR uint8 pronto para cv2.imwrite. + """ + if not packed_raw_by_camera or "CAM_A" not in packed_raw_by_camera: + return None + + stream_meta = meta_stream or {} + + camera_info = stream_meta.get("camera_info", {}) or {} + cam_meta = camera_info.get("CAM_A", {}) or {} + + bit_depth = int(cam_meta.get("bit_depth", 10)) + + bayer = ( + cam_meta.get("bayer_pattern") + or cam_meta.get("bayer") + or stream_meta.get("bayer_pattern") + or bayer_pattern + or "RGGB" + ) + bayer = str(bayer).upper() + + arr = packed_raw_by_camera["CAM_A"] + + if arr is None: + return None + + packed = arr + if packed.ndim == 3 and packed.shape[2] == 1: + packed = packed[:, :, 0] + + if packed.ndim != 2: + return None + + # Mantém a mesma lógica do validador: + # RAW10 packed => sensor_w = packed_w * 4 // 5 + packed_h, packed_w = packed.shape[:2] + + if bit_depth == 10: + real_w = int(cam_meta.get("width", sensor_width)) + real_h = int(cam_meta.get("height", sensor_height)) + + # Fallback caso o meta não tenha width/height confiáveis + if real_w <= 0 or real_h <= 0: + real_w = int((packed_w * 4) // 5) + real_h = int(packed_h) + else: + real_w = int(cam_meta.get("width", sensor_width)) + real_h = int(cam_meta.get("height", sensor_height)) + + core = RawProcessorCore( + sensor_width=real_w, + sensor_height=real_h, + bayer_pattern=bayer, + ) + + preview = RawProcessorPreview( + sensor_width=real_w, + sensor_height=real_h, + bayer_pattern=bayer, + ) + + raw16 = core.unpack_raw10_packed( + packed, + sensor_width=real_w, + sensor_height=real_h, + ) + + preview_bgr = preview.raw16_to_preview_bgr( + raw16, + bit_depth=bit_depth, + ) + + return preview_bgr + + def _find_decoded_by_role(self, decoded, role): + role = str(role).lower() + + for cam_id, item in decoded.items(): + if str(item.get("role", "")).lower() == role: + return cam_id, item + + raise RuntimeError(f"Nenhuma câmera com role={role} encontrada.") + + def _frame_to_float01(self, cam_id, img, role): + if img is None: + return None + + arr = img + + if arr.dtype == np.uint8: + arr01 = arr.astype(np.float32) / 255.0 + elif arr.dtype == np.uint16: + arr01 = arr.astype(np.float32) / 65535.0 + else: + arr01 = arr.astype(np.float32) + if arr01.max() > 1.5: + arr01 = arr01 / 255.0 + + arr01 = np.clip(arr01, 0.0, 1.0) + + if role == "rgb": + # DepthAI/OpenCV entrega BGR HWC. Calibradores esperam RGB HWC. + if arr01.ndim == 3 and arr01.shape[2] == 3: + arr01 = cv2.cvtColor((arr01 * 255).astype(np.uint8), cv2.COLOR_BGR2RGB).astype(np.float32) / 255.0 + elif arr01.ndim == 2: + arr01 = np.stack([arr01, arr01, arr01], axis=2) + + return arr01 + + # Espectrais devem virar mono HW. + if arr01.ndim == 3: + arr01 = cv2.cvtColor((arr01 * 255).astype(np.uint8), cv2.COLOR_BGR2GRAY).astype(np.float32) / 255.0 + + return arr01 + + @staticmethod + def _rgb01_to_bgr(rgb01): + rgb_u8 = np.clip(rgb01 * 255.0, 0, 255).astype(np.uint8) + return cv2.cvtColor(rgb_u8, cv2.COLOR_RGB2BGR) + + @staticmethod + def _gray01_to_bgr(gray01): + g = np.clip(gray01 * 255.0, 0, 255).astype(np.uint8) + return cv2.cvtColor(g, cv2.COLOR_GRAY2BGR) + + + + def decode_oak_aligned_multispec(self, frame, meta): + """ + Decodifica frames já alinhados pela OAK. + + Entrada esperada: + frame = { + "CAM_A": BGR uint8 HWC, + "CAM_B": GRAY uint8 HW, + "CAM_C": GRAY uint8 HW, + } + + Saída: + decoded por role, em float32 0..1. + """ + if not isinstance(frame, dict): + raise RuntimeError("MULTISPEC alinhado esperado como dict de câmeras.") + + decoded = {} + camera_info = meta.get("camera_info", {}) or {} + + for cam_id, img in frame.items(): + info = camera_info.get(cam_id, {}) or {} + role = str(info.get("role", "")).lower() + + if not role: + role = str(self.svc.manager.roles.get(cam_id, cam_id)).lower() + + img01 = self._frame_to_float01(cam_id, img, role) + + decoded[cam_id] = { + "name": role.upper(), + "role": role, + "image": img01, + "meta": { + **info, + "cam_id": cam_id, + "role": role, + "aligned_by_oak": True, + "geometry_stage": "oak", + "homography_applied": role in ("re", "nir"), + "crop_resize_applied": True, + "shape": list(img.shape), + "dtype": str(img.dtype), + }, + } + + return decoded + + def build_multispec_tensor_from_oak_aligned(self, decoded, meta=None): + """ + Monta CHW [R,G,B,RE,NIR] sem reaplicar homografia. + """ + _, rgb_item = self._find_decoded_by_role(decoded, "rgb") + _, re_item = self._find_decoded_by_role(decoded, "re") + _, nir_item = self._find_decoded_by_role(decoded, "nir") + + rgb = rgb_item["image"].astype(np.float32, copy=False) + re = re_item["image"].astype(np.float32, copy=False) + nir = nir_item["image"].astype(np.float32, copy=False) + + if rgb.ndim != 3 or rgb.shape[2] != 3: + raise RuntimeError(f"RGB alinhado inválido: shape={rgb.shape}") + + h, w = rgb.shape[:2] + + if re.ndim == 3: + re = re[:, :, 0] + + if nir.ndim == 3: + nir = nir[:, :, 0] + + if re.shape[:2] != (h, w): + re = cv2.resize(re, (w, h), interpolation=cv2.INTER_LINEAR) + + if nir.shape[:2] != (h, w): + nir = cv2.resize(nir, (w, h), interpolation=cv2.INTER_LINEAR) + + tensor = np.stack( + [ + rgb[:, :, 0], # R + rgb[:, :, 1], # G + rgb[:, :, 2], # B + re, + nir, + ], + axis=0, + ).astype(np.float32, copy=False) + + return np.ascontiguousarray(tensor) diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/oak_fcc3_core/oak_fcc3_manager.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/oak_fcc3_core/oak_fcc3_manager.py new file mode 100644 index 000000000..536be8425 --- /dev/null +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/oak_fcc3_core/oak_fcc3_manager.py @@ -0,0 +1,1860 @@ +import json +import os +import time +from collections import deque +import threading +import copy + +import cv2 +import depthai as dai +import numpy as np + + +class OakFcc3Manager: + """ + Manager OAK-FFC-3 com dois fluxos principais: + + 1) RAW_BRUTO + - Mantém o comportamento antigo. + - CAM_A/CAM_B/CAM_C enviam RAW10 packed direto para o PC. + - O PC faz decode, flat/radiometric, homografia/fusão/crop/resize. + + 2) MULTISPEC + - Câmeras sempre em 800p nativo. + - OAK aplica homografia/crop/resize via ImageManip. + - PC recebe frames já alinhados: + CAM_A/rgb -> BGR uint8 + CAM_B/re -> GRAY uint8 + CAM_C/nir -> GRAY uint8 + - O Client deve montar o tensor sem reaplicar homografia. + """ + + SENSOR_W = 1280 + SENSOR_H = 800 + + def __init__( + self, + fps=30, + width=640, + height=400, + frame_type="RAW_BRUTO", + output_dtype="uint8", + capture_mode="AUTO", + raw_policy="allow_single", + roles=None, + sync_mode="best", + sync_tolerance_ms=12.0, + buffer_size=8, + only_camera=None, + mx_id=None, + module_calibration_json=None, + module_params=None, + ): + self.fps = fps + + # Para compatibilidade, mantemos width/height. + # No RAW_BRUTO isso não muda o sensor, pois usamos 800p fixo. + # No MULTISPEC isso representa a saída final alinhada da OAK. + self.width = int(width) + self.height = int(height) + self.size = (self.width, self.height) + + self.sensor_width = self.SENSOR_W + self.sensor_height = self.SENSOR_H + + self.frame_type = str(frame_type).upper() + self.output_dtype = output_dtype + self.capture_mode = capture_mode + self.raw_policy = raw_policy + self.only_camera = only_camera + + self.roles = roles or { + "CAM_A": "rgb", + "CAM_B": "re", + "CAM_C": "nir", + } + + self.sync_mode = sync_mode + self.sync_tolerance_ms = sync_tolerance_ms + self.buffer_size = buffer_size + + self.mx_id = str(mx_id) if mx_id else None + self.dev_info = None + self.device = None + self.pipeline = None + self.queues = {} + self.buffers = {} + self.camera_info = {} + + self.has_imu_pipeline = False + self.tem_imu = False + self.q_imu = None + + self.running = False + self.frame_id = 0 + self.control_queues = {} + self.camera_controls = { + cam_id: self._default_controls_for_role(role) + for cam_id, role in self.roles.items() + } + self._last_raw_dims = {} + + self.module_calibration_json = module_calibration_json + self.module_params = module_params if isinstance(module_params, dict) else self._load_module_params(module_calibration_json) + self.fusion_config = (self.module_params or {}).get("fusion_config", {}) or {} + self.aligned_geometry = None + + self.async_capture_enabled = True + self.async_capture_mode = "latest" # latest | queue + self.async_capture_max_queue = 2 + self._capture_thread = None + self._capture_stop_event = threading.Event() + self._capture_lock = threading.RLock() + self._capture_cond = threading.Condition(self._capture_lock) + self._latest_packet = None + self._latest_packet_seq = 0 + self._last_consumed_packet_seq = 0 + self._packet_queue = deque(maxlen=self.async_capture_max_queue) + self._capture_thread_stats = { + "started": False, + "packets": 0, + "dropped_latest": 0, + "dropped_queue": 0, + "last_error": None, + "last_loop_ms": 0.0, + } + + def __enter__(self): + self.start() + return self + + def __exit__(self, exc_type, exc, tb): + self.stop() + + # ============================================================ + # Modes + # ============================================================ + + def _is_preview_mode(self): + return str(self.frame_type).upper() == "PREVIEW" + + def _is_multispec_mode(self): + return str(self.frame_type).upper() == "MULTISPEC" + + def _is_raw_mode(self): + return str(self.frame_type).upper() == "RAW_BRUTO" + + # ============================================================ + # Config helpers + # ============================================================ + + def _load_module_params(self, path): + if not path or not os.path.isfile(path): + return {} + + with open(path, "r", encoding="utf-8") as f: + return json.load(f) + + def _default_controls_for_role(self, role: str): + role = str(role).lower() + + if role == "rgb": + return { + "ae_enable": False, + "awb_enable": False, + "exposure_time_us": 2000, + "analogue_gain": 1.0, + "colour_gains": [1.0, 1.0], + } + + return { + "ae_enable": False, + "awb_enable": False, + "exposure_time_us": 5000, + "analogue_gain": 1.0, + "colour_gains": None, + } + + # ============================================================ + # Device discovery + # ============================================================ + + def list_cameras(self): + dev_info = self._resolve_device_info() + + with dai.Device(dev_info) as dev: + result = [] + for f in dev.getConnectedCameraFeatures(): + result.append({ + "socket": f.socket.name, + "sensor": f.sensorName, + "role": self.roles.get(f.socket.name, "unknown"), + }) + return result + + def _device_id_from_info(self, dev_info): + for name in ("getMxId", "getDeviceId"): + try: + fn = getattr(dev_info, name, None) + if callable(fn): + value = fn() + if value: + return str(value) + except Exception: + pass + + try: + value = getattr(dev_info, "mxid", None) + if value: + return str(value) + except Exception: + pass + + try: + value = getattr(dev_info, "deviceId", None) + if value: + return str(value) + except Exception: + pass + + return None + + def _resolve_device_info(self): + devices = dai.Device.getAllAvailableDevices() + + if not devices: + raise RuntimeError("Nenhum dispositivo DepthAI/OAK encontrado.") + + if self.mx_id is None: + return devices[0] + + target = str(self.mx_id).strip() + + for dev_info in devices: + dev_id = self._device_id_from_info(dev_info) + if dev_id == target: + return dev_info + + disponiveis = [ + self._device_id_from_info(d) or str(getattr(d, "name", "unknown")) + for d in devices + ] + + raise RuntimeError( + f"Dispositivo DepthAI com ID '{target}' não encontrado. " + f"Disponíveis: {disponiveis}" + ) + + def _socket_from_name(self, socket_name): + socket_name = str(socket_name).upper() + if socket_name == "CAM_A": + return dai.CameraBoardSocket.CAM_A + if socket_name == "CAM_B": + return dai.CameraBoardSocket.CAM_B + if socket_name == "CAM_C": + return dai.CameraBoardSocket.CAM_C + if socket_name == "CAM_D": + return dai.CameraBoardSocket.CAM_D + raise ValueError(f"Socket não suportado: {socket_name}") + + # ============================================================ + # Pipeline creation, classic RAW/PREVIEW + # ============================================================ + + def _create_camera_node_classic(self, socket, sensor_name: str, role: str): + """ + Fluxo clássico. + + RGB/OV9782: + ColorCamera raw para RAW_BRUTO. + + MONO/OV9282: + MonoCamera raw quando disponível. + """ + sensor_name_u = str(sensor_name or "").upper() + role_u = str(role or "").lower() + + is_rgb = ( + role_u == "rgb" + or "OV9782" in sensor_name_u + or socket == dai.CameraBoardSocket.CAM_A + ) + + if is_rgb: + cam = self.pipeline.createColorCamera() + cam.setBoardSocket(socket) + + try: + cam.setResolution(dai.ColorCameraProperties.SensorResolution.THE_800_P) + except Exception: + try: + cam.setResolution(dai.ColorCameraProperties.SensorResolution.THE_1080_P) + except Exception: + pass + + cam.setInterleaved(False) + cam.setColorOrder(dai.ColorCameraProperties.ColorOrder.RGB) + cam.setFps(float(self.fps)) + + if self._is_preview_mode(): + try: + cam.setVideoSize(int(self.width), int(self.height)) + return cam, cam.video + except Exception: + return cam, cam.preview + + return cam, cam.raw + + mono = self.pipeline.create(dai.node.MonoCamera) + mono.setBoardSocket(socket) + + try: + mono.setResolution(dai.MonoCameraProperties.SensorResolution.THE_800_P) + except Exception: + try: + mono.setResolution(dai.MonoCameraProperties.SensorResolution.THE_720_P) + except Exception: + try: + mono.setResolution(dai.MonoCameraProperties.SensorResolution.THE_400_P) + except Exception: + pass + + mono.setFps(float(self.fps)) + + if self._is_preview_mode(): + return mono, mono.out + + if hasattr(mono, "raw"): + return mono, mono.raw + + return mono, mono.out + + def _create_imu_node(self, pipeline): + self.has_imu_pipeline = False + + try: + imu = pipeline.create(dai.node.IMU) + imu.enableIMUSensor(dai.IMUSensor.ACCELEROMETER_RAW, 100) + imu.enableIMUSensor(dai.IMUSensor.GYROSCOPE_RAW, 100) + imu.setBatchReportThreshold(1) + imu.setMaxBatchReports(20) + + xout_imu = pipeline.create(dai.node.XLinkOut) + xout_imu.setStreamName("imu") + imu.out.link(xout_imu.input) + + self.has_imu_pipeline = True + print("[OAK] Pipeline IMU criado") + + except Exception as e: + self.has_imu_pipeline = False + print(f"[WARN] IMU indisponível no pipeline: {e}") + + # ============================================================ + # Pipeline creation, MULTISPEC aligned on OAK + # ============================================================ + + def _create_color_camera_multispec(self, socket): + cam = self.pipeline.create(dai.node.ColorCamera) + cam.setBoardSocket(socket) + cam.setResolution(dai.ColorCameraProperties.SensorResolution.THE_800_P) + cam.setFps(float(self.fps)) + cam.setInterleaved(False) + + # Usamos BGR porque getCvFrame/OpenCV lida direto com BGR. + # O Client converte para RGB float no _frame_to_float01. + cam.setColorOrder(dai.ColorCameraProperties.ColorOrder.BGR) + cam.setVideoSize(int(self.sensor_width), int(self.sensor_height)) + return cam + + def _create_mono_camera_multispec(self, socket): + cam = self.pipeline.create(dai.node.MonoCamera) + cam.setBoardSocket(socket) + cam.setResolution(dai.MonoCameraProperties.SensorResolution.THE_800_P) + cam.setFps(float(self.fps)) + return cam + + def _make_xout(self, name): + if hasattr(dai.node, "XLinkOut"): + xout = self.pipeline.create(dai.node.XLinkOut) + xout.setStreamName(name) + return xout + + if hasattr(self.pipeline, "createXLinkOut"): + xout = self.pipeline.createXLinkOut() + xout.setStreamName(name) + return xout + + raise RuntimeError("Não encontrei XLinkOut nesta versão do DepthAI.") + + def _apply_four_point_transform(self, config, src_quad_px, dst_quad_px): + src_pts = [dai.Point2f(float(x), float(y)) for x, y in src_quad_px] + dst_pts = [dai.Point2f(float(x), float(y)) for x, y in dst_quad_px] + + if hasattr(config, "setWarpTransformFourPoints"): + try: + config.setWarpTransformFourPoints(src_pts, dst_pts, False) + return + except TypeError: + config.setWarpTransformFourPoints(src_pts, False) + return + + if hasattr(config, "addTransformFourPoints"): + config.addTransformFourPoints(src_pts, dst_pts, False) + return + + raise RuntimeError( + "ImageManipConfig não tem setWarpTransformFourPoints nem addTransformFourPoints" + ) + + def _create_warp_manip( + self, + name, + out_w, + out_h, + src_quad_px, + frame_type=None, + max_output_frame_size=None, + ): + manip = self.pipeline.create(dai.node.ImageManip) + + dst_quad_px = [ + (0.0, 0.0), + (float(out_w - 1), 0.0), + (float(out_w - 1), float(out_h - 1)), + (0.0, float(out_h - 1)), + ] + + self._apply_four_point_transform( + manip.initialConfig, + src_quad_px, + dst_quad_px, + ) + + if hasattr(manip.initialConfig, "setResize"): + try: + manip.initialConfig.setResize(int(out_w), int(out_h)) + except Exception: + pass + + if frame_type is not None: + try: + manip.initialConfig.setFrameType(frame_type) + except Exception: + pass + + if max_output_frame_size is None: + max_output_frame_size = int(out_w * out_h * 3) + + manip.setMaxOutputFrameSize(int(max_output_frame_size)) + + xout = self._make_xout(name) + manip.out.link(xout.input) + + return manip, xout + + def _start_multispec_pipeline(self, features): + """ + Cria pipeline onde a OAK entrega frames já alinhados. + Stream names continuam CAM_A/CAM_B/CAM_C para preservar o contrato. + """ + self.aligned_geometry = self._prepare_multispec_geometry() + + feature_by_socket = {f.socket.name: f for f in features} + + required = ["CAM_A", "CAM_B", "CAM_C"] + missing = [cam_id for cam_id in required if cam_id not in feature_by_socket] + if missing: + raise RuntimeError( + f"MULTISPEC exige CAM_A/CAM_B/CAM_C ativos. Ausentes: {missing}" + ) + + for cam_id in required: + if self.only_camera is not None and cam_id != self.only_camera: + continue + + f = feature_by_socket[cam_id] + role = str(self.roles.get(cam_id, "unknown")).lower() + socket = f.socket + + print( + f"[OAK] Criando câmera MULTISPEC {cam_id} " + f"sensor={f.sensorName} role={role}" + ) + + if role == "rgb": + cam = self._create_color_camera_multispec(socket) + src_output = cam.video + quad = self.aligned_geometry["quad_rgb"] + out_type = dai.ImgFrame.Type.BGR888p + max_size = self.width * self.height * 3 + channels = 3 + bit_depth = 8 + raw_format = "BGR888p" + elif role == "re": + cam = self._create_mono_camera_multispec(socket) + src_output = cam.out + quad = self.aligned_geometry["quad_re"] + out_type = dai.ImgFrame.Type.GRAY8 + max_size = self.width * self.height + channels = 1 + bit_depth = 8 + raw_format = "GRAY8" + elif role == "nir": + cam = self._create_mono_camera_multispec(socket) + src_output = cam.out + quad = self.aligned_geometry["quad_nir"] + out_type = dai.ImgFrame.Type.GRAY8 + max_size = self.width * self.height + channels = 1 + bit_depth = 8 + raw_format = "GRAY8" + else: + raise RuntimeError(f"Role não suportada no MULTISPEC: cam_id={cam_id}, role={role}") + + self.apply_initial_camera_controls_to_node(cam, cam_id) + + xin_ctrl = self.pipeline.create(dai.node.XLinkIn) + xin_ctrl.setStreamName(f"{cam_id}_ctrl") + xin_ctrl.out.link(cam.inputControl) + + manip, _ = self._create_warp_manip( + name=cam_id, + out_w=self.width, + out_h=self.height, + src_quad_px=quad, + frame_type=out_type, + max_output_frame_size=max_size, + ) + src_output.link(manip.inputImage) + + self.queues[cam_id] = None + self.buffers[cam_id] = deque(maxlen=self.buffer_size) + self.control_queues[cam_id] = None + + self.camera_info[cam_id] = { + "id": cam_id, + "socket": cam_id, + "sensor": f.sensorName, + "role": role, + "interface": "OAK_ALIGNED", + "raw_format": raw_format, + "channels": channels, + "bit_depth": bit_depth, + "width": int(self.width), + "height": int(self.height), + "aligned_by_oak": True, + "homography_applied": role in ("re", "nir"), + "crop_resize_applied": True, + } + + # ============================================================ + # Homography helpers for MULTISPEC + # ============================================================ + + def _prepare_multispec_geometry(self): + fusion = self.fusion_config or {} + + if str(fusion.get("alignment_mode", "homography")).lower() != "homography": + raise RuntimeError( + "frame_type=MULTISPEC na OAK exige fusion_config.alignment_mode='homography'." + ) + + homographies = fusion.get("homographies", {}) or {} + H_re_raw = homographies.get("re_to_rgb") + H_nir_raw = homographies.get("nir_to_rgb") + + if H_re_raw is None or H_nir_raw is None: + raise RuntimeError( + "module_params precisa conter fusion_config.homographies.re_to_rgb e nir_to_rgb " + "para frame_type=MULTISPEC." + ) + + calib_size = fusion.get("homography_calibration_size", None) + runtime_size = (self.sensor_width, self.sensor_height) + + H_re = self._scale_homography_to_runtime(H_re_raw, calib_size, runtime_size) + H_nir = self._scale_homography_to_runtime(H_nir_raw, calib_size, runtime_size) + + crop_box = self._compute_common_crop_box( + self.sensor_width, + self.sensor_height, + H_re, + H_nir, + ) + + quad_rgb = self._identity_quad_for_crop(crop_box) + quad_re = self._quad_for_output_crop_to_input(H_re, crop_box) + quad_nir = self._quad_for_output_crop_to_input(H_nir, crop_box) + + quad_rgb = self._clamp_quad(quad_rgb, self.sensor_width, self.sensor_height) + quad_re = self._clamp_quad(quad_re, self.sensor_width, self.sensor_height) + quad_nir = self._clamp_quad(quad_nir, self.sensor_width, self.sensor_height) + + print("============================================") + print("[OAK MULTISPEC] Geometria alinhada na OAK") + print(f"sensor : {self.sensor_width}x{self.sensor_height}") + print(f"output : {self.width}x{self.height}") + print(f"crop_box RGB : {crop_box}") + print(f"quad_rgb : {quad_rgb}") + print(f"quad_re : {quad_re}") + print(f"quad_nir : {quad_nir}") + print("============================================") + + return { + "reference": "rgb", + "mode": "homography", + "sensor_size": [int(self.sensor_width), int(self.sensor_height)], + "output_size": [int(self.width), int(self.height)], + "crop_box": [int(v) for v in crop_box], + "H_re": H_re, + "H_nir": H_nir, + "quad_rgb": quad_rgb, + "quad_re": quad_re, + "quad_nir": quad_nir, + } + + def _scale_homography_to_runtime(self, H, calib_size, runtime_size): + H = np.asarray(H, dtype=np.float32) + + if calib_size is None: + if abs(H[2, 2]) > 1e-9: + H = H / H[2, 2] + return H.astype(np.float32) + + calib_w, calib_h = calib_size + runtime_w, runtime_h = runtime_size + + calib_w = float(calib_w) + calib_h = float(calib_h) + runtime_w = float(runtime_w) + runtime_h = float(runtime_h) + + if calib_w <= 0 or calib_h <= 0: + return H.astype(np.float32) + + sx = runtime_w / calib_w + sy = runtime_h / calib_h + + S = np.array( + [ + [sx, 0.0, 0.0], + [0.0, sy, 0.0], + [0.0, 0.0, 1.0], + ], + dtype=np.float32, + ) + + S_inv = np.array( + [ + [1.0 / sx, 0.0, 0.0], + [0.0, 1.0 / sy, 0.0], + [0.0, 0.0, 1.0], + ], + dtype=np.float32, + ) + + H_runtime = S @ H @ S_inv + + if abs(H_runtime[2, 2]) > 1e-9: + H_runtime = H_runtime / H_runtime[2, 2] + + return H_runtime.astype(np.float32) + + def _warp_mask(self, mask, H, out_w, out_h): + return cv2.warpPerspective( + mask, + H, + (out_w, out_h), + flags=cv2.INTER_NEAREST, + borderMode=cv2.BORDER_CONSTANT, + borderValue=0, + ) + + def _compute_common_crop_box(self, runtime_w, runtime_h, H_re, H_nir): + base = np.ones((runtime_h, runtime_w), dtype=np.uint8) * 255 + + rgb_mask = base + re_mask = self._warp_mask(base, H_re, runtime_w, runtime_h) + nir_mask = self._warp_mask(base, H_nir, runtime_w, runtime_h) + + common = (rgb_mask > 0) & (re_mask > 0) & (nir_mask > 0) + + ys, xs = np.where(common) + if xs.size == 0 or ys.size == 0: + raise RuntimeError("Área comum vazia. Verifique as homografias.") + + x0 = int(xs.min()) + x1 = int(xs.max()) + 1 + y0 = int(ys.min()) + y1 = int(ys.max()) + 1 + + return x0, y0, x1, y1 + + def _apply_H_to_point(self, H, x, y): + p = np.array([float(x), float(y), 1.0], dtype=np.float32) + q = H @ p + if abs(q[2]) < 1e-9: + return float(q[0]), float(q[1]) + return float(q[0] / q[2]), float(q[1] / q[2]) + + def _quad_for_output_crop_to_input(self, H_src_to_rgb, crop_box): + x0, y0, x1, y1 = crop_box + + dst_corners_rgb = [ + (x0, y0), + (x1, y0), + (x1, y1), + (x0, y1), + ] + + H_inv = np.linalg.inv(H_src_to_rgb).astype(np.float32) + + src_quad = [] + for x, y in dst_corners_rgb: + sx, sy = self._apply_H_to_point(H_inv, x, y) + src_quad.append((sx, sy)) + + return src_quad + + def _identity_quad_for_crop(self, crop_box): + x0, y0, x1, y1 = crop_box + return [ + (float(x0), float(y0)), + (float(x1), float(y0)), + (float(x1), float(y1)), + (float(x0), float(y1)), + ] + + def _clamp_quad(self, quad, w, h): + out = [] + for x, y in quad: + x = max(0.0, min(float(w - 1), float(x))) + y = max(0.0, min(float(h - 1), float(y))) + out.append((x, y)) + return out + + # ============================================================ + # Start / stop + # ============================================================ + + def start(self): + if self.running: + return + + self.dev_info = self._resolve_device_info() + self.mx_id = self._device_id_from_info(self.dev_info) or self.mx_id + + self.device = dai.Device(self.dev_info) + self.pipeline = dai.Pipeline() + + features = self.device.getConnectedCameraFeatures() + + self.queues.clear() + self.buffers.clear() + self.camera_info.clear() + self.control_queues.clear() + self._last_raw_dims.clear() + self.aligned_geometry = None + + if self._is_multispec_mode(): + self._start_multispec_pipeline(features) + else: + for f in features: + socket = f.socket + socket_name = socket.name + if self.only_camera is not None and socket_name != self.only_camera: + continue + + role = self.roles.get(socket_name, "unknown") + + print(f"[OAK] Criando câmera {socket_name} sensor={f.sensorName} role={role}") + + cam, output = self._create_camera_node_classic( + socket=socket, + sensor_name=f.sensorName, + role=role, + ) + + self.apply_initial_camera_controls_to_node(cam, socket_name) + + xin_ctrl = self.pipeline.create(dai.node.XLinkIn) + xin_ctrl.setStreamName(f"{socket_name}_ctrl") + xin_ctrl.out.link(cam.inputControl) + + xout = self.pipeline.create(dai.node.XLinkOut) + xout.setStreamName(socket_name) + output.link(xout.input) + + cam_id = socket_name + + self.queues[cam_id] = None + self.buffers[cam_id] = deque(maxlen=self.buffer_size) + self.control_queues[cam_id] = None + + self.camera_info[cam_id] = { + "id": cam_id, + "socket": socket_name, + "sensor": f.sensorName, + "role": role, + } + + self._validate_capture_mode() + + self._create_imu_node(self.pipeline) + + self.device.startPipeline(self.pipeline) + + self.q_imu = None + self.tem_imu = False + + if self.has_imu_pipeline: + try: + self.q_imu = self.device.getOutputQueue( + name="imu", + maxSize=50, + blocking=False, + ) + self.tem_imu = True + print("[OAK] Fila IMU criada") + except Exception as e: + self.q_imu = None + self.tem_imu = False + print(f"[WARN] Fila IMU indisponível: {e}") + + for cam_id in self.camera_info.keys(): + self.queues[cam_id] = self.device.getOutputQueue( + name=cam_id, + maxSize=self.buffer_size, + blocking=False, + ) + + self.control_queues[cam_id] = self.device.getInputQueue( + name=f"{cam_id}_ctrl", + maxSize=4, + blocking=False, + ) + + self.running = True + + if not hasattr(self, "async_capture_enabled"): + self.async_capture_enabled = True + if not hasattr(self, "async_capture_mode"): + self.async_capture_mode = "latest" + if not hasattr(self, "async_capture_max_queue"): + self.async_capture_max_queue = 2 + self._reset_async_capture_state() + self._start_async_capture_thread() + time.sleep(0.05) + + def stop(self): + if not self.running: + return + + self._stop_async_capture_thread() + + try: + if self.pipeline is not None and hasattr(self.pipeline, "stop"): + self.pipeline.stop() + except Exception: + pass + + try: + if self.device is not None: + self.device.close() + except Exception: + pass + + self.pipeline = None + self.device = None + self.queues.clear() + self.buffers.clear() + self.camera_info.clear() + self.control_queues.clear() + self._last_raw_dims.clear() + self.aligned_geometry = None + self.q_imu = None + self.tem_imu = False + self.has_imu_pipeline = False + self.running = False + + def _is_fatal_depthai_error(self, erro): + txt = str(erro) + + sinais = [ + "X_LINK_ERROR", + "Communication exception", + "Couldn't read data from stream", + "Device already closed", + "device has been closed", + ] + + return any(s in txt for s in sinais) + + # ============================================================ + # Status + # ============================================================ + + def get_status(self): + return { + "mx_id": self.mx_id, + "backend": "oak_fcc3", + "running": self.running, + "fps": self.fps, + "width": self.width, + "height": self.height, + "sensor_width": self.sensor_width, + "sensor_height": self.sensor_height, + "frame_type": self.frame_type, + "output_dtype": self.output_dtype, + "capture_mode": self.capture_mode, + "raw_policy": self.raw_policy, + "sync_tolerance_ms": self.sync_tolerance_ms, + "buffer_size": self.buffer_size, + "geometry_stage": "oak" if self._is_multispec_mode() else "pc", + "aligned_geometry": self._serializable_aligned_geometry(), + "cameras": list(self.camera_info.values()), + "async_capture": self.get_async_capture_status() if hasattr(self, "get_async_capture_status") else None, + "tem_imu": bool(getattr(self, "tem_imu", False)), + "has_imu_pipeline": bool(getattr(self, "has_imu_pipeline", False)), + } + + def _serializable_aligned_geometry(self): + if not isinstance(self.aligned_geometry, dict): + return None + + out = {} + for k, v in self.aligned_geometry.items(): + if isinstance(v, np.ndarray): + out[k] = v.tolist() + else: + out[k] = v + return out + + # ============================================================ + # Frame capture + # ============================================================ + + def get_next_frame(self, timeout=1.0): + if not self.running: + last_error = None + try: + last_error = self._capture_thread_stats.get("last_error") + except Exception: + pass + + raise RuntimeError( + f"OakFcc3Manager não está rodando. Último erro: {last_error}" + ) + + # Fallback síncrono se desligar async. + if not bool(getattr(self, "async_capture_enabled", True)): + return self._get_next_frame_sync_instrumented(timeout=timeout) + + t0 = time.perf_counter() + deadline = t0 + float(timeout) + + with self._capture_cond: + while time.perf_counter() < deadline: + packet = None + + mode = str(getattr(self, "async_capture_mode", "latest")).lower() + + if mode == "queue": + if len(self._packet_queue) > 0: + packet = self._packet_queue.popleft() + else: + latest = self._latest_packet + if latest is not None and int(latest.get("seq", 0)) > int(self._last_consumed_packet_seq): + packet = latest + + if packet is not None: + seq = int(packet.get("seq", 0)) + self._last_consumed_packet_seq = seq + + frames = packet["frames"] + meta = dict(packet["meta"]) + + age_ms = (time.perf_counter() - float(packet.get("created_perf_counter", time.perf_counter()))) * 1000.0 + get_wait_ms = (time.perf_counter() - t0) * 1000.0 + + cp = dict(meta.get("capture_perf", {}) or {}) + cp["async_consumer"] = True + cp["async_packet_seq"] = seq + cp["async_packet_age_ms"] = float(age_ms) + cp["async_get_wait_ms"] = float(get_wait_ms) + cp["async_status"] = self.get_async_capture_status() + meta["capture_perf"] = cp + + return frames, meta + + remaining = deadline - time.perf_counter() + if remaining <= 0: + break + + self._capture_cond.wait(timeout=min(0.005, remaining)) + + raise TimeoutError( + f"Timeout aguardando pacote assíncrono do OAK-FFC-3. " + f"status={self.get_async_capture_status()}" + ) + + def _get_next_frame_sync_instrumented(self, timeout=1.0): + if not self.running: + raise RuntimeError("OakFcc3Manager não está rodando. Chame start() primeiro.") + + perf = self._new_capture_perf() if hasattr(self, "_new_capture_perf") else None + t_start_wall = time.time() + t_start = time.perf_counter() + + while time.time() - t_start_wall < timeout: + if perf is not None: + perf["loop_count"] += 1 + perf["buffer_lengths_before_sync"] = self._buffer_lengths_snapshot() + + t0 = time.perf_counter() + self._drain_queues_to_buffers(perf=perf) + if perf is not None: + perf["drain_total_ms"] += (time.perf_counter() - t0) * 1000.0 + perf["buffer_lengths_after_drain"] = self._buffer_lengths_snapshot() + + t0 = time.perf_counter() + synced = self._try_get_synced_packet(perf=perf) + if perf is not None: + perf["sync_select_ms"] += (time.perf_counter() - t0) * 1000.0 + + if synced is not None: + frames, timestamps, sync_dt_ms, sync_ok, frame_controls = synced + self.frame_id += 1 + + t0 = time.perf_counter() + meta = self._build_meta(frames, timestamps, sync_dt_ms, sync_ok, frame_controls=frame_controls) + if perf is not None: + perf["meta_ms"] += (time.perf_counter() - t0) * 1000.0 + perf["wait_total_ms"] = (time.perf_counter() - t_start) * 1000.0 + perf["sync_dt_ms"] = float(sync_dt_ms) + perf["sync_ok"] = bool(sync_ok) + perf["buffer_lengths_after_sync"] = self._buffer_lengths_snapshot() + perf["wait_reason"] = "synced_packet_ready_sync" + meta["capture_perf"] = perf + + return frames, meta + + t0 = time.perf_counter() + time.sleep(0.001) + if perf is not None: + perf["sleep_ms"] += (time.perf_counter() - t0) * 1000.0 + perf["sleep_count"] += 1 + + raise TimeoutError( + f"Timeout aguardando pacote sincronizado do OAK-FFC-3. " + f"Tolerância atual={self.sync_tolerance_ms} ms." + ) + + + def _extract_frame_controls(self, msg): + controls = { + "exposure_time_us": None, + "sensitivity_iso": None, + "analogue_gain_est": None, + "color_temperature_k": None, + "lens_position": None, + "sequence_num": None, + "errors": [], + } + + try: + if hasattr(msg, "getSequenceNum"): + controls["sequence_num"] = int(msg.getSequenceNum()) + except Exception as e: + controls["errors"].append(f"sequence_num:{type(e).__name__}:{e}") + + try: + if hasattr(msg, "getExposureTime"): + exp = msg.getExposureTime() + + if hasattr(exp, "total_seconds"): + controls["exposure_time_us"] = int(exp.total_seconds() * 1_000_000) + else: + controls["exposure_time_us"] = int(exp) + else: + controls["errors"].append("missing:getExposureTime") + except Exception as e: + controls["errors"].append(f"exposure:{type(e).__name__}:{e}") + + try: + if hasattr(msg, "getSensitivity"): + iso = msg.getSensitivity() + controls["sensitivity_iso"] = int(iso) + controls["analogue_gain_est"] = float(iso) / 100.0 + else: + controls["errors"].append("missing:getSensitivity") + except Exception as e: + controls["errors"].append(f"sensitivity:{type(e).__name__}:{e}") + + try: + if hasattr(msg, "getColorTemperature"): + ct = int(msg.getColorTemperature()) + controls["color_temperature_k"] = ct if ct > 0 else None + else: + controls["errors"].append("missing:getColorTemperature") + except Exception as e: + controls["errors"].append(f"color_temperature:{type(e).__name__}:{e}") + + try: + if hasattr(msg, "getLensPosition"): + lp = int(msg.getLensPosition()) + controls["lens_position"] = lp + else: + controls["errors"].append("missing:getLensPosition") + except Exception as e: + controls["errors"].append(f"lens_position:{type(e).__name__}:{e}") + + if not controls["errors"]: + controls.pop("errors", None) + + return controls + + def _drain_queues_to_buffers(self, perf=None): + for cam_id, q in self.queues.items(): + if perf is not None: + perf["drained_by_cam"].setdefault(cam_id, 0) + perf["queue_has_true_by_cam"].setdefault(cam_id, 0) + + while True: + t0_has = self._cap_now_ms() + has_msg = q.has() + if perf is not None: + perf["drain_has_ms"] += self._cap_now_ms() - t0_has + + if not has_msg: + break + + if perf is not None: + perf["queue_has_true_by_cam"][cam_id] += 1 + + t0_get = self._cap_now_ms() + msg = q.get() + if perf is not None: + perf["drain_get_msg_ms"] += self._cap_now_ms() - t0_get + + t0_ts = self._cap_now_ms() + try: + ts = msg.getTimestamp().total_seconds() + except Exception: + ts = time.time() + if perf is not None: + perf["drain_get_timestamp_ms"] += self._cap_now_ms() - t0_ts + + if self._is_preview_mode() or self._is_multispec_mode(): + t0_data = self._cap_now_ms() + frame = msg.getCvFrame() + if perf is not None: + perf["drain_get_data_ms"] += self._cap_now_ms() - t0_data + + if frame is None: + continue + + if self._is_multispec_mode(): + self._last_raw_dims[cam_id] = { + "sensor_width": int(frame.shape[1]), + "sensor_height": int(frame.shape[0]), + "stride": int(frame.strides[0]) if hasattr(frame, "strides") else int(frame.shape[1]), + "packed_width": int(frame.shape[1]), + } + + else: + t0_data = self._cap_now_ms() + data = msg.getData() + if perf is not None: + perf["drain_get_data_ms"] += self._cap_now_ms() - t0_data + + t0_copy = self._cap_now_ms() + raw = np.frombuffer(data, dtype=np.uint8).copy() + if perf is not None: + perf["drain_frombuffer_copy_ms"] += self._cap_now_ms() - t0_copy + + t0_shape = self._cap_now_ms() + h = int(msg.getHeight()) + w = int(msg.getWidth()) + stride = self._get_imgframe_stride(msg, raw.size, h, w) + + expected = h * stride + + if raw.size < expected: + raise RuntimeError( + f"RAW menor que esperado: raw.size={raw.size}, esperado={expected}, " + f"w={w}, h={h}, stride={stride}" + ) + + frame = raw[:expected].reshape((h, stride)) + if perf is not None: + perf["drain_reshape_ms"] += self._cap_now_ms() - t0_shape + + self._last_raw_dims[cam_id] = { + "sensor_width": w, + "sensor_height": h, + "stride": stride, + "packed_width": stride, + } + + t0_ctrl = self._cap_now_ms() + frame_controls = self._extract_frame_controls(msg) + if perf is not None: + perf["drain_controls_ms"] += self._cap_now_ms() - t0_ctrl + + self.buffers[cam_id].append({ + "frame": frame, + "timestamp": ts, + "controls": frame_controls, + }) + + if perf is not None: + perf["drained_total"] += 1 + perf["drained_by_cam"][cam_id] += 1 + + def _get_imgframe_stride(self, msg, raw_size: int, h: int, w: int) -> int: + try: + return int(msg.getStride()) + except Exception: + pass + + try: + if h > 0 and raw_size % h == 0: + return int(raw_size // h) + except Exception: + pass + + return int(np.ceil(w * 5.0 / 4.0)) + + def _try_get_synced_packet(self, perf=None): + required_cam_ids = self._get_required_cam_ids() + + for cam_id in required_cam_ids: + if cam_id not in self.buffers or len(self.buffers[cam_id]) == 0: + if perf is not None: + perf["wait_reason"] = f"empty_buffer:{cam_id}" + return None + + ref_cam_id = min(required_cam_ids, key=lambda cid: len(self.buffers[cid])) + ref_item = self.buffers[ref_cam_id][0] + ref_ts = ref_item["timestamp"] + + selected = {} + + for cam_id in required_cam_ids: + best_item = None + best_dt = None + + for item in self.buffers[cam_id]: + dt = abs(item["timestamp"] - ref_ts) + if best_dt is None or dt < best_dt: + best_dt = dt + best_item = item + + if best_item is None: + if perf is not None: + perf["wait_reason"] = f"no_best_item:{cam_id}" + return None + + selected[cam_id] = best_item + + timestamps = { + cam_id: item["timestamp"] + for cam_id, item in selected.items() + } + + frame_controls = { + cam_id: item.get("controls", {}) + for cam_id, item in selected.items() + } + + if perf is not None: + perf["selected_ts_by_cam"] = {cam_id: float(ts) for cam_id, ts in timestamps.items()} + perf["selected_seq_by_cam"] = { + cam_id: item.get("controls", {}).get("sequence_num") + for cam_id, item in selected.items() + } + + ts_values = list(timestamps.values()) + sync_dt_ms = (max(ts_values) - min(ts_values)) * 1000.0 if len(ts_values) >= 2 else 0.0 + sync_ok = sync_dt_ms <= self.sync_tolerance_ms + + if not sync_ok and self.sync_mode == "strict": + oldest_cam_id = min(timestamps, key=timestamps.get) + if len(self.buffers[oldest_cam_id]) > 0: + self.buffers[oldest_cam_id].popleft() + if perf is not None: + perf["wait_reason"] = f"strict_drop_oldest:{oldest_cam_id}" + perf["sync_dt_ms"] = float(sync_dt_ms) + perf["sync_ok"] = False + return None + + frames = { + cam_id: item["frame"] + for cam_id, item in selected.items() + } + + for cam_id, used_item in selected.items(): + while len(self.buffers[cam_id]) > 0: + item = self.buffers[cam_id].popleft() + if item is used_item: + break + + if perf is not None: + perf["wait_reason"] = "synced_selected" + perf["sync_dt_ms"] = float(sync_dt_ms) + perf["sync_ok"] = bool(sync_ok) + + return frames, timestamps, sync_dt_ms, sync_ok, frame_controls + + def _get_available_cam_ids_ordered(self): + role_order = ["rgb", "re", "nir"] + available = list(self.queues.keys()) + + def sort_key(cam_id): + role = str(self.roles.get(cam_id, "unknown")).lower() + try: + return role_order.index(role) + except ValueError: + return 99 + + return sorted(available, key=sort_key) + + def _get_required_cam_ids(self): + available = self._get_available_cam_ids_ordered() + + if self._is_multispec_mode(): + # MULTISPEC precisa sempre do trio completo para montar [R,G,B,RE,NIR]. + return available[:3] + + if self.capture_mode == "SINGLE": + return available[:1] + + if self.capture_mode == "DOUBLE": + return available[:2] + + if self.capture_mode == "TRIPLE": + return available[:3] + + if self.capture_mode == "AUTO": + if self.raw_policy == "require_triple": + return available[:3] + return available + + return available + + # ============================================================ + # Meta + # ============================================================ + + def _build_meta(self, frames, timestamps, sync_dt_ms, sync_ok, frame_controls): + payload_sources = list(frames.keys()) + + shapes = { + cam_id: list(arr.shape) + for cam_id, arr in frames.items() + } + + dtypes = { + cam_id: str(arr.dtype) + for cam_id, arr in frames.items() + } + + camera_info = {} + for cam_id, info in self.camera_info.items(): + item = dict(info) + arr = frames.get(cam_id) + + if arr is not None: + item["shape"] = list(arr.shape) + item["dtype"] = str(arr.dtype) + + if self._is_multispec_mode(): + role = str(item.get("role", "")).lower() + item["interface"] = "OAK_ALIGNED" + item["raw_format"] = "BGR888p" if role == "rgb" else "GRAY8" + item["channels"] = 3 if arr.ndim == 3 else 1 + item["bit_depth"] = 8 if arr.dtype == np.uint8 else 16 + item["height"] = int(arr.shape[0]) + item["width"] = int(arr.shape[1]) + item["packed"] = False + item["aligned_by_oak"] = True + item["homography_applied"] = role in ("re", "nir") + item["crop_applied"] = True + item["resize_applied"] = True + item["color_order"] = "BGR" if role == "rgb" else None + + elif self._is_preview_mode(): + item["interface"] = "OAK" + item["channels"] = 3 if arr.ndim == 3 else 1 + item["bit_depth"] = 8 if arr.dtype == np.uint8 else 16 + item["height"] = int(arr.shape[0]) + item["width"] = int(arr.shape[1]) + + else: + raw_dims = self._last_raw_dims.get(cam_id, {}) + item["interface"] = "OAK_RAW" + item["raw_format"] = "RAW10_PACKED" + item["channels"] = 1 + item["bit_depth"] = 10 + item["height"] = int(raw_dims.get("sensor_height", arr.shape[0])) + item["width"] = int(raw_dims.get("sensor_width", arr.shape[1])) + item["stride"] = int(raw_dims.get("stride", arr.shape[1])) + item["packed_width"] = int(raw_dims.get("packed_width", arr.shape[1])) + item["shape"] = list(arr.shape) + item["dtype"] = str(arr.dtype) + item["packed"] = True + + camera_info[cam_id] = item + + meta = { + "frame_id": self.frame_id, + "backend": "oak_fcc3", + "frame_type": self.frame_type, + "capture_mode": self.capture_mode, + "output_dtype": self.output_dtype, + "dtype": self.output_dtype, + "payload_sources": payload_sources, + "camera_info": camera_info, + "timestamps": timestamps, + "frame_controls": frame_controls or {}, + "sync_dt_ms": sync_dt_ms, + "sync_ok": sync_ok, + "sync_tolerance_ms": self.sync_tolerance_ms, + "shapes": shapes, + "dtypes": dtypes, + "codec_name": "none", + "codec_family": "none", + "dt_comp": 0.0, + "dt_send_payload_prev": 0.0, + } + + if self._is_multispec_mode(): + meta.update({ + "output_layout": "dict_by_camera_aligned", + "geometry_stage": "oak", + "aligned_by_oak": True, + "fusion_alignment": self._serializable_aligned_geometry(), + }) + else: + meta.update({ + "output_layout": "dict_by_camera", + "geometry_stage": "pc", + "aligned_by_oak": False, + }) + + return meta + + # ============================================================ + # Validation + # ============================================================ + + def _validate_capture_mode(self): + n = len(self.queues) + + if self._is_multispec_mode() and n < 3: + raise RuntimeError(f"frame_type=MULTISPEC exige 3 câmeras, mas detectou {n}.") + + if self.capture_mode == "TRIPLE" and n < 3: + raise RuntimeError(f"CaptureMode TRIPLE exige 3 câmeras, mas detectou {n}.") + + if self.capture_mode == "DOUBLE" and n < 2: + raise RuntimeError(f"CaptureMode DOUBLE exige 2 câmeras, mas detectou {n}.") + + if self.raw_policy == "require_triple" and n < 3: + raise RuntimeError(f"raw_policy=require_triple exige 3 câmeras, mas detectou {n}.") + + # ============================================================ + # Camera controls + # ============================================================ + + def get_camera_controls(self, cam_id): + self._validate_cam_id_known(cam_id) + return dict(self.camera_controls.get(cam_id, {})) + + def set_ae_enable(self, cam_id, enable: bool): + self._validate_cam_id_running(cam_id) + + enable = bool(enable) + ctrl_state = self.camera_controls[cam_id] + ctrl_state["ae_enable"] = enable + + ctrl = dai.CameraControl() + + if enable: + if hasattr(ctrl, "setAutoExposureEnable"): + ctrl.setAutoExposureEnable() + else: + exp_us = int(ctrl_state.get("exposure_time_us") or 15000) + gain = float(ctrl_state.get("analogue_gain") or 1.0) + ctrl.setManualExposure(exp_us, self._gain_to_iso(gain)) + + self._send_control(cam_id, ctrl) + + return dict(ctrl_state) + + def set_awb_enable(self, cam_id, enable: bool): + self._validate_cam_id_running(cam_id) + + enable = bool(enable) + ctrl_state = self.camera_controls[cam_id] + ctrl_state["awb_enable"] = enable + + ctrl = dai.CameraControl() + + if hasattr(dai.CameraControl, "AutoWhiteBalanceMode"): + if enable: + ctrl.setAutoWhiteBalanceMode(dai.CameraControl.AutoWhiteBalanceMode.AUTO) + else: + ctrl.setAutoWhiteBalanceMode(dai.CameraControl.AutoWhiteBalanceMode.OFF) + + self._send_control(cam_id, ctrl) + + return dict(ctrl_state) + + def set_exposure_time(self, cam_id, exposure_time_us: int): + self._validate_cam_id_running(cam_id) + + ctrl_state = self.camera_controls[cam_id] + exposure_time_us = int(exposure_time_us) + exposure_time_us = max(1, exposure_time_us) + + ctrl_state["exposure_time_us"] = exposure_time_us + ctrl_state["ae_enable"] = False + + gain = float(ctrl_state.get("analogue_gain") or 1.0) + + ctrl = dai.CameraControl() + ctrl.setManualExposure(exposure_time_us, self._gain_to_iso(gain)) + + self._send_control(cam_id, ctrl) + + return dict(ctrl_state) + + def set_analogue_gain(self, cam_id, analogue_gain: float): + self._validate_cam_id_running(cam_id) + + ctrl_state = self.camera_controls[cam_id] + analogue_gain = float(analogue_gain) + analogue_gain = max(1.0, analogue_gain) + + ctrl_state["analogue_gain"] = analogue_gain + ctrl_state["ae_enable"] = False + + exposure_time_us = int(ctrl_state.get("exposure_time_us") or 15000) + + ctrl = dai.CameraControl() + ctrl.setManualExposure(exposure_time_us, self._gain_to_iso(analogue_gain)) + + self._send_control(cam_id, ctrl) + + return dict(ctrl_state) + + def set_colour_gains(self, cam_id, red_gain: float, blue_gain: float): + self._validate_cam_id_running(cam_id) + + ctrl_state = self.camera_controls[cam_id] + ctrl_state["colour_gains"] = [float(red_gain), float(blue_gain)] + ctrl_state["awb_enable"] = False + + ctrl = dai.CameraControl() + + if hasattr(ctrl, "setManualWhiteBalance"): + # Placeholder seguro. Algumas versões não expõem red/blue diretamente. + pass + + self._send_control(cam_id, ctrl) + + return dict(ctrl_state) + + def apply_camera_controls(self, cam_id, controls: dict): + self._validate_cam_id_running(cam_id) + + result = dict(self.camera_controls.get(cam_id, {})) + + if "ae_enable" in controls: + result = self.set_ae_enable(cam_id, bool(controls["ae_enable"])) + + if "awb_enable" in controls: + result = self.set_awb_enable(cam_id, bool(controls["awb_enable"])) + + ae_is_on = bool(self.camera_controls[cam_id].get("ae_enable", False)) + + if not ae_is_on: + if "exposure_time_us" in controls and controls["exposure_time_us"] is not None: + result = self.set_exposure_time(cam_id, int(controls["exposure_time_us"])) + + if "analogue_gain" in controls and controls["analogue_gain"] is not None: + result = self.set_analogue_gain(cam_id, float(controls["analogue_gain"])) + + else: + if "exposure_time_us" in controls and controls["exposure_time_us"] is not None: + self.camera_controls[cam_id]["exposure_time_us"] = int(controls["exposure_time_us"]) + + if "analogue_gain" in controls and controls["analogue_gain"] is not None: + self.camera_controls[cam_id]["analogue_gain"] = float(controls["analogue_gain"]) + + result = dict(self.camera_controls[cam_id]) + + return result + + def apply_initial_camera_controls_to_node(self, cam, cam_id): + ctrl_state = self.camera_controls.get(cam_id, {}) + + ae = bool(ctrl_state.get("ae_enable", False)) + exp_us = int(ctrl_state.get("exposure_time_us") or 15000) + gain = float(ctrl_state.get("analogue_gain") or 1.0) + + if not ae: + cam.initialControl.setManualExposure( + exp_us, + self._gain_to_iso(gain), + ) + + def _send_control(self, cam_id, ctrl): + if cam_id not in self.control_queues: + raise RuntimeError(f"Fila de controle não existe para {cam_id}") + + self.control_queues[cam_id].send(ctrl) + + def _validate_cam_id_known(self, cam_id): + if cam_id not in self.camera_controls: + raise ValueError(f"cam_id inválido: {cam_id}") + + def _validate_cam_id_running(self, cam_id): + self._validate_cam_id_known(cam_id) + + if not self.running: + raise RuntimeError("Manager não está rodando.") + + if cam_id not in self.control_queues: + raise RuntimeError(f"Câmera {cam_id} não está ativa no pipeline.") + + @staticmethod + def _gain_to_iso(gain: float) -> int: + gain = max(1.0, float(gain)) + iso = int(round(gain * 100)) + return max(100, min(1600, iso)) + + + + def _cap_now_ms(self): + return time.perf_counter() * 1000.0 + + def _new_capture_perf(self): + return { + "wait_total_ms": 0.0, + "drain_total_ms": 0.0, + "drain_has_ms": 0.0, + "drain_get_msg_ms": 0.0, + "drain_get_timestamp_ms": 0.0, + "drain_get_data_ms": 0.0, + "drain_frombuffer_copy_ms": 0.0, + "drain_reshape_ms": 0.0, + "drain_controls_ms": 0.0, + "sync_select_ms": 0.0, + "meta_ms": 0.0, + "sleep_ms": 0.0, + "sleep_count": 0, + "loop_count": 0, + "drained_total": 0, + "drained_by_cam": {}, + "queue_has_true_by_cam": {}, + "buffer_lengths_before_sync": {}, + "buffer_lengths_after_drain": {}, + "buffer_lengths_after_sync": {}, + "selected_seq_by_cam": {}, + "selected_ts_by_cam": {}, + "sync_dt_ms": None, + "sync_ok": None, + "wait_reason": None, + } + + def _add_ms(self, perf, key, t0_ms): + if perf is not None: + perf[key] = float(perf.get(key, 0.0) or 0.0) + float(self._cap_now_ms() - t0_ms) + + def _buffer_lengths_snapshot(self): + return { + cam_id: int(len(buf)) + for cam_id, buf in self.buffers.items() + } + + + + def _reset_async_capture_state(self): + self._capture_stop_event = threading.Event() + self._capture_lock = threading.RLock() + self._capture_cond = threading.Condition(self._capture_lock) + self._latest_packet = None + self._latest_packet_seq = 0 + self._last_consumed_packet_seq = 0 + self._packet_queue = deque(maxlen=int(getattr(self, "async_capture_max_queue", 2))) + self._capture_thread_stats = { + "started": False, + "packets": 0, + "dropped_latest": 0, + "dropped_queue": 0, + "last_error": None, + "last_loop_ms": 0.0, + "last_packet_age_ms": None, + } + + def _start_async_capture_thread(self): + if not bool(getattr(self, "async_capture_enabled", True)): + return + + if self._capture_thread is not None and self._capture_thread.is_alive(): + return + + self._capture_stop_event.clear() + + self._capture_thread = threading.Thread( + target=self._async_capture_loop, + name="OakFcc3AsyncCapture", + daemon=True, + ) + self._capture_thread.start() + + def _stop_async_capture_thread(self): + try: + if hasattr(self, "_capture_stop_event") and self._capture_stop_event is not None: + self._capture_stop_event.set() + + if hasattr(self, "_capture_cond") and self._capture_cond is not None: + with self._capture_cond: + self._capture_cond.notify_all() + + th = getattr(self, "_capture_thread", None) + if th is not None and th.is_alive(): + th.join(timeout=1.0) + except Exception: + pass + + self._capture_thread = None + + def _async_capture_loop(self): + if not hasattr(self, "_capture_thread_stats"): + self._reset_async_capture_state() + + self._capture_thread_stats["started"] = True + + while not self._capture_stop_event.is_set(): + t_loop0 = time.perf_counter() + + try: + # Perf interno leve para diagnóstico. Não precisa imprimir todo frame. + perf = self._new_capture_perf() if hasattr(self, "_new_capture_perf") else None + + # Importante: esta thread é a única que mexe nas queues/buffers. + self._drain_queues_to_buffers(perf=perf) + synced = self._try_get_synced_packet(perf=perf) + + if synced is None: + # Dorme curto. Pode testar 0.0005 se quiser reduzir latência. + time.sleep(0.001) + continue + + frames, timestamps, sync_dt_ms, sync_ok, frame_controls = synced + + self.frame_id += 1 + meta = self._build_meta( + frames, + timestamps, + sync_dt_ms, + sync_ok, + frame_controls=frame_controls, + ) + + now = time.perf_counter() + packet = { + "seq": int(self._latest_packet_seq + 1), + "created_perf_counter": float(now), + "frames": frames, + "meta": meta, + } + + # Adiciona perf de captura assíncrona no meta. + if perf is not None: + perf["async_thread"] = True + perf["wait_total_ms"] = (time.perf_counter() - t_loop0) * 1000.0 + perf["sync_dt_ms"] = float(sync_dt_ms) + perf["sync_ok"] = bool(sync_ok) + perf["wait_reason"] = "async_packet_ready" + meta["capture_perf"] = perf + + with self._capture_cond: + self._latest_packet_seq += 1 + packet["seq"] = int(self._latest_packet_seq) + + if str(getattr(self, "async_capture_mode", "latest")).lower() == "queue": + before = len(self._packet_queue) + self._packet_queue.append(packet) + if before == self._packet_queue.maxlen: + self._capture_thread_stats["dropped_queue"] += 1 + else: + # latest mode: substitui pacote antigo se consumidor não pegou. + if self._latest_packet is not None and self._last_consumed_packet_seq < self._latest_packet.get("seq", 0): + self._capture_thread_stats["dropped_latest"] += 1 + self._latest_packet = packet + + self._capture_thread_stats["packets"] += 1 + self._capture_thread_stats["last_error"] = None + self._capture_thread_stats["last_loop_ms"] = (time.perf_counter() - t_loop0) * 1000.0 + + self._capture_cond.notify_all() + + except Exception as e: + erro = f"{type(e).__name__}: {e}" + + try: + self._capture_thread_stats["last_error"] = erro + except Exception: + pass + + if self._is_fatal_depthai_error(e): + try: + self._capture_thread_stats["fatal_error"] = True + except Exception: + pass + + self.running = False + + try: + self._capture_stop_event.set() + except Exception: + pass + + try: + with self._capture_cond: + self._capture_cond.notify_all() + except Exception: + pass + + break + + time.sleep(0.005) + + try: + self._capture_thread_stats["started"] = False + except Exception: + pass + + def get_async_capture_status(self): + with self._capture_lock: + latest_age_ms = None + if self._latest_packet is not None: + latest_age_ms = (time.perf_counter() - float(self._latest_packet.get("created_perf_counter", 0.0))) * 1000.0 + + st = dict(getattr(self, "_capture_thread_stats", {}) or {}) + st.update({ + "enabled": bool(getattr(self, "async_capture_enabled", True)), + "mode": str(getattr(self, "async_capture_mode", "latest")), + "thread_alive": bool(self._capture_thread is not None and self._capture_thread.is_alive()), + "latest_seq": int(getattr(self, "_latest_packet_seq", 0)), + "last_consumed_seq": int(getattr(self, "_last_consumed_packet_seq", 0)), + "queue_len": int(len(getattr(self, "_packet_queue", []))), + "latest_age_ms": latest_age_ms, + }) + return st diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/oak_fcc3_core/oak_fcc3_service.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/oak_fcc3_core/oak_fcc3_service.py new file mode 100644 index 000000000..1fde71780 --- /dev/null +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/oak_fcc3_core/oak_fcc3_service.py @@ -0,0 +1,230 @@ +import time +from .oak_fcc3_manager import OakFcc3Manager + + +class OakFcc3Service: + def __init__(self, timeout=10, **kwargs): + self.timeout = timeout + self.manager = OakFcc3Manager(**kwargs) + self.connected = False + + def connect(self): + self.connected = True + return {"ok": True, "backend": "oak_fcc3", "connected": True} + + def disconnect(self): + self.stop() + self.connected = False + return {"ok": True, "connected": False} + + def ping(self): + return { + "ok": True, + "backend": "oak_fcc3", + "msg": "pong", + "ts": time.time(), + } + + def get_status(self): + status = self.manager.get_status() + active_ids = [c["id"] for c in status.get("cameras", [])] + active_roles = { + c.get("role"): c.get("id") + for c in status.get("cameras", []) + } + + status.update({ + "ok": True, + "connected": self.connected, + "active_camera_ids": active_ids, + "active_roles": active_roles, + "camera_count_active": len(active_ids), + }) + + return status + + def get_config(self): + return { + "mx_id": self.manager.mx_id, + "ok": True, + "fps": self.manager.fps, + "width": self.manager.width, + "height": self.manager.height, + "frame_type": self.manager.frame_type, + "output_dtype": self.manager.output_dtype, + "capture_mode": self.manager.capture_mode, + "raw_policy": self.manager.raw_policy, + "sync_mode": getattr(self.manager, "sync_mode", "best"), + "sync_tolerance_ms": self.manager.sync_tolerance_ms, + } + + def set_fps(self, fps): + self._ensure_stopped_for_config() + self.manager.fps = int(fps) + return {"ok": True, "fps": self.manager.fps} + + def set_resolution(self, width, height): + self._ensure_stopped_for_config() + self.manager.width = int(width) + self.manager.height = int(height) + self.manager.size = (self.manager.width, self.manager.height) + return { + "ok": True, + "width": self.manager.width, + "height": self.manager.height, + } + + def set_capture_mode(self, mode): + self._ensure_stopped_for_config() + mode = str(mode).upper() + self.manager.capture_mode = self._validate_capture_mode(mode) + return {"ok": True, "capture_mode": self.manager.capture_mode} + + def set_frame_type(self, frame_type): + self._ensure_stopped_for_config() + frame_type = str(frame_type).upper() + self.manager.frame_type = self._validate_frame_type(frame_type) + return {"ok": True, "frame_type": self.manager.frame_type} + + def set_output_dtype(self, dtype): + self._ensure_stopped_for_config() + dtype = str(dtype).lower() + self.manager.output_dtype = self._validate_output_dtype(dtype) + return {"ok": True, "output_dtype": self.manager.output_dtype} + + def begin(self, frame_type=None, output_dtype=None, capture_mode=None): + if not self.connected: + self.connect() + + if self.manager.running: + return { + "ok": True, + "started": True, + "already_running": True, + "status": self.get_status(), + } + + if frame_type is not None: + self.manager.frame_type = self._validate_frame_type(frame_type) + + if output_dtype is not None: + self.manager.output_dtype = self._validate_output_dtype(output_dtype) + + if capture_mode is not None: + self.manager.capture_mode = self._validate_capture_mode(capture_mode) + + self.manager.start() + + return { + "ok": True, + "started": True, + "already_running": False, + "status": self.get_status(), + } + + def capture_frame(self, timeout=None): + if timeout is None: + timeout = self.timeout + + frame, meta = self.manager.get_next_frame(timeout=timeout) + + return frame, meta + + def stop(self): + self.manager.stop() + return {"ok": True, "stopped": True} + + def _ensure_stopped_for_config(self): + if self.manager.running: + raise RuntimeError( + "Configuração estrutural só pode ser alterada com o manager parado. " + "Chame stop() antes." + ) + + def resolve_camera_id(self, cam_id=None, role=None): + if role is not None: + role = str(role).lower() + + status = self.manager.get_status() + for cam in status.get("cameras", []): + if str(cam.get("role", "")).lower() == role: + return cam["id"] + + raise ValueError(f"Nenhuma câmera ativa encontrada para role={role}") + + if cam_id is None: + raise ValueError("Informe cam_id ou role.") + + return str(cam_id) + + + def get_camera_controls(self, cam_id=None, role=None): + cam_id = self.resolve_camera_id(cam_id=cam_id, role=role) + ctrl = self.manager.get_camera_controls(cam_id) + ctrl["ok"] = True + ctrl["camera_id"] = cam_id + ctrl["role"] = role + return ctrl + + def set_ae_enable(self, cam_id=None, role=None, enable=False): + cam_id = self.resolve_camera_id(cam_id=cam_id, role=role) + ctrl = self.manager.set_ae_enable(cam_id, bool(enable)) + ctrl["ok"] = True + ctrl["camera_id"] = cam_id + ctrl["role"] = role + return ctrl + + def set_awb_enable(self, cam_id=None, role=None, enable=False): + cam_id = self.resolve_camera_id(cam_id=cam_id, role=role) + ctrl = self.manager.set_awb_enable(cam_id, bool(enable)) + ctrl["ok"] = True + ctrl["camera_id"] = cam_id + ctrl["role"] = role + return ctrl + + def set_exposure_time(self, cam_id=None, role=None, exposure_time_us=None): + if exposure_time_us is None: + raise ValueError("exposure_time_us é obrigatório.") + cam_id = self.resolve_camera_id(cam_id=cam_id, role=role) + ctrl = self.manager.set_exposure_time(cam_id, int(exposure_time_us)) + ctrl["ok"] = True + ctrl["camera_id"] = cam_id + ctrl["role"] = role + return ctrl + + def set_analogue_gain(self, cam_id=None, role=None, analogue_gain=None): + if analogue_gain is None: + raise ValueError("analogue_gain é obrigatório.") + cam_id = self.resolve_camera_id(cam_id=cam_id, role=role) + ctrl = self.manager.set_analogue_gain(cam_id, float(analogue_gain)) + ctrl["ok"] = True + ctrl["camera_id"] = cam_id + ctrl["role"] = role + return ctrl + + def apply_camera_controls(self, cam_id=None, role=None, controls=None): + cam_id = self.resolve_camera_id(cam_id=cam_id, role=role) + ctrl = self.manager.apply_camera_controls(cam_id, controls or {}) + ctrl["ok"] = True + ctrl["camera_id"] = cam_id + ctrl["role"] = role + return ctrl + + + def _validate_frame_type(self, frame_type): + frame_type = str(frame_type).upper() + if frame_type not in ("RAW_BRUTO", "RGB", "MULTISPEC", "PREVIEW"): + raise ValueError(f"frame_type inválido: {frame_type}") + return frame_type + + def _validate_output_dtype(self, dtype): + dtype = str(dtype).lower() + if dtype not in ("uint8", "uint16", "float32"): + raise ValueError(f"output_dtype inválido: {dtype}") + return dtype + + def _validate_capture_mode(self, mode): + mode = str(mode).upper() + if mode not in ("AUTO", "SINGLE", "DOUBLE", "TRIPLE"): + raise ValueError(f"capture_mode inválido: {mode}") + return mode diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/oak_fcc3_core/radiometric_controller.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/oak_fcc3_core/radiometric_controller.py new file mode 100644 index 000000000..c9c363fa1 --- /dev/null +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/oak_fcc3_core/radiometric_controller.py @@ -0,0 +1,2017 @@ +import time +import json +import numpy as np + + +class RadiometricController: + """ + Controlador radiométrico para o módulo RGB/RE/NIR. + + Modos principais: + metering_mode: + - "global": usa uma região grande da cena, robusta por percentis. + - "reference_patches": usa patches/ROIs conhecidos, por exemplo branco/cinza/preto. + - "legacy_patch": compatível com o comportamento antigo: strip_y/patch_x. + + spectral_control_mode: + - "independent": controla rgb, re e nir separadamente. + - "shared": controla rgb separado e aplica uma decisão conjunta para re/nir. + """ + + def __init__( + self, + client, + enabled=True, + config_json_path=None, + interval_s=0.5, + strip_y0_pct=0.95, + strip_y1_pct=1.0, + patch_x0_pct=0.35, + patch_x1_pct=0.75, + target_mean=0.55, + deadband=0.04, + alpha=0.18, + exp_min_us=100, + exp_max_us=80000, + gain_min=1.0, + gain_max=4.0, + exp_step_gain=0.55, + prefer_exposure=True, + verbose=False, + ): + self.client = client + cfg = self._load_config_json(config_json_path) + + self.enabled = bool(cfg.get("enabled", enabled)) + self.interval_s = float(cfg.get("interval_s", interval_s)) + self.verbose = bool(cfg.get("verbose", verbose)) + + self.metering_mode = str(cfg.get("metering_mode", "global")).lower() + self.spectral_control_mode = str(cfg.get("spectral_control_mode", "shared")).lower() + + if self.metering_mode not in ("global", "reference_patches", "legacy_patch"): + self.metering_mode = "global" + + if self.spectral_control_mode not in ("shared", "independent"): + self.spectral_control_mode = "shared" + + self.strip_y0_pct = float(cfg.get("strip_y0_pct", strip_y0_pct)) + self.strip_y1_pct = float(cfg.get("strip_y1_pct", strip_y1_pct)) + self.patch_x0_pct = float(cfg.get("patch_x0_pct", patch_x0_pct)) + self.patch_x1_pct = float(cfg.get("patch_x1_pct", patch_x1_pct)) + + global_roi = cfg.get("global_roi_pct", {}) or {} + self.global_roi_pct = self._safe_roi_pct( + global_roi, + fallback={"x0": 0.08, "y0": 0.08, "x1": 0.92, "y1": 0.92}, + ) + + raw_global_by_role = cfg.get("global_roi_pct_by_role", {}) or {} + self.global_roi_pct_by_role = {} + + for role in self.ROLES: + roi = raw_global_by_role.get(role) + if isinstance(roi, dict) and roi: + self.global_roi_pct_by_role[role] = self._safe_roi_pct( + roi, + fallback=self.global_roi_pct, + ) + else: + self.global_roi_pct_by_role[role] = dict(self.global_roi_pct) + + self.reference_patches = cfg.get("reference_patches", []) or [] + self.patch_aggregation = str(cfg.get("patch_aggregation", "weighted_mean")).lower() + + # Novo contrato de ROIs dinâmicas por patch/cor/câmera. + # Compatível com o formato antigo: roi_pct_by_role e roi_pct continuam + # servindo como fallback quando a lista dinâmica não existir. + self.patch_roi_contract = str(cfg.get("patch_roi_contract", "legacy_single_roi")).lower() + self.patch_roi_reduce_method = str(cfg.get("patch_roi_reduce_method", "median_valid_rois")).lower() + self.patch_roi_outlier_reject = bool(cfg.get("patch_roi_outlier_reject", True)) + self.patch_roi_max_p50_delta = float(cfg.get("patch_roi_max_p50_delta", 0.12)) + self.patch_roi_min_valid_rois = int(cfg.get("patch_roi_min_valid_rois", 1)) + + self.patch_control_mode = str(cfg.get("patch_control_mode", "gray_primary")).lower() + + self.patch_require_order = bool(cfg.get("patch_require_order", True)) + self.patch_min_separation = float(cfg.get("patch_min_separation", 0.08)) + + self.patch_white_sat_limit_pct = float(cfg.get("patch_white_sat_limit_pct", 0.50)) + self.patch_white_p95_limit = float(cfg.get("patch_white_p95_limit", 0.90)) + + self.patch_black_dark_limit_pct = float(cfg.get("patch_black_dark_limit_pct", 80.0)) + self.patch_black_max_p50 = float(cfg.get("patch_black_max_p50", 0.20)) + + self.patch_gray_min_p50 = float(cfg.get("patch_gray_min_p50", 0.08)) + self.patch_gray_max_p50 = float(cfg.get("patch_gray_max_p50", 0.85)) + + self.control_metric = str(cfg.get("control_metric", "p50")).lower() + self.target_value = float(cfg.get("target_value", cfg.get("target_mean", target_mean))) + self.target_mean = self.target_value + self.deadband = float(cfg.get("deadband", deadband)) + self.alpha = float(cfg.get("alpha", alpha)) + + self.p95_limit = float(cfg.get("p95_limit", 0.92)) + self.saturation_limit_pct = float(cfg.get("saturation_limit_pct", 0.50)) + self.dark_limit_pct = float(cfg.get("dark_limit_pct", 35.0)) + + self.reduce_fast_factor = float(cfg.get("reduce_fast_factor", 0.82)) + self.factor_min = float(cfg.get("factor_min", 0.72)) + self.factor_max = float(cfg.get("factor_max", 1.28)) + + self.saturation_hard_pct = float(cfg.get("saturation_hard_pct", 20.0)) + self.saturation_extreme_pct = float(cfg.get("saturation_extreme_pct", 60.0)) + + # ============================================================ + # Global Saturation Guard + Sun Guard + # ============================================================ + # O controle por patches continua sendo a referência radiométrica. + # Estas guardas olham a cena útil inteira para detectar regiões + # saturadas/sol direto fora dos cartões. + self.global_saturation_guard_enabled = bool(cfg.get("global_saturation_guard_enabled", True)) + self.global_guard_roi_pct = self._safe_roi_pct( + cfg.get("global_guard_roi_pct", cfg.get("global_roi_pct", {})) or {}, + fallback={"x0": 0.05, "y0": 0.05, "x1": 0.95, "y1": 0.95}, + ) + + raw_guard_by_role = cfg.get("global_guard_roi_pct_by_role", {}) or {} + self.global_guard_roi_pct_by_role = {} + for role in self.ROLES: + roi = raw_guard_by_role.get(role) + if isinstance(roi, dict) and roi: + self.global_guard_roi_pct_by_role[role] = self._safe_roi_pct( + roi, + fallback=self.global_guard_roi_pct, + ) + else: + self.global_guard_roi_pct_by_role[role] = dict(self.global_guard_roi_pct) + + self.global_guard_sat_threshold = float(cfg.get("global_guard_sat_threshold", 0.985)) + self.global_guard_near_sat_threshold = float(cfg.get("global_guard_near_sat_threshold", 0.940)) + + self.global_guard_sat_pct_soft = float(cfg.get("global_guard_sat_pct_soft", 0.08)) + self.global_guard_sat_pct_hard = float(cfg.get("global_guard_sat_pct_hard", 0.35)) + self.global_guard_sat_pct_extreme = float(cfg.get("global_guard_sat_pct_extreme", 1.50)) + + self.global_guard_blob_pct_soft = float(cfg.get("global_guard_blob_pct_soft", 0.025)) + self.global_guard_blob_pct_hard = float(cfg.get("global_guard_blob_pct_hard", 0.120)) + self.global_guard_blob_pct_extreme = float(cfg.get("global_guard_blob_pct_extreme", 0.400)) + + self.global_guard_min_blob_px = int(cfg.get("global_guard_min_blob_px", 48)) + self.global_guard_downsample_max_side = int(cfg.get("global_guard_downsample_max_side", 320)) + + self.global_guard_reduce_factor_soft = float(cfg.get("global_guard_reduce_factor_soft", 0.88)) + self.global_guard_reduce_factor_hard = float(cfg.get("global_guard_reduce_factor_hard", 0.68)) + self.global_guard_reduce_factor_extreme = float(cfg.get("global_guard_reduce_factor_extreme", 0.45)) + + self.sun_guard_enabled = bool(cfg.get("sun_guard_enabled", True)) + self.sun_guard_p99_threshold = float(cfg.get("sun_guard_p99_threshold", 0.900)) + self.sun_guard_near_sat_pct_threshold = float(cfg.get("sun_guard_near_sat_pct_threshold", 1.00)) + self.sun_guard_freeze_increase_cycles = int(cfg.get("sun_guard_freeze_increase_cycles", 2)) + self.sun_guard_allow_decrease = bool(cfg.get("sun_guard_allow_decrease", True)) + + # Modo parrudo: quando há clarão real, a guarda deixa de ser só + # consultiva e vira proteção prioritária. Isso evita perder frames + # por saturação quando a área clara está fora dos cartões. + self.guard_force_apply_enabled = bool(cfg.get("guard_force_apply_enabled", True)) + self.guard_force_apply_soft = bool(cfg.get("guard_force_apply_soft", True)) + self.guard_force_apply_hard = bool(cfg.get("guard_force_apply_hard", True)) + self.guard_force_apply_extreme = bool(cfg.get("guard_force_apply_extreme", True)) + self.guard_force_apply_on_patch_saturation = bool(cfg.get("guard_force_apply_on_patch_saturation", True)) + + # Depois de um clarão, seguramos qualquer aumento por alguns ciclos. + # Isso dá tempo para o pipeline aplicar a exposição e impede sanfona + # quando a chapa branca entra/sai rapidamente do campo central. + self.guard_freeze_cycles_soft = int(cfg.get("guard_freeze_cycles_soft", max(2, self.sun_guard_freeze_increase_cycles))) + self.guard_freeze_cycles_hard = int(cfg.get("guard_freeze_cycles_hard", max(4, self.sun_guard_freeze_increase_cycles))) + self.guard_freeze_cycles_extreme = int(cfg.get("guard_freeze_cycles_extreme", max(6, self.sun_guard_freeze_increase_cycles))) + + # Se a decisão chegar ao mínimo de exposição, reenviamos o comando + # em emergência mesmo que o estado interno já ache que está no mínimo. + # Isso cobre atraso de pipeline e diferença entre estado lógico e câmera real. + self.guard_reapply_min_exp_on_emergency = bool(cfg.get("guard_reapply_min_exp_on_emergency", True)) + self.guard_min_exp_margin_us = int(cfg.get("guard_min_exp_margin_us", 60)) + + self._sun_guard_hold_cycles = { + "rgb": 0, + "re": 0, + "nir": 0, + "spectral_shared": 0, + } + + self.gain_return_enabled = bool(cfg.get("gain_return_enabled", True)) + self.gain_return_factor = float(cfg.get("gain_return_factor", 0.60)) + self.gain_reduce_on_saturation = bool(cfg.get("gain_reduce_on_saturation", True)) + + self.exp_high_ratio_for_gain = float(cfg.get("exp_high_ratio_for_gain", 0.85)) + self.exp_low_ratio_for_gain_return = float(cfg.get("exp_low_ratio_for_gain_return", 0.65)) + + self.gain_increase_required_cycles = int(cfg.get("gain_increase_required_cycles", 5)) + self.gain_decrease_required_cycles = int(cfg.get("gain_decrease_required_cycles", 2)) + + self.gain_step_up = float(cfg.get("gain_step_up", 0.25)) + self.gain_step_down = float(cfg.get("gain_step_down", 0.50)) + + self.gain_hard_reset_on_saturation = bool(cfg.get("gain_hard_reset_on_saturation", False)) + + self._underexposed_cycles = { + "rgb": 0, + "re": 0, + "nir": 0, + "spectral_shared": 0, + } + + self._overexposed_cycles = { + "rgb": 0, + "re": 0, + "nir": 0, + "spectral_shared": 0, + } + + self.control_strategy = str(cfg.get("control_strategy", "ratio")).lower() + + self.ratio_alpha = float(cfg.get("ratio_alpha", 0.55)) + self.ratio_min = float(cfg.get("ratio_min", 0.55)) + self.ratio_max = float(cfg.get("ratio_max", 1.85)) + + self.ready_required_cycles = int(cfg.get("ready_required_cycles", 3)) + self._ready_cycles = { + "rgb": 0, + "re": 0, + "nir": 0, + "spectral_shared": 0, + } + + self.exp_min_us = int(cfg.get("exp_min_us", exp_min_us)) + self.exp_max_us = int(cfg.get("exp_max_us", exp_max_us)) + self.gain_min = float(cfg.get("gain_min", gain_min)) + self.gain_max = float(cfg.get("gain_max", gain_max)) + self.role_limits = cfg.get("role_limits", {}) or {} + + self.exp_step_gain = float(cfg.get("exp_step_gain", exp_step_gain)) + self.prefer_exposure = bool(cfg.get("prefer_exposure", prefer_exposure)) + + self.exp_apply_threshold_us = int(cfg.get("exp_apply_threshold_us", 80)) + self.gain_apply_threshold = float(cfg.get("gain_apply_threshold", 0.05)) + + self.apply_same_spectral_to_both = bool(cfg.get("apply_same_spectral_to_both", True)) + self.spectral_roles = tuple(cfg.get("spectral_roles", ["re", "nir"])) + + self.last_update_ts = 0.0 + self.last_result = {} + + self.state = { + "rgb": {"exp": 15000, "gain": 1.0}, + "nir": {"exp": 15000, "gain": 1.0}, + "re": {"exp": 15000, "gain": 1.0}, + } + + self._ae_disabled = set() + self._last_applied = { + "rgb": {"exp": None, "gain": None}, + "nir": {"exp": None, "gain": None}, + "re": {"exp": None, "gain": None}, + } + + self.patch_two_roi_soften_risk = bool(cfg.get("patch_two_roi_soften_risk", True)) + self.patch_two_roi_white_risk_percentile = float(cfg.get("patch_two_roi_white_risk_percentile", 75.0)) + self.patch_two_roi_other_risk_percentile = float(cfg.get("patch_two_roi_other_risk_percentile", 50.0)) + self.patch_white_single_roi_saturation_reject = bool(cfg.get("patch_white_single_roi_saturation_reject", True)) + self.patch_white_roi_reject_sat_pct = float(cfg.get("patch_white_roi_reject_sat_pct", 5.0)) + self.patch_white_roi_reject_p95 = float(cfg.get("patch_white_roi_reject_p95", 0.995)) + + def _load_config_json(self, path): + if not path: + return {} + try: + with open(path, "r", encoding="utf-8") as f: + data = json.load(f) + except Exception: + return {} + cfg = data.get("radiometric_config", {}) + return cfg if isinstance(cfg, dict) else {} + + ROLES = ("rgb", "re", "nir") + + @classmethod + def _normalize_role(cls, role: str) -> str: + role = str(role or "").lower() + return role if role in cls.ROLES else "rgb" + + @staticmethod + def _safe_roi_pct(roi_pct, fallback=None) -> dict: + if fallback is None: + fallback = {"x0": 0.08, "y0": 0.08, "x1": 0.92, "y1": 0.92} + + if not isinstance(roi_pct, dict): + roi_pct = fallback + + return { + "x0": float(roi_pct.get("x0", fallback.get("x0", 0.08))), + "y0": float(roi_pct.get("y0", fallback.get("y0", 0.08))), + "x1": float(roi_pct.get("x1", fallback.get("x1", 0.92))), + "y1": float(roi_pct.get("y1", fallback.get("y1", 0.92))), + } + + def _get_global_roi_pct_for_role(self, role: str) -> dict: + role = self._normalize_role(role) + + by_role = getattr(self, "global_roi_pct_by_role", {}) or {} + if isinstance(by_role, dict): + roi = by_role.get(role) + if isinstance(roi, dict) and roi: + return self._safe_roi_pct(roi, fallback=self.global_roi_pct) + + return self._safe_roi_pct(self.global_roi_pct) + + def _get_global_guard_roi_pct_for_role(self, role: str) -> dict: + role = self._normalize_role(role) + + by_role = getattr(self, "global_guard_roi_pct_by_role", {}) or {} + if isinstance(by_role, dict): + roi = by_role.get(role) + if isinstance(roi, dict) and roi: + return self._safe_roi_pct(roi, fallback=self.global_guard_roi_pct) + + return self._safe_roi_pct(self.global_guard_roi_pct) + + def _get_patch_roi_items_for_role(self, patch: dict, role: str) -> list[dict]: + """ + Retorna uma lista normalizada de ROIs para um patch/cor em uma câmera. + + Contrato novo: + patch["roi_list_by_role"][role] = [ + {"name": "gray_rgb_01", "enabled": True, "roi_pct": {...}}, + ... + ] + + Fallbacks legados: + patch["roi_pct_by_role"][role] + patch["roi_pct"] + """ + role = self._normalize_role(role) + items: list[dict] = [] + + list_by_role = patch.get("roi_list_by_role", {}) + raw_items = [] + if isinstance(list_by_role, dict): + role_items = list_by_role.get(role) + if isinstance(role_items, list): + raw_items = role_items + + for idx, item in enumerate(raw_items, start=1): + if not isinstance(item, dict): + continue + if item.get("enabled", True) is False: + continue + roi = item.get("roi_pct") + if not isinstance(roi, dict) or not roi: + continue + items.append({ + "name": str(item.get("name") or f"{patch.get('type', 'patch')}_{role}_{idx:02d}"), + "enabled": True, + "roi_pct": self._safe_roi_pct(roi), + "roi_source": "roi_list_by_role", + "index": int(idx - 1), + }) + + if items: + return items + + # Fallback 1: contrato antigo por câmera. + by_role = patch.get("roi_pct_by_role", {}) + if isinstance(by_role, dict): + roi = by_role.get(role) + if isinstance(roi, dict) and roi: + return [{ + "name": f"{patch.get('type', 'patch')}_{role}_legacy_by_role", + "enabled": True, + "roi_pct": self._safe_roi_pct(roi), + "roi_source": "roi_pct_by_role", + "index": 0, + }] + + # Fallback 2: contrato antigo global do patch. + legacy = patch.get("roi_pct") + if isinstance(legacy, dict) and legacy: + return [{ + "name": f"{patch.get('type', 'patch')}_{role}_legacy", + "enabled": True, + "roi_pct": self._safe_roi_pct(legacy), + "roi_source": "roi_pct", + "index": 0, + }] + + return [] + + def _get_patch_roi_pct_for_role(self, patch: dict, role: str): + """Compatibilidade para código legado: retorna a primeira ROI ativa.""" + items = self._get_patch_roi_items_for_role(patch, role) + if not items: + return None, "missing" + item = items[0] + return item["roi_pct"], item.get("roi_source", "roi_list_by_role") + + def get_patch_target_for_role(self, patch, role, fallback): + by_role = patch.get("target_value_by_role", {}) + if isinstance(by_role, dict) and role in by_role: + return float(by_role[role]) + return float(patch.get("target_value", fallback)) + + def sync_from_camera_controls(self, camera_controls: dict | None): + if not isinstance(camera_controls, dict): + return + for role, ctrl in camera_controls.items(): + role = str(role).lower() + if role not in self.state or not isinstance(ctrl, dict): + continue + src = ctrl + if "requested" in ctrl and isinstance(ctrl["requested"], dict): + src = ctrl["requested"] + exp = src.get("exposure_time_us") + gain = src.get("analogue_gain") + if exp is not None: + self.state[role]["exp"] = int(exp) + if gain is not None: + self.state[role]["gain"] = float(gain) + + def sync_from_actual_camera_controls(self): + for role in ("rgb", "re", "nir"): + try: + ctrl = self.client.svc.get_camera_controls(role=role) + exp = ctrl.get("exposure_time_us") + gain = ctrl.get("analogue_gain") + if exp is not None: + self.state[role]["exp"] = int(exp) + if gain is not None: + self.state[role]["gain"] = float(gain) + except Exception: + pass + + def update(self, decoded: dict, meta: dict | None = None): + if not self.enabled: + return None + now = time.perf_counter() + if now - self.last_update_ts < self.interval_s: + return None + self.last_update_ts = now + + results = {} + rgb_result = self._update_single_role(decoded, "rgb") + if rgb_result is not None: + results["rgb"] = rgb_result + + if self.spectral_control_mode == "independent": + for role in self.spectral_roles: + result = self._update_single_role(decoded, role) + if result is not None: + results[role] = result + else: + result = self._update_spectral_shared(decoded) + if result is not None: + results["spectral_shared"] = result + + self.last_result = results + return results + + def _update_single_role(self, decoded: dict, role: str): + cam_id = self._resolve_cam_id(decoded, role) + if cam_id is None: + return None + img = decoded[cam_id].get("image") + if img is None: + return None + metrics = self.measure_image(img, role=role) + decision = self.compute_control(role, metrics) + apply_resp = self.apply_control(role, decision) + result = { + "mode": "single_role", + "cam_id": cam_id, + "role": role, + "metering_mode": self.metering_mode, + "metrics": metrics, + "decision": decision, + "apply": apply_resp, + } + self._print_metrics_debug(role, result) + + return result + + def _update_spectral_shared(self, decoded: dict): + role_items = {} + for role in self.spectral_roles: + cam_id = self._resolve_cam_id(decoded, role) + if cam_id is None: + continue + img = decoded[cam_id].get("image") + if img is None: + continue + role_items[role] = { + "cam_id": cam_id, + "metrics": self.measure_image(img, role=role), + } + if not role_items: + return None + + shared_metrics = self.aggregate_spectral_metrics(role_items) + state_role = "re" if "re" in role_items else list(role_items.keys())[0] + decision = self.compute_control(state_role, shared_metrics, virtual_role="spectral_shared") + + apply_resp = {} + if self.apply_same_spectral_to_both: + for role in role_items.keys(): + apply_resp[role] = self.apply_control(role, decision) + else: + apply_resp[state_role] = self.apply_control(state_role, decision) + + result = { + "mode": "shared_spectral", + "roles": role_items, + "metering_mode": self.metering_mode, + "metrics": shared_metrics, + "decision": decision, + "apply": apply_resp, + } + self._print_metrics_debug("spectral_shared", result) + + return result + + def _update_ready_state( + self, + log_role: str, + action: str, + error: float, + p95: float, + sat_pct: float, + ) -> tuple[bool, int]: + key = str(log_role).lower() + + is_ready_now = ( + action == "hold" + and abs(float(error)) <= self.deadband + and float(p95) <= self.p95_limit + and float(sat_pct) <= self.saturation_limit_pct + ) + + if is_ready_now: + self._ready_cycles[key] = self._ready_cycles.get(key, 0) + 1 + else: + self._ready_cycles[key] = 0 + + cycles = self._ready_cycles.get(key, 0) + return cycles >= self.ready_required_cycles, cycles + + def _update_exposure_pressure_state( + self, + log_role: str, + error: float, + p95: float, + sat_pct: float, + ) -> tuple[int, int]: + key = str(log_role).lower() + + under = ( + error > self.deadband + and p95 < self.p95_limit + and sat_pct <= self.saturation_limit_pct + ) + + over = ( + error < -self.deadband + or p95 > self.p95_limit + or sat_pct > self.saturation_limit_pct + ) + + if under: + self._underexposed_cycles[key] = self._underexposed_cycles.get(key, 0) + 1 + else: + self._underexposed_cycles[key] = 0 + + if over: + self._overexposed_cycles[key] = self._overexposed_cycles.get(key, 0) + 1 + else: + self._overexposed_cycles[key] = 0 + + return ( + self._underexposed_cycles.get(key, 0), + self._overexposed_cycles.get(key, 0), + ) + + def _resolve_cam_id(self, decoded, role): + role = str(role).lower() + for cam_id, data in decoded.items(): + if str(data.get("role", "")).lower() == role: + return cam_id + return None + + def measure_image(self, img01: np.ndarray, role: str) -> dict: + role = self._normalize_role(role) + gray = self.to_luma_or_gray(img01) + + if self.metering_mode == "reference_patches": + return self.measure_reference_patches(gray, role=role) + + if self.metering_mode == "legacy_patch": + return self.measure_legacy_patch(gray) + + return self.measure_global(gray, role=role) + + @staticmethod + def to_luma_or_gray(img01: np.ndarray) -> np.ndarray: + if img01.ndim == 3: + return ( + 0.299 * img01[:, :, 0] + + 0.587 * img01[:, :, 1] + + 0.114 * img01[:, :, 2] + ).astype(np.float32) + return img01.astype(np.float32) + + def measure_global(self, img_gray: np.ndarray, role: str = "rgb") -> dict: + role = self._normalize_role(role) + + h, w = img_gray.shape[:2] + roi_pct = self._get_global_roi_pct_for_role(role) + + roi = self._roi_pct_to_pixels( + h, w, + roi_pct["x0"], + roi_pct["y0"], + roi_pct["x1"], + roi_pct["y1"], + ) + + arr = self._crop_array(img_gray, roi) + stats = self.compute_stats(arr) + + stats["roi"] = list(roi) + stats["roi_pct"] = dict(roi_pct) + stats["roi_source"] = "global_roi_pct_by_role" + stats["role"] = role + stats["source"] = "global" + + return stats + + def measure_legacy_patch(self, img_gray: np.ndarray) -> dict: + h, w = img_gray.shape[:2] + roi = self._roi_pct_to_pixels( + h, w, + self.patch_x0_pct, + self.strip_y0_pct, + self.patch_x1_pct, + self.strip_y1_pct, + ) + arr = self._crop_array(img_gray, roi) + stats = self.compute_stats(arr) + stats["roi"] = list(roi) + stats["source"] = "legacy_patch" + return stats + + def measure_reference_patches(self, img_gray: np.ndarray, role: str) -> dict: + role = self._normalize_role(role) + + h, w = img_gray.shape[:2] + patch_results = [] + + for patch in self.reference_patches: + if not isinstance(patch, dict): + continue + + roles = patch.get("roles", ["rgb", "re", "nir", "all"]) + roles = [str(r).lower() for r in roles] + + if role not in roles and "all" not in roles: + continue + + roi_items = self._get_patch_roi_items_for_role(patch, role) + if not roi_items: + continue + + target = self.get_patch_target_for_role( + patch=patch, + role=role, + fallback=self.target_value, + ) + if target is not None: + target = float(target) + + roi_results = [] + for roi_item in roi_items: + roi_pct = roi_item.get("roi_pct") + if not isinstance(roi_pct, dict): + continue + + x0 = float(roi_pct.get("x0", 0.0)) + y0 = float(roi_pct.get("y0", 0.0)) + x1 = float(roi_pct.get("x1", 1.0)) + y1 = float(roi_pct.get("y1", 1.0)) + + roi = self._roi_pct_to_pixels(h, w, x0, y0, x1, y1) + arr = self._crop_array(img_gray, roi) + stats = self.compute_stats(arr) + + roi_results.append({ + "name": roi_item.get("name"), + "enabled": bool(roi_item.get("enabled", True)), + "role": role, + "roi": list(roi), + "roi_pct": {"x0": x0, "y0": y0, "x1": x1, "y1": y1}, + "roi_source": roi_item.get("roi_source", "roi_list_by_role"), + "index": int(roi_item.get("index", len(roi_results))), + "stats": stats, + }) + + if not roi_results: + continue + + patch_stats, roi_quality = self.aggregate_roi_metrics( + roi_results=roi_results, + patch_type=str(patch.get("type", "reference")).lower(), + ) + + first_roi = next( + (r for r in roi_results if r.get("stats", {}).get("valid")), + roi_results[0], + ) + + patch_results.append({ + "name": patch.get("name", f"patch_{len(patch_results) + 1}"), + "type": patch.get("type", "reference"), + "role": role, + "roi": first_roi.get("roi", []), + "roi_pct": first_roi.get("roi_pct", {}), + "roi_source": "roi_list_by_role" if len(roi_results) > 1 else first_roi.get("roi_source", "roi_list_by_role"), + "roi_count": int(len(roi_results)), + "roi_valid_count": int(roi_quality.get("valid_count", 0)), + "roi_results": roi_results, + "roi_quality": roi_quality, + "weight": float(patch.get("weight", 1.0)), + "target_value": target, + "stats": patch_stats, + }) + + if not patch_results: + stats = self.measure_global(img_gray, role=role) + stats["source"] = "reference_patches_fallback_global" + stats["patches"] = [] + stats["global_guard"] = self.measure_global_guard(img_gray, role=role) + return stats + + metrics = self.aggregate_patch_metrics(patch_results) + metrics["role"] = role + metrics["global_guard"] = self.measure_global_guard(img_gray, role=role) + + return metrics + + def aggregate_roi_metrics(self, roi_results: list[dict], patch_type: str = "reference") -> tuple[dict, dict]: + """ + Agrega várias ROIs da mesma cor/câmera em uma métrica única. + + O controle usa uma estatística robusta, normalmente mediana dos p50. + As guardas de proteção usam p95/saturação máximos para não ignorar + uma ROI branca estourada, mesmo quando outra ROI está boa. + """ + valid = [r for r in roi_results if r.get("stats", {}).get("valid")] + warnings = [] + + if not valid: + return { + "valid": False, + "pixels": 0, + "mean": 0.0, + "std": 0.0, + "p05": 0.0, + "p50": 0.0, + "p95": 0.0, + "sat_pct": 0.0, + "dark_pct": 0.0, + }, { + "valid": False, + "warnings": ["no_valid_roi"], + "roi_count": len(roi_results), + "valid_count": 0, + "used_count": 0, + "rejected_count": 0, + "p50_values": [], + } + + p50_values = np.array([float(r["stats"].get("p50", 0.0)) for r in valid], dtype=np.float32) + median_p50 = float(np.median(p50_values)) + spread_p50 = float(np.max(p50_values) - np.min(p50_values)) if len(p50_values) > 1 else 0.0 + + used = valid + rejected = [] + + if self.patch_roi_outlier_reject and len(valid) >= 3: + kept = [] + + for r in valid: + p50 = float(r["stats"].get("p50", 0.0)) + + if abs(p50 - median_p50) <= self.patch_roi_max_p50_delta: + kept.append(r) + else: + rejected.append(r) + + if kept: + used = kept + + if rejected: + warnings.append(f"roi_outliers_rejected:{len(rejected)}") + + elif self.patch_roi_outlier_reject and len(valid) == 2 and spread_p50 > self.patch_roi_max_p50_delta: + # Com apenas 2 ROIs não dá para saber qual é o outlier. + # Então não descartamos nenhuma. Usamos as duas, mas marcamos que + # existe divergência para a etapa de agregação suavizar p95/saturação. + warnings.append(f"roi_p50_spread_high:{spread_p50:.3f}") + used = valid + + patch_type_norm = str(patch_type or "reference").lower() + + # ------------------------------------------------------------ + # Caso especial: branco com 2 ROIs + # ------------------------------------------------------------ + # Se apenas uma ROI branca saturou muito e a outra está limpa, + # tratamos a saturada como reflexo/ROI ruim naquele ciclo. + # Se as duas saturaram, mantém as duas e deixa o controller reduzir. + if ( + patch_type_norm == "white" + and self.patch_white_single_roi_saturation_reject + and len(used) == 2 + ): + clean_rois = [] + saturated_rois = [] + + for r in used: + st = r.get("stats", {}) or {} + roi_sat = float(st.get("sat_pct", 0.0)) + roi_p95 = float(st.get("p95", 0.0)) + + is_bad_white_roi = ( + roi_sat > self.patch_white_roi_reject_sat_pct + or roi_p95 >= self.patch_white_roi_reject_p95 + ) + + if is_bad_white_roi: + saturated_rois.append(r) + else: + clean_rois.append(r) + + # Só rejeita se exatamente uma saturou e existe uma ROI limpa. + # Se as duas saturaram, é saturação real do branco, então reduz exposição. + if len(saturated_rois) == 1 and len(clean_rois) == 1: + bad_name = str(saturated_rois[0].get("name", "unknown")) + warnings.append(f"white_single_saturated_roi_rejected:{bad_name}") + + rejected.extend(saturated_rois) + used = clean_rois + + if len(used) < self.patch_roi_min_valid_rois: + warnings.append(f"valid_roi_count_low:{len(used)}<{self.patch_roi_min_valid_rois}") + + def arr(key: str) -> np.ndarray: + return np.array([float(r["stats"].get(key, 0.0)) for r in used], dtype=np.float32) + + means = arr("mean") + stds = arr("std") + p05s = arr("p05") + p50s = arr("p50") + p95s = arr("p95") + sats = arr("sat_pct") + darks = arr("dark_pct") + pixels = int(sum(int(r["stats"].get("pixels", 0)) for r in used)) + + reduce_method = self.patch_roi_reduce_method + if reduce_method not in ("median_valid_rois", "mean_valid_rois"): + reduce_method = "median_valid_rois" + + reducer = np.mean if reduce_method == "mean_valid_rois" else np.median + + # ------------------------------------------------------------ + # Agregação robusta de risco para múltiplas ROIs + # ------------------------------------------------------------ + # Para o valor de controle usamos redução robusta. + # Para risco de saturação, o código antigo usava sempre o máximo. + # Isso é seguro, mas com 2 cartões pode deixar uma única ROI mandar + # derrubar a exposição inteira. Então suavizamos quando há divergência. + two_roi_disagreement = ( + self.patch_two_roi_soften_risk + and self.patch_roi_outlier_reject + and len(valid) == 2 + and spread_p50 > self.patch_roi_max_p50_delta + ) + + patch_type_norm = str(patch_type or "reference").lower() + + if two_roi_disagreement: + if patch_type_norm == "white": + risk_percentile = self.patch_two_roi_white_risk_percentile + else: + risk_percentile = self.patch_two_roi_other_risk_percentile + + p95_agg = float(np.percentile(p95s, risk_percentile)) + sat_agg = float(np.percentile(sats, risk_percentile)) + + warnings.append( + f"roi_risk_softened_for_2_rois:{patch_type_norm}:p{risk_percentile:.0f}" + ) + else: + p95_agg = float(np.max(p95s)) + sat_agg = float(np.max(sats)) + + + stats = { + "valid": True, + "pixels": pixels, + "mean": float(reducer(means)), + "std": float(reducer(stds)), + "p05": float(reducer(p05s)), + "p50": float(reducer(p50s)), + + # Proteção suavizada quando há exatamente 2 ROIs divergentes. + "p95": p95_agg, + "sat_pct": sat_agg, + + "dark_pct": float(reducer(darks)), + "roi_reduce_method": reduce_method, + "roi_count": int(len(roi_results)), + "roi_valid_count": int(len(valid)), + "roi_used_count": int(len(used)), + "roi_rejected_count": int(len(rejected)), + "roi_p50_values": [float(v) for v in p50_values.tolist()], + "roi_p50_spread": spread_p50, + "roi_two_roi_disagreement": bool(two_roi_disagreement), + } + + quality = { + "valid": bool(len(used) >= self.patch_roi_min_valid_rois), + "warnings": warnings, + "roi_count": int(len(roi_results)), + "valid_count": int(len(valid)), + "used_count": int(len(used)), + "rejected_count": int(len(rejected)), + "p50_values": [float(v) for v in p50_values.tolist()], + "p50_spread": spread_p50, + "reduce_method": reduce_method, + "used_names": [str(r.get("name")) for r in used], + "rejected_names": [str(r.get("name")) for r in rejected], + "patch_type": str(patch_type).lower(), + } + + return stats, quality + + def aggregate_patch_metrics(self, patch_results: list[dict]) -> dict: + valid = [p for p in patch_results if p["stats"].get("valid")] + + if not valid: + return { + "valid": False, + "source": "reference_patches", + "patches": patch_results, + "mean": 0.0, + "p50": 0.0, + "p95": 0.0, + "sat_pct": 0.0, + "dark_pct": 0.0, + "control_value": 0.0, + "target_value": self.target_value, + "weighted_error": 0.0, + "patch_quality": { + "valid": False, + "warnings": ["no_valid_patches"], + }, + } + + black = self._find_patch_result(valid, "black") + gray = self._find_patch_result(valid, "gray") + white = self._find_patch_result(valid, "white") + + warnings = [] + roi_quality_by_type = {} + + for p in valid: + ptype = str(p.get("type", "reference")).lower() + rq = p.get("roi_quality", {}) or {} + roi_quality_by_type[ptype] = rq + for warn in rq.get("warnings", []) or []: + warnings.append(f"{ptype}_{warn}") + + # Stats gerais de proteção. + p95s = np.array([p["stats"]["p95"] for p in valid], dtype=np.float32) + sats = np.array([p["stats"]["sat_pct"] for p in valid], dtype=np.float32) + darks = np.array([p["stats"]["dark_pct"] for p in valid], dtype=np.float32) + means = np.array([p["stats"]["mean"] for p in valid], dtype=np.float32) + p50s = np.array([p["stats"]["p50"] for p in valid], dtype=np.float32) + + p95_max = float(np.max(p95s)) + sat_max = float(np.max(sats)) + dark_mean = float(np.mean(darks)) + mean_mean = float(np.mean(means)) + p50_mean = float(np.mean(p50s)) + + # Valores por patch, quando existem. + black_p50 = float(black["stats"]["p50"]) if black else None + gray_p50 = float(gray["stats"]["p50"]) if gray else None + white_p50 = float(white["stats"]["p50"]) if white else None + + black_target = float(black.get("target_value", 0.06)) if black else 0.06 + gray_target = float(gray.get("target_value", self.target_value)) if gray else self.target_value + white_target = float(white.get("target_value", 0.80)) if white else 0.80 + + # ============================================================ + # Validações de coerência dos cartões + # ============================================================ + + if gray is None: + warnings.append("missing_gray_patch") + + if self.patch_require_order and black and gray and white: + if not (black_p50 < gray_p50 < white_p50): + warnings.append( + f"patch_order_invalid: black={black_p50:.3f}, gray={gray_p50:.3f}, white={white_p50:.3f}" + ) + + if (gray_p50 - black_p50) < self.patch_min_separation: + warnings.append( + f"black_gray_separation_low: diff={gray_p50 - black_p50:.3f}" + ) + + if (white_p50 - gray_p50) < self.patch_min_separation: + warnings.append( + f"gray_white_separation_low: diff={white_p50 - gray_p50:.3f}" + ) + + if white: + white_sat = float(white["stats"]["sat_pct"]) + white_p95 = float(white["stats"]["p95"]) + if white_sat > self.patch_white_sat_limit_pct: + warnings.append(f"white_patch_saturated: sat={white_sat:.2f}%") + if white_p95 > self.patch_white_p95_limit: + warnings.append(f"white_patch_p95_high: p95={white_p95:.3f}") + + if black: + black_dark = float(black["stats"]["dark_pct"]) + if black_dark > self.patch_black_dark_limit_pct: + warnings.append(f"black_patch_too_dark: dark={black_dark:.1f}%") + if black_p50 > self.patch_black_max_p50: + warnings.append(f"black_patch_too_bright: p50={black_p50:.3f}") + + if gray: + if gray_p50 < self.patch_gray_min_p50: + warnings.append(f"gray_patch_too_dark: p50={gray_p50:.3f}") + if gray_p50 > self.patch_gray_max_p50: + warnings.append(f"gray_patch_too_bright: p50={gray_p50:.3f}") + + # ============================================================ + # Modo recomendado: cinza como controle principal + # ============================================================ + if self.patch_control_mode == "gray_primary" and gray is not None: + control_value = gray_p50 + target_value = gray_target + weighted_error = target_value - control_value + + control_source = "gray_primary" + + else: + # Fallback: média ponderada original, mas preservando guardas. + weights = np.array([max(0.0, p.get("weight", 1.0)) for p in valid], dtype=np.float32) + + if float(weights.sum()) <= 1e-9: + weights = np.ones(len(valid), dtype=np.float32) + + weights = weights / weights.sum() + + patch_errors = [] + control_values = [] + + for p in valid: + target = p.get("target_value") + if target is None: + target = self.target_value + + value = p["stats"].get( + self.control_metric, + p["stats"].get("p50", p["stats"].get("mean", 0.0)) + ) + + control_values.append(float(value)) + patch_errors.append(float(target) - float(value)) + + weighted_error = float(np.sum(np.array(patch_errors, dtype=np.float32) * weights)) + control_value = float(np.sum(np.array(control_values, dtype=np.float32) * weights)) + target_value = self.target_value + control_source = "weighted_patches" + + # ============================================================ + # Guardas de saturação e faixa útil + # ============================================================ + # Se o branco saturou, queremos que o compute_control reduza exposição, + # mesmo que o cinza esteja aparentemente bom. + if white: + white_sat = float(white["stats"]["sat_pct"]) + white_p95 = float(white["stats"]["p95"]) + + sat_max = max(sat_max, white_sat) + p95_max = max(p95_max, white_p95) + + # Se o cinza está ausente, a métrica ainda pode funcionar por fallback, + # mas marcamos warning para debug. + quality_valid = gray is not None and len(warnings) == 0 + + return { + "valid": True, + "source": "reference_patches", + "patches": patch_results, + + # Métricas agregadas informativas. + "mean": mean_mean, + "p50": p50_mean, + "p95": p95_max, + "sat_pct": sat_max, + "dark_pct": dark_mean, + + # Métricas usadas pelo controle. + "control_metric": self.control_metric, + "control_value": float(control_value), + "target_value": float(target_value), + "weighted_error": float(weighted_error), + + # Debug/qualidade. + "patch_control_mode": self.patch_control_mode, + "control_source": control_source, + "patch_quality": { + "valid": bool(quality_valid), + "warnings": warnings, + "black_p50": black_p50, + "gray_p50": gray_p50, + "white_p50": white_p50, + "black_target": black_target, + "gray_target": gray_target, + "white_target": white_target, + "roi_quality_by_type": roi_quality_by_type, + }, + } + + def aggregate_spectral_metrics(self, role_items: dict) -> dict: + valid_items = {role: item for role, item in role_items.items() if item["metrics"].get("valid")} + if not valid_items: + return { + "valid": False, + "source": "spectral_shared", + "roles": role_items, + "mean": 0.0, + "p50": 0.0, + "p95": 0.0, + "sat_pct": 0.0, + "dark_pct": 0.0, + "control_value": 0.0, + "target_value": self.target_value, + } + + metrics_list = [item["metrics"] for item in valid_items.values()] + p95 = max(float(m.get("p95", 0.0)) for m in metrics_list) + sat_pct = max(float(m.get("sat_pct", 0.0)) for m in metrics_list) + mean = float(np.mean([float(m.get("mean", 0.0)) for m in metrics_list])) + p50 = float(np.mean([float(m.get("p50", m.get("mean", 0.0))) for m in metrics_list])) + dark_pct = float(np.mean([float(m.get("dark_pct", 0.0)) for m in metrics_list])) + control_values = [ + float(m.get("control_value", m.get(self.control_metric, m.get("p50", m.get("mean", 0.0))))) + for m in metrics_list + ] + + target_values = [ + float(m.get("target_value", self.target_value)) + for m in metrics_list + ] + + errors = [ + float(m.get("weighted_error", target - value)) + for m, target, value in zip(metrics_list, target_values, control_values) + ] + + control_value = float(np.mean(control_values)) + target_value = float(np.mean(target_values)) + weighted_error = float(np.mean(errors)) + + global_guards_by_role = { + role: item["metrics"].get("global_guard", {}) + for role, item in valid_items.items() + } + + return { + "valid": True, + "source": "spectral_shared", + "roles": role_items, + "mean": mean, + "p50": p50, + "p95": p95, + "sat_pct": sat_pct, + "dark_pct": dark_pct, + "control_metric": self.control_metric, + "control_value": control_value, + "target_value": target_value, + "weighted_error": weighted_error, + "global_guard": self._merge_global_guards(global_guards_by_role), + "control_values_by_role": { + role: float(item["metrics"].get("control_value", item["metrics"].get("p50", 0.0))) + for role, item in valid_items.items() + }, + "targets_by_role": { + role: float(item["metrics"].get("target_value", self.target_value)) + for role, item in valid_items.items() + }, + "patch_quality_by_role": { + role: item["metrics"].get("patch_quality", {}) + for role, item in valid_items.items() + }, + } + + def measure_global_guard(self, img_gray: np.ndarray, role: str = "rgb") -> dict: + """ + Mede risco global de saturação/sol direto na área útil da cena. + + Esta guarda não substitui os patches. Ela apenas detecta regiões + saturadas fora dos cartões, por exemplo uma chapa branca em sol direto + enquanto os patches permanecem na sombra. + """ + role = self._normalize_role(role) + + if not self.global_saturation_guard_enabled and not self.sun_guard_enabled: + return {"enabled": False, "role": role, "active": False, "severity": "none"} + + h, w = img_gray.shape[:2] + roi_pct = self._get_global_guard_roi_pct_for_role(role) + roi = self._roi_pct_to_pixels( + h, w, + roi_pct["x0"], + roi_pct["y0"], + roi_pct["x1"], + roi_pct["y1"], + ) + x0, y0, x1, y1 = roi + arr2d = np.asarray(img_gray[y0:y1, x0:x1], dtype=np.float32) + + if arr2d.size == 0: + return { + "enabled": True, + "role": role, + "active": False, + "severity": "none", + "valid": False, + "reason": "empty_roi", + "roi": list(roi), + "roi_pct": dict(roi_pct), + } + + flat = arr2d.reshape(-1) + sat_mask = arr2d >= self.global_guard_sat_threshold + near_mask = arr2d >= self.global_guard_near_sat_threshold + + sat_pct = float(sat_mask.mean() * 100.0) + near_sat_pct = float(near_mask.mean() * 100.0) + p95 = float(np.percentile(flat, 95)) + p99 = float(np.percentile(flat, 99)) + p999 = float(np.percentile(flat, 99.9)) + max_value = float(np.max(flat)) + + blob = self._largest_blob_pct(sat_mask) + largest_blob_pct = float(blob.get("largest_blob_pct", 0.0)) + + severity = "none" + exp_factor = 1.0 + guard_reasons = [] + + if self.global_saturation_guard_enabled: + if sat_pct >= self.global_guard_sat_pct_extreme or largest_blob_pct >= self.global_guard_blob_pct_extreme: + severity = "extreme" + exp_factor = self.global_guard_reduce_factor_extreme + guard_reasons.append("global_saturation_extreme") + elif sat_pct >= self.global_guard_sat_pct_hard or largest_blob_pct >= self.global_guard_blob_pct_hard: + severity = "hard" + exp_factor = self.global_guard_reduce_factor_hard + guard_reasons.append("global_saturation_hard") + elif sat_pct >= self.global_guard_sat_pct_soft or largest_blob_pct >= self.global_guard_blob_pct_soft: + severity = "soft" + exp_factor = self.global_guard_reduce_factor_soft + guard_reasons.append("global_saturation_soft") + + sun_active = False + if self.sun_guard_enabled: + sun_active = ( + p99 >= self.sun_guard_p99_threshold + or near_sat_pct >= self.sun_guard_near_sat_pct_threshold + or severity in ("soft", "hard", "extreme") + ) + if sun_active: + guard_reasons.append("sun_guard_active") + + return { + "enabled": True, + "valid": True, + "role": role, + "active": bool(severity != "none" or sun_active), + "severity": severity, + "sun_active": bool(sun_active), + "exp_factor": float(exp_factor), + "reason": "+".join(guard_reasons) if guard_reasons else "ok", + "roi": list(roi), + "roi_pct": dict(roi_pct), + "thresholds": { + "sat": self.global_guard_sat_threshold, + "near_sat": self.global_guard_near_sat_threshold, + "sun_p99": self.sun_guard_p99_threshold, + }, + "stats": { + "pixels": int(flat.size), + "p95": p95, + "p99": p99, + "p999": p999, + "max": max_value, + "sat_pct": sat_pct, + "near_sat_pct": near_sat_pct, + "largest_blob_px": int(blob.get("largest_blob_px", 0)), + "largest_blob_pct": largest_blob_pct, + "blob_count": int(blob.get("blob_count", 0)), + }, + } + + def _largest_blob_pct(self, mask: np.ndarray) -> dict: + """Calcula o maior blob saturado em porcentagem da ROI.""" + mask = np.asarray(mask, dtype=bool) + total_px = int(mask.size) + if total_px <= 0 or not bool(mask.any()): + return {"largest_blob_px": 0, "largest_blob_pct": 0.0, "blob_count": 0} + + h, w = mask.shape[:2] + max_side = max(h, w) + step = 1 + if self.global_guard_downsample_max_side > 0 and max_side > self.global_guard_downsample_max_side: + step = int(np.ceil(max_side / float(self.global_guard_downsample_max_side))) + mask_small = mask[::step, ::step] + else: + mask_small = mask + + hs, ws = mask_small.shape[:2] + visited = np.zeros(mask_small.shape, dtype=bool) + largest = 0 + blob_count = 0 + + ys, xs = np.nonzero(mask_small) + for sy, sx in zip(ys.tolist(), xs.tolist()): + if visited[sy, sx] or not mask_small[sy, sx]: + continue + + stack = [(sy, sx)] + visited[sy, sx] = True + area = 0 + + while stack: + cy, cx = stack.pop() + area += 1 + for ny in (cy - 1, cy, cy + 1): + if ny < 0 or ny >= hs: + continue + for nx in (cx - 1, cx, cx + 1): + if nx < 0 or nx >= ws or visited[ny, nx] or not mask_small[ny, nx]: + continue + visited[ny, nx] = True + stack.append((ny, nx)) + + estimated_area = int(area * step * step) + if estimated_area >= self.global_guard_min_blob_px: + blob_count += 1 + largest = max(largest, estimated_area) + + return { + "largest_blob_px": int(largest), + "largest_blob_pct": float((largest / max(1, total_px)) * 100.0), + "blob_count": int(blob_count), + } + + def _merge_global_guards(self, guards_by_role: dict) -> dict: + valid_guards = { + role: guard for role, guard in (guards_by_role or {}).items() + if isinstance(guard, dict) and guard.get("valid", False) + } + if not valid_guards: + return {"enabled": self.global_saturation_guard_enabled or self.sun_guard_enabled, "valid": False, "severity": "none", "sun_active": False} + + severity_rank = {"none": 0, "soft": 1, "hard": 2, "extreme": 3} + inv_rank = {v: k for k, v in severity_rank.items()} + max_rank = max(severity_rank.get(str(g.get("severity", "none")), 0) for g in valid_guards.values()) + + stats = { + "sat_pct": max(float(g.get("stats", {}).get("sat_pct", 0.0)) for g in valid_guards.values()), + "near_sat_pct": max(float(g.get("stats", {}).get("near_sat_pct", 0.0)) for g in valid_guards.values()), + "p95": max(float(g.get("stats", {}).get("p95", 0.0)) for g in valid_guards.values()), + "p99": max(float(g.get("stats", {}).get("p99", 0.0)) for g in valid_guards.values()), + "p999": max(float(g.get("stats", {}).get("p999", 0.0)) for g in valid_guards.values()), + "largest_blob_pct": max(float(g.get("stats", {}).get("largest_blob_pct", 0.0)) for g in valid_guards.values()), + "largest_blob_px": max(int(g.get("stats", {}).get("largest_blob_px", 0)) for g in valid_guards.values()), + "blob_count": sum(int(g.get("stats", {}).get("blob_count", 0)) for g in valid_guards.values()), + } + + factors = [float(g.get("exp_factor", 1.0)) for g in valid_guards.values() if str(g.get("severity", "none")) != "none"] + return { + "enabled": True, + "valid": True, + "role": "spectral_shared", + "active": bool(max_rank > 0 or any(bool(g.get("sun_active", False)) for g in valid_guards.values())), + "severity": inv_rank.get(max_rank, "none"), + "sun_active": any(bool(g.get("sun_active", False)) for g in valid_guards.values()), + "exp_factor": min(factors) if factors else 1.0, + "reason": "+".join(sorted(set(str(g.get("reason", "ok")) for g in valid_guards.values()))), + "stats": stats, + "by_role": valid_guards, + } + + def compute_stats(self, arr: np.ndarray) -> dict: + arr = np.asarray(arr, dtype=np.float32).reshape(-1) + if arr.size == 0: + return { + "valid": False, + "pixels": 0, + "mean": 0.0, + "std": 0.0, + "p05": 0.0, + "p50": 0.0, + "p95": 0.0, + "sat_pct": 0.0, + "dark_pct": 0.0, + } + return { + "valid": True, + "pixels": int(arr.size), + "mean": float(arr.mean()), + "std": float(arr.std()), + "p05": float(np.percentile(arr, 5)), + "p50": float(np.percentile(arr, 50)), + "p95": float(np.percentile(arr, 95)), + "sat_pct": float((arr >= 0.98).mean() * 100.0), + "dark_pct": float((arr <= 0.02).mean() * 100.0), + } + + @staticmethod + def _crop_array(img: np.ndarray, roi: tuple[int, int, int, int]) -> np.ndarray: + x0, y0, x1, y1 = roi + return img[y0:y1, x0:x1].reshape(-1) + + @staticmethod + def _roi_pct_to_pixels(h: int, w: int, x0_pct: float, y0_pct: float, x1_pct: float, y1_pct: float): + x0 = int(w * x0_pct) + x1 = int(w * x1_pct) + y0 = int(h * y0_pct) + y1 = int(h * y1_pct) + x0 = max(0, min(w - 1, x0)) + x1 = max(x0 + 1, min(w, x1)) + y0 = max(0, min(h - 1, y0)) + y1 = max(y0 + 1, min(h, y1)) + return x0, y0, x1, y1 + + @staticmethod + def _find_patch_result(patch_results: list[dict], patch_type: str): + patch_type = str(patch_type).lower() + for p in patch_results: + if str(p.get("type", "")).lower() == patch_type: + return p + return None + + def _guard_freeze_cycles_for_severity(self, severity: str) -> int: + severity = str(severity or "none").lower() + if severity == "extreme": + return int(self.guard_freeze_cycles_extreme) + if severity == "hard": + return int(self.guard_freeze_cycles_hard) + if severity == "soft": + return int(self.guard_freeze_cycles_soft) + return int(self.sun_guard_freeze_increase_cycles) + + def _guard_force_apply_for_severity(self, severity: str) -> bool: + if not self.guard_force_apply_enabled: + return False + severity = str(severity or "none").lower() + if severity == "extreme": + return bool(self.guard_force_apply_extreme) + if severity == "hard": + return bool(self.guard_force_apply_hard) + if severity == "soft": + return bool(self.guard_force_apply_soft) + return False + + def compute_control(self, role: str, metrics: dict, virtual_role: str | None = None) -> dict: + state_role = str(role).lower() + log_role = virtual_role or state_role + + st = self.state.setdefault(state_role, {"exp": 15000, "gain": 1.0}) + old_exp = int(st["exp"]) + old_gain = float(st["gain"]) + limits = self._limits_for_role(state_role) + + if not metrics.get("valid"): + ready, ready_cycles = self._update_ready_state( + log_role=log_role, + action="hold", + error=999.0, + p95=1.0, + sat_pct=100.0, + ) + return { + "role": log_role, + "state_role": state_role, + "action": "hold", + "reason": "métrica inválida", + "old_exp": old_exp, + "new_exp": old_exp, + "old_gain": old_gain, + "new_gain": old_gain, + "ready": ready, + "ready_cycles": ready_cycles, + "ready_required_cycles": self.ready_required_cycles, + } + + control_value = float(metrics.get( + "control_value", + metrics.get(self.control_metric, metrics.get("p50", metrics.get("mean", 0.0))) + )) + target = float(metrics.get("target_value", self.target_value)) + error = float(metrics.get("weighted_error", target - control_value)) + p95 = float(metrics.get("p95", 0.0)) + sat_pct = float(metrics.get("sat_pct", 0.0)) + global_guard = metrics.get("global_guard", {}) or {} + + guard_active = bool(global_guard.get("active", False)) + guard_severity = str(global_guard.get("severity", "none")).lower() + sun_active = bool(global_guard.get("sun_active", False)) + guard_stats = global_guard.get("stats", {}) or {} + guard_sat_pct = float(guard_stats.get("sat_pct", 0.0)) + guard_p99 = float(guard_stats.get("p99", 0.0)) + guard_near_sat_pct = float(guard_stats.get("near_sat_pct", 0.0)) + guard_blob_pct = float(guard_stats.get("largest_blob_pct", 0.0)) + + # A Sun Guard não manda na exposição sozinha: ela congela aumentos por + # alguns ciclos quando a cena útil parece estar sob sol/brilho forte. + # Reduções continuam permitidas para proteger contra estouro. + if sun_active: + freeze_cycles = self._guard_freeze_cycles_for_severity(guard_severity) + self._sun_guard_hold_cycles[log_role] = max( + self._sun_guard_hold_cycles.get(log_role, 0), + freeze_cycles, + ) + else: + self._sun_guard_hold_cycles[log_role] = max(0, self._sun_guard_hold_cycles.get(log_role, 0) - 1) + + sun_hold_cycles = self._sun_guard_hold_cycles.get(log_role, 0) + + # Para os contadores de pressão, a guarda global conta como sobreexposição + # quando há saturação real fora dos patches. + pressure_p95 = max(p95, float(guard_stats.get("p95", 0.0))) if guard_active else p95 + pressure_sat = max(sat_pct, guard_sat_pct) if guard_active else sat_pct + + under_cycles, over_cycles = self._update_exposure_pressure_state( + log_role=log_role, + error=error, + p95=pressure_p95, + sat_pct=pressure_sat, + ) + + exp_min = int(limits["exp_min_us"]) + exp_max = int(limits["exp_max_us"]) + gain_min = float(limits["gain_min"]) + gain_max = float(limits["gain_max"]) + + new_exp = old_exp + new_gain = old_gain + action = "hold" + reason = "dentro da faixa morta" + + ratio = None + factor = 1.0 + gain_policy = "hold" + force_apply_exposure = False + force_apply_gain = False + force_apply_reason = "" + + # ============================================================ + # 0) Global Saturation Guard: proteção de cena inteira. + # ============================================================ + if self.global_saturation_guard_enabled and guard_severity in ("soft", "hard", "extreme"): + exp_factor = float(global_guard.get("exp_factor", self.global_guard_reduce_factor_soft)) + exp_factor = self._clamp(exp_factor, 0.10, 1.0) + new_exp = int(self._clamp(old_exp * exp_factor, exp_min, exp_max)) + if self._guard_force_apply_for_severity(guard_severity): + force_apply_exposure = True + force_apply_reason = f"global_guard_{guard_severity}" + + if self.gain_reduce_on_saturation and old_gain > gain_min: + if self.gain_hard_reset_on_saturation and guard_severity == "extreme": + new_gain = gain_min + gain_policy = "hard_reset_gain_on_global_guard" + else: + desired_gain = old_gain - self.gain_step_down + new_gain = float(self._clamp(desired_gain, gain_min, gain_max)) + gain_policy = "decrease_gain_step_on_global_guard" + else: + new_gain = old_gain + gain_policy = "hold_gain" + + if new_gain != old_gain: + force_apply_gain = True + + action = "decrease_exposure" + reason = ( + f"global_saturation_guard:{guard_severity} " + f"sat={guard_sat_pct:.3f}% near={guard_near_sat_pct:.3f}% " + f"blob={guard_blob_pct:.3f}% p99={guard_p99:.3f} " + f"exp_factor={exp_factor:.3f} gain_policy={gain_policy}" + ) + + # ============================================================ + # 1) Proteção forte contra saturação / p95 alto dos patches. + # ============================================================ + elif sat_pct > self.saturation_limit_pct or p95 > self.p95_limit: + if sat_pct >= self.saturation_extreme_pct: + exp_factor = 0.45 + elif sat_pct >= self.saturation_hard_pct: + exp_factor = 0.32 + elif sat_pct > self.saturation_limit_pct: + exp_factor = 0.55 + else: + exp_factor = self.reduce_fast_factor + + new_exp = int(self._clamp(old_exp * exp_factor, exp_min, exp_max)) + if self.guard_force_apply_enabled and self.guard_force_apply_on_patch_saturation: + force_apply_exposure = True + force_apply_reason = "patch_saturation_or_p95" + + if self.gain_reduce_on_saturation and old_gain > gain_min: + if self.gain_hard_reset_on_saturation and sat_pct >= self.saturation_extreme_pct: + new_gain = gain_min + gain_policy = "hard_reset_gain_on_extreme_saturation" + else: + desired_gain = old_gain - self.gain_step_down + new_gain = float(self._clamp(desired_gain, gain_min, gain_max)) + gain_policy = "decrease_gain_step_on_saturation" + else: + new_gain = old_gain + gain_policy = "hold_gain" + + if new_gain != old_gain: + force_apply_gain = True + + action = "decrease_exposure" + reason = ( + f"saturação/p95 alto: sat={sat_pct:.2f}% p95={p95:.3f} " + f"exp_factor={exp_factor:.3f} gain_policy={gain_policy}" + ) + + # ============================================================ + # 2) Fora da faixa morta: controle por ratio/linear. + # ============================================================ + elif abs(error) > self.deadband: + if self.control_strategy == "ratio": + safe_value = max(control_value, 1e-6) + ratio = target / safe_value + ratio = self._clamp(ratio, self.ratio_min, self.ratio_max) + factor = 1.0 + self.ratio_alpha * (ratio - 1.0) + else: + factor = 1.0 + self.exp_step_gain * error + factor = max(self.factor_min, min(self.factor_max, factor)) + + if self.prefer_exposure: + # ---------------------------------------------------- + # 2A) Cena escura nos patches: subir exposição primeiro. + # A Sun Guard pode congelar aumentos se a cena útil está + # sob sol/brilho forte, mesmo com patches na sombra. + # ---------------------------------------------------- + if error > 0: + if self.sun_guard_enabled and sun_hold_cycles > 0: + new_exp = old_exp + new_gain = old_gain + factor = 1.0 + action = "hold" + gain_policy = "hold_gain_sun_guard" + reason = ( + f"sun_guard congelou aumento: cycles={sun_hold_cycles} " + f"value={control_value:.3f} target={target:.3f} " + f"p99={guard_p99:.3f} near={guard_near_sat_pct:.3f}% " + f"sat={guard_sat_pct:.3f}% blob={guard_blob_pct:.3f}%" + ) + else: + desired_exp = int(self._clamp(old_exp * factor, exp_min, exp_max)) + new_exp = desired_exp + new_gain = old_gain + gain_policy = "hold_gain_prefer_exposure" + + exp_high_threshold = int(exp_max * self.exp_high_ratio_for_gain) + + if ( + desired_exp >= exp_high_threshold + and under_cycles >= self.gain_increase_required_cycles + ): + desired_gain = old_gain + self.gain_step_up + new_gain = float(self._clamp(desired_gain, gain_min, gain_max)) + gain_policy = f"increase_gain_slow_under_cycles_{under_cycles}" + else: + new_gain = old_gain + gain_policy = f"hold_gain_under_cycles_{under_cycles}" + + action = "increase_exposure" + reason = ( + f"subindo exposição por {self.control_metric}: " + f"value={control_value:.3f} target={target:.3f} " + f"error={error:.3f} factor={factor:.3f} gain_policy={gain_policy}" + ) + + # ---------------------------------------------------- + # 2B) Cena clara: reduzir normalmente. + # ---------------------------------------------------- + else: + desired_exp = int(self._clamp(old_exp * factor, exp_min, exp_max)) + new_exp = desired_exp + + if ( + self.gain_return_enabled + and old_gain > gain_min + and over_cycles >= self.gain_decrease_required_cycles + ): + desired_gain = old_gain - self.gain_step_down + new_gain = float(self._clamp(desired_gain, gain_min, gain_max)) + gain_policy = f"return_gain_step_over_cycles_{over_cycles}" + else: + new_gain = old_gain + gain_policy = f"hold_gain_over_cycles_{over_cycles}" + + action = "decrease_exposure" + reason = ( + f"reduzindo brilho por {self.control_metric}: " + f"value={control_value:.3f} target={target:.3f} " + f"error={error:.3f} factor={factor:.3f} gain_policy={gain_policy}" + ) + + else: + if self.sun_guard_enabled and error > 0 and sun_hold_cycles > 0: + new_gain = old_gain + action = "hold" + gain_policy = "hold_gain_sun_guard" + reason = ( + f"sun_guard congelou aumento de ganho: cycles={sun_hold_cycles} " + f"value={control_value:.3f} target={target:.3f}" + ) + else: + desired_gain = old_gain * factor + new_gain = float(self._clamp(desired_gain, gain_min, gain_max)) + action = "increase_gain" if error > 0 else "decrease_gain" + gain_policy = "direct_gain_control" + reason = ( + f"corrigindo ganho por {self.control_metric}: " + f"value={control_value:.3f} target={target:.3f} " + f"error={error:.3f} factor={factor:.3f}" + ) + + # ============================================================ + # 3) Dentro da faixa morta: opcionalmente devolver ganho. + # ============================================================ + else: + if self.gain_return_enabled and old_gain > gain_min: + exp_low_threshold = int(exp_max * self.exp_low_ratio_for_gain_return) + + if old_exp < exp_low_threshold: + new_gain = float(self._clamp(old_gain * self.gain_return_factor, gain_min, gain_max)) + gain_policy = "return_gain_while_ready" + action = "decrease_gain" + reason = ( + f"dentro da faixa, devolvendo ganho: " + f"value={control_value:.3f} target={target:.3f} " + f"gain={old_gain:.2f}->{new_gain:.2f}" + ) + else: + gain_policy = "hold_gain_high_exp" + else: + gain_policy = "hold_gain" + + new_exp = int(self._clamp(new_exp, exp_min, exp_max)) + new_gain = float(self._clamp(new_gain, gain_min, gain_max)) + + if ( + self.guard_reapply_min_exp_on_emergency + and force_apply_exposure + and action == "decrease_exposure" + and new_exp <= (exp_min + self.guard_min_exp_margin_us) + ): + # Mesmo que old_exp == new_exp, reenviar o mínimo protege contra + # latência/dessincronia entre estado interno e exposição real da câmera. + force_apply_exposure = True + if not force_apply_reason: + force_apply_reason = "emergency_reapply_min_exp" + + ready, ready_cycles = self._update_ready_state( + log_role=log_role, + action=action, + error=error, + p95=max(p95, float(guard_stats.get("p95", 0.0))) if guard_active else p95, + sat_pct=max(sat_pct, guard_sat_pct) if guard_active else sat_pct, + ) + + return { + "role": log_role, + "state_role": state_role, + "action": action, + "reason": reason, + "metering_mode": self.metering_mode, + "spectral_control_mode": self.spectral_control_mode, + "control_metric": self.control_metric, + "control_value": control_value, + "target_value": target, + "error": error, + "p95": p95, + "sat_pct": sat_pct, + "global_guard": global_guard, + "sun_guard_hold_cycles": int(sun_hold_cycles), + "force_apply_exposure": bool(force_apply_exposure), + "force_apply_gain": bool(force_apply_gain), + "force_apply_reason": force_apply_reason, + "old_exp": old_exp, + "new_exp": new_exp, + "old_gain": old_gain, + "new_gain": new_gain, + "limits": limits, + "ready": ready, + "ready_cycles": ready_cycles, + "ready_required_cycles": self.ready_required_cycles, + "control_strategy": self.control_strategy, + "ratio": ratio, + "factor": float(factor), + "gain_policy": gain_policy, + } + + def should_apply_exposure(self, old_exp: int, new_exp: int) -> bool: + delta = abs(int(new_exp) - int(old_exp)) + + # Threshold menor quando a exposição está baixa. + relative_threshold = int(max(8, old_exp * 0.03)) + + # Mantém limite absoluto máximo para não ficar insensível. + threshold = min(self.exp_apply_threshold_us, relative_threshold) + + return delta >= threshold + + def apply_control(self, role: str, decision: dict): + role = str(role).lower() + new_exp = int(decision["new_exp"]) + new_gain = float(decision["new_gain"]) + force_apply_exposure = bool(decision.get("force_apply_exposure", False) or decision.get("force_apply", False)) + force_apply_gain = bool(decision.get("force_apply_gain", False)) + force_apply_reason = str(decision.get("force_apply_reason", "")) + + self.state[role]["exp"] = new_exp + self.state[role]["gain"] = new_gain + responses = {} + last = self._last_applied.setdefault(role, {"exp": None, "gain": None}) + try: + if role not in self._ae_disabled: + responses["ae"] = self.client.svc.set_ae_enable(role=role, enable=False) + if role == "rgb": + responses["awb"] = self.client.svc.set_awb_enable(role=role, enable=False) + self._ae_disabled.add(role) + if last["exp"] is None: + exp_delta_ok = True + else: + exp_delta_ok = self.should_apply_exposure(last["exp"], new_exp) + if force_apply_exposure or exp_delta_ok: + responses["exposure"] = self.client.svc.set_exposure_time(role=role, exposure_time_us=new_exp) + last["exp"] = new_exp + if force_apply_exposure: + responses["force_exposure"] = { + "enabled": True, + "reason": force_apply_reason, + "threshold_us": self.exp_apply_threshold_us, + } + else: + responses["exposure_skipped"] = { + "reason": "below_threshold", + "threshold_us": self.exp_apply_threshold_us, + "last_exp": last["exp"], + "new_exp": new_exp, + } + + gain_delta_ok = last["gain"] is None or abs(new_gain - last["gain"]) >= self.gain_apply_threshold + if force_apply_gain or gain_delta_ok: + responses["gain"] = self.client.svc.set_analogue_gain(role=role, analogue_gain=new_gain) + last["gain"] = new_gain + if force_apply_gain: + responses["force_gain"] = { + "enabled": True, + "reason": force_apply_reason, + "threshold": self.gain_apply_threshold, + } + except Exception as e: + responses["error"] = str(e) + if self.verbose: + print( + f"[RAD_APPLY] role={role} " + f"action={decision.get('action')} " + f"exp={decision.get('old_exp')}->{decision.get('new_exp')} " + f"gain={decision.get('old_gain'):.2f}->{decision.get('new_gain'):.2f} " + f"force_exp={bool(decision.get('force_apply_exposure', False))} " + f"force_reason={decision.get('force_apply_reason', '')} " + f"ok={'error' not in responses}" + ) + return responses + + def _limits_for_role(self, role: str) -> dict: + role_cfg = self.role_limits.get(role, {}) or {} + return { + "exp_min_us": int(role_cfg.get("exp_min_us", self.exp_min_us)), + "exp_max_us": int(role_cfg.get("exp_max_us", self.exp_max_us)), + "gain_min": float(role_cfg.get("gain_min", self.gain_min)), + "gain_max": float(role_cfg.get("gain_max", self.gain_max)), + } + + def _smooth_int(self, old, desired): + return int(round((1.0 - self.alpha) * old + self.alpha * desired)) + + def _smooth_float(self, old, desired): + return float((1.0 - self.alpha) * old + self.alpha * desired) + + @staticmethod + def _clamp(v, lo, hi): + return max(lo, min(hi, v)) + + def _print_metrics_debug(self, role: str, result: dict): + if not self.verbose: + return + + metrics = result.get("metrics", {}) + decision = result.get("decision", {}) + + print( + f"[RAD_METRICS] role={role} " + f"mode={result.get('mode')} " + f"metering={result.get('metering_mode')} " + f"action={decision.get('action')} " + f"exp={decision.get('old_exp')}->{decision.get('new_exp')} " + f"gain={decision.get('old_gain')}->{decision.get('new_gain')} " + f"control={decision.get('control_value'):.3f} " + f"target={decision.get('target_value'):.3f} " + f"p95={decision.get('p95'):.3f} " + f"sat={decision.get('sat_pct'):.2f}%" + ) + + guard = decision.get("global_guard") or metrics.get("global_guard") or {} + if guard.get("active") or guard.get("sun_active"): + gst = guard.get("stats", {}) or {} + print( + f" [GLOBAL_GUARD] role={guard.get('role', role)} " + f"severity={guard.get('severity', 'none')} " + f"sun={bool(guard.get('sun_active', False))} " + f"reason={guard.get('reason', 'ok')} " + f"p99={gst.get('p99', 0):.3f} " + f"sat={gst.get('sat_pct', 0):.3f}% " + f"near={gst.get('near_sat_pct', 0):.3f}% " + f"blob={gst.get('largest_blob_pct', 0):.3f}% " + f"hold_cycles={decision.get('sun_guard_hold_cycles', 0)}" + ) + + # Caso normal: rgb individual + patches = metrics.get("patches", []) + if patches: + for p in patches: + st = p.get("stats", {}) + print( + f" [PATCH] role={p.get('role', role)} " + f"type={p.get('type')} " + f"roi_source={p.get('roi_source')} " + f"roi_count={p.get('roi_valid_count', p.get('roi_count', 1))}/{p.get('roi_count', 1)} " + f"roi_pct={p.get('roi_pct')} " + f"p50={st.get('p50', 0):.3f} " + f"p95={st.get('p95', 0):.3f} " + f"sat={st.get('sat_pct', 0):.2f}% " + f"dark={st.get('dark_pct', 0):.1f}%" + ) + + # Caso spectral_shared: RE/NIR agregados + roles = metrics.get("roles", {}) + if roles: + for r, item in roles.items(): + m = item.get("metrics", {}) + print( + f" [ROLE_METRICS] role={r} " + f"cam_id={item.get('cam_id')} " + f"control={m.get('control_value', 0):.3f} " + f"target={m.get('target_value', 0):.3f} " + f"p95={m.get('p95', 0):.3f} " + f"sat={m.get('sat_pct', 0):.2f}% " + f"warnings={m.get('patch_quality', {}).get('warnings', [])}" + ) + + for p in m.get("patches", []): + st = p.get("stats", {}) + print( + f" [PATCH] role={p.get('role', r)} " + f"type={p.get('type')} " + f"roi_source={p.get('roi_source')} " + f"roi_pct={p.get('roi_pct')} " + f"p50={st.get('p50', 0):.3f} " + f"p95={st.get('p95', 0):.3f} " + f"sat={st.get('sat_pct', 0):.2f}% " + f"dark={st.get('dark_pct', 0):.1f}%" + ) diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/oak_fcc3_core/raw_processor_core.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/oak_fcc3_core/raw_processor_core.py new file mode 100644 index 000000000..439adfca7 --- /dev/null +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/oak_fcc3_core/raw_processor_core.py @@ -0,0 +1,4808 @@ +import json +import os +import time +import cv2 +import numpy as np +import math +from typing import Optional + + +try: + import numba as _numba + _HAS_NUMBA = True +except Exception: + _numba = None + _HAS_NUMBA = False + +if _HAS_NUMBA: + + @_numba.njit(cache=True, fastmath=True) + def _raw10_get_pixel_numba(row, x): + group = (x // 4) * 5 + idx = x & 3 + + b = row[group + idx] + b4 = row[group + 4] + + if idx == 0: + low = b4 & 0x03 + elif idx == 1: + low = (b4 >> 2) & 0x03 + elif idx == 2: + low = (b4 >> 4) & 0x03 + else: + low = (b4 >> 6) & 0x03 + + return (int(b) << 2) | int(low) + + + @_numba.njit(cache=True, fastmath=True, parallel=True) + def _raw10_mono_to_float32_numba(packed, out, width, height, scale): + groups_per_row = width // 4 + + for y in _numba.prange(height): + row = packed[y] + for g in range(groups_per_row): + base = g * 5 + x = g * 4 + + b0 = int(row[base + 0]) + b1 = int(row[base + 1]) + b2 = int(row[base + 2]) + b3 = int(row[base + 3]) + b4 = int(row[base + 4]) + + p0 = (b0 << 2) | ((b4 >> 0) & 0x03) + p1 = (b1 << 2) | ((b4 >> 2) & 0x03) + p2 = (b2 << 2) | ((b4 >> 4) & 0x03) + p3 = (b3 << 2) | ((b4 >> 6) & 0x03) + + out[y, x + 0] = p0 * scale + out[y, x + 1] = p1 * scale + out[y, x + 2] = p2 * scale + out[y, x + 3] = p3 * scale + + + @_numba.njit(cache=True, fastmath=True, parallel=True) + def _raw10_bayer_planes_to_rgb_numba( + packed, + out_rgb, + width, + height, + scale, + pattern_code, + gain_r, + gain_g, + gain_b, + ): + """ + packed: 2D uint8 RAW10 packed, shape=(height, packed_width) + out_rgb: 3D float32, shape=(height//2, width//2, 3) + + pattern_code: + 0 = RGGB + 1 = BGGR + 2 = GRBG + 3 = GBRG + """ + out_h = height // 2 + out_w = width // 2 + + for oy in _numba.prange(out_h): + y0 = oy * 2 + y1 = y0 + 1 + row0 = packed[y0] + row1 = packed[y1] + + for ox in range(out_w): + x0 = ox * 2 + x1 = x0 + 1 + + ee = _raw10_get_pixel_numba(row0, x0) * scale + eo = _raw10_get_pixel_numba(row0, x1) * scale + oe = _raw10_get_pixel_numba(row1, x0) * scale + oo = _raw10_get_pixel_numba(row1, x1) * scale + + if pattern_code == 0: # RGGB + r = ee + g = (eo + oe) * 0.5 + b = oo + elif pattern_code == 1: # BGGR + b = ee + g = (eo + oe) * 0.5 + r = oo + elif pattern_code == 2: # GRBG + g = (ee + oo) * 0.5 + r = eo + b = oe + else: # GBRG + g = (ee + oo) * 0.5 + b = eo + r = oe + + r *= gain_r + g *= gain_g + b *= gain_b + + # Clip manual barato. + if r < 0.0: + r = 0.0 + elif r > 1.0: + r = 1.0 + + if g < 0.0: + g = 0.0 + elif g > 1.0: + g = 1.0 + + if b < 0.0: + b = 0.0 + elif b > 1.0: + b = 1.0 + + out_rgb[oy, ox, 0] = r + out_rgb[oy, ox, 1] = g + out_rgb[oy, ox, 2] = b + + + @_numba.njit(cache=True, fastmath=True, parallel=True) + def _raw10_to_raw16_numba(packed, out, width, height): + groups_per_row = width // 4 + + for y in _numba.prange(height): + row = packed[y] + + for g in range(groups_per_row): + base = g * 5 + x = g * 4 + + b0 = int(row[base + 0]) + b1 = int(row[base + 1]) + b2 = int(row[base + 2]) + b3 = int(row[base + 3]) + b4 = int(row[base + 4]) + + out[y, x + 0] = (b0 << 2) | ((b4 >> 0) & 0x03) + out[y, x + 1] = (b1 << 2) | ((b4 >> 2) & 0x03) + out[y, x + 2] = (b2 << 2) | ((b4 >> 4) & 0x03) + out[y, x + 3] = (b3 << 2) | ((b4 >> 6) & 0x03) + + + @_numba.njit(cache=True, fastmath=True, parallel=True) + def _rgb16_to_float32_gain_clip_numba(rgb16, out, height, width, scale, gain_r, gain_g, gain_b): + for y in _numba.prange(height): + for x in range(width): + r = float(rgb16[y, x, 0]) * scale * gain_r + g = float(rgb16[y, x, 1]) * scale * gain_g + b = float(rgb16[y, x, 2]) * scale * gain_b + + if r < 0.0: + r = 0.0 + elif r > 1.0: + r = 1.0 + + if g < 0.0: + g = 0.0 + elif g > 1.0: + g = 1.0 + + if b < 0.0: + b = 0.0 + elif b > 1.0: + b = 1.0 + + out[y, x, 0] = r + out[y, x, 1] = g + out[y, x, 2] = b + + + @_numba.njit(cache=True, fastmath=True, parallel=True) + def _apply_flat_gain_numba(base, gain, out, height, width, clip_output): + for y in _numba.prange(height): + for x in range(width): + v = float(base[y, x]) * float(gain[y, x]) + + if clip_output: + if v < 0.0: + v = 0.0 + elif v > 1.0: + v = 1.0 + + out[y, x] = v + + + @_numba.njit(cache=True, fastmath=True, parallel=True) + def _apply_tensor_flat_gain_chw_numba(tensor, gain, channels, height, width, clip_output): + for c in _numba.prange(channels): + for y in range(height): + for x in range(width): + v = float(tensor[c, y, x]) * float(gain[c, y, x]) + + if clip_output: + if v < 0.0: + v = 0.0 + elif v > 1.0: + v = 1.0 + + tensor[c, y, x] = v + + + + +class RawProcessorCore: + def __init__(self, sensor_width: int, sensor_height: int, bayer_pattern: str = "BGGR", calibration_json_path=None): + self.sensor_width = sensor_width + self.sensor_height = sensor_height + self.bayer_pattern = bayer_pattern.upper() + self.rgb_processing_config = { + "mode": "linear_demosaic", # "linear_demosaic", "linear_demosaic_half" ou "bayer_planes" + "demosaic_algorithm": "ea", # "ea" ou "bilinear" + } + + self.fusion_config = { + "alignment_mode": "manual_affine", + "baseline_mm": 75.0, + "manual_offsets": { + "nir": {"dx": 0, "dy": 0, "theta_deg": 0.0}, + "re": {"dx": 0, "dy": 0, "theta_deg": 0.0}, + }, + "homographies": { + "nir_to_rgb": None, + "re_to_rgb": None, + }, + "crop_valid_common": True, + "resize_after_crop": True, + "target_size": None, + } + + self.rgb_calibration = { + "enabled": False, + "gains": { + "R": 1.0, + "G": 1.0, + "B": 1.0 + } + } + + self.calibration_json_path = calibration_json_path + self.calibration_base_dir = os.path.dirname(os.path.abspath(calibration_json_path)) if calibration_json_path else os.getcwd() + + self.flatfield_config = { + "enabled": False, + "npz_file": None, + "apply_before_fusion": True, + "apply_after_decode": True, + "apply_space": "native_camera_space", + "map_type": "gain", + "channels": ["R", "G", "B", "RE", "NIR"], + "channel_maps": {}, + "subtract_dark": False, + "clip_output": True, + } + self.flatfield_maps = {} + self.flatfield_loaded = False + + self.radiometric_normalization_config = { + "enabled": False, + "method": "oak_ae_frame_controls_v1", + "apply_stage": "after_dark_before_flat_gain", + + "control_source": "stream_meta.frame_controls", + "role_mapping_source": "camera_info", + + "factor_model": "exposure_time_us_x_iso", + "iso_base": 100.0, + + "reference_mode": "fixed", + "reference_controls": { + "rgb": { + "exposure_time_us": 10000, + "sensitivity_iso": 400, + }, + "re": { + "exposure_time_us": 15000, + "sensitivity_iso": 400, + }, + "nir": { + "exposure_time_us": 15000, + "sensitivity_iso": 400, + }, + }, + + "scale_limits": { + "default": { + "min": 0.15, + "max": 6.0, + }, + "rgb": { + "min": 0.15, + "max": 6.0, + }, + "re": { + "min": 0.15, + "max": 8.0, + }, + "nir": { + "min": 0.15, + "max": 8.0, + }, + }, + + "missing_controls_policy": "skip", + "invalid_controls_policy": "skip", + "clip_output": False, + "save_debug": True, + } + self.last_radiometric_normalization_result = None + + self.radiometric_config = {} + self.patch_normalization_config = { + "enabled": False, + "apply_when_metering_mode": "reference_patches", + "apply_stage": "after_fusion", + "method": "gray_scale_with_white_guard", + "space": "multispec_tensor", + "targets_by_patch_channel": { + "black": { + "R": 0.06, + "G": 0.06, + "B": 0.06, + "RE": 0.06, + "NIR": 0.06 + }, + "gray": { + "R": 0.34, + "G": 0.34, + "B": 0.34, + "RE": 0.24, + "NIR": 0.30 + }, + "white": { + "R": 0.78, + "G": 0.78, + "B": 0.78, + "RE": 0.78, + "NIR": 0.78 + } + }, + "white_guard_max": 0.92, + "white_guard_max_by_channel": { + "R": 0.92, + "G": 0.92, + "B": 0.92, + "RE": 0.88, + "NIR": 0.88 + }, + "scale_min": 0.35, + "scale_max": 2.50, + "clip_output": True, + "require_valid_gray": True, + "use_black_for_offset": False, + "save_patch_stats": True, + } + self.last_patch_normalization_result = None + self.last_frame_quality_result = None + self.last_fusion_result = None + self.camera_settings = {} + + self._flatfield_runtime_cache = {} + + self.last_decode_perf = {} + self._last_decode_perf_log_ts = 0.0 + + if calibration_json_path: + self.load_config_json(calibration_json_path) + + + def warmup_numba_raw10_decode(self): + if not _HAS_NUMBA: + return {"ok": False, "reason": "numba_not_available"} + + w, h = 1280, 800 + packed_w = int(np.ceil(w * 10 / 8)) + dummy = np.zeros((h, packed_w), dtype=np.uint8) + + _ = self._raw10_mono_to_float01_aggressive(dummy, w, h, 10) + _ = self._raw10_rgb_bayer_planes_to_rgb_float01_aggressive(dummy, w, h, "RGGB", 10) + _ = self._raw10_to_raw16_aggressive(dummy, w, h) + + raw16 = self._raw10_to_raw16_aggressive(dummy, w, h) + rgb16 = cv2.cvtColor(raw16, cv2.COLOR_BayerRG2RGB) + _ = self._rgb16_to_float32_gain_clip_aggressive(rgb16, 10) + + dummy_tensor = np.zeros((5, 640, 1024), dtype=np.float32) + dummy_gain = np.ones((5, 640, 1024), dtype=np.float32) + _apply_tensor_flat_gain_chw_numba(dummy_tensor, dummy_gain, 5, 640, 1024, True) + + return {"ok": True, "backend": "numba", "shape": [h, packed_w]} + + def unpack_raw10_packed( + self, + packed_frame: np.ndarray, + sensor_width: Optional[int] = None, + sensor_height: Optional[int] = None + ): + if packed_frame.ndim == 3 and packed_frame.shape[2] == 1: + packed_frame = packed_frame[:, :, 0] + + width = sensor_width if sensor_width is not None else self.sensor_width + height = sensor_height if sensor_height is not None else self.sensor_height + + if width % 4 != 0: + raise ValueError(f"Largura {width} não é múltipla de 4 para RAW10 packed") + + expected_packed_width = math.ceil(width * 10 / 8) + + actual_h, actual_w = packed_frame.shape[:2] + padding = actual_w - expected_packed_width + + if actual_h != height: + raise ValueError( + f"[ERRO FRAME] Altura packed inesperada: {packed_frame.shape}, " + f"esperado altura={height}" + ) + + if actual_w < expected_packed_width: + raise ValueError( + f"[ERRO FRAME] Largura packed menor que a útil esperada: {packed_frame.shape}, " + f"esperado pelo menos ({height}, {expected_packed_width})" + ) + + if padding > 64: + raise ValueError( + f"[ERRO FRAME] Padding excessivo no packed: {packed_frame.shape}, " + f"esperado útil ({height}, {expected_packed_width}), padding={padding}" + ) + + packed_frame = packed_frame[:, :expected_packed_width] + groups = packed_frame.reshape(height, width // 4, 5).astype(np.uint16) + + b0 = groups[:, :, 0] + b1 = groups[:, :, 1] + b2 = groups[:, :, 2] + b3 = groups[:, :, 3] + b4 = groups[:, :, 4] + + p0 = (b0 << 2) | ((b4 >> 0) & 0x03) + p1 = (b1 << 2) | ((b4 >> 2) & 0x03) + p2 = (b2 << 2) | ((b4 >> 4) & 0x03) + p3 = (b3 << 2) | ((b4 >> 6) & 0x03) + + raw16 = np.empty((height, width), dtype=np.uint16) + raw16[:, 0::4] = p0 + raw16[:, 1::4] = p1 + raw16[:, 2::4] = p2 + raw16[:, 3::4] = p3 + + return raw16 + + def extract_bayer_channels(self, raw16: np.ndarray) -> dict: + p = self.bayer_pattern.upper() + + if p == "RGGB": + r = raw16[0::2, 0::2] + g1 = raw16[0::2, 1::2] + g2 = raw16[1::2, 0::2] + b = raw16[1::2, 1::2] + + elif p == "BGGR": + b = raw16[0::2, 0::2] + g1 = raw16[0::2, 1::2] + g2 = raw16[1::2, 0::2] + r = raw16[1::2, 1::2] + + elif p == "GRBG": + g1 = raw16[0::2, 0::2] + r = raw16[0::2, 1::2] + b = raw16[1::2, 0::2] + g2 = raw16[1::2, 1::2] + + elif p == "GBRG": + g1 = raw16[0::2, 0::2] + b = raw16[0::2, 1::2] + r = raw16[1::2, 0::2] + g2 = raw16[1::2, 1::2] + + else: + raise ValueError(f"Padrão Bayer não suportado: {p}") + + return {"R": r, "G1": g1, "G2": g2, "B": b} + + def bayer_planes_to_rgb_linear( + self, + raw16: np.ndarray, + bit_depth: int = 10, + ) -> tuple[np.ndarray, np.ndarray, np.ndarray]: + ch = self.extract_bayer_channels(raw16) + + max_val = float((1 << bit_depth) - 1) + + r = ch["R"].astype(np.float32) / max_val + g = ((ch["G1"].astype(np.float32) + ch["G2"].astype(np.float32)) * 0.5) / max_val + b = ch["B"].astype(np.float32) / max_val + + return ( + np.clip(r, 0.0, 1.0), + np.clip(g, 0.0, 1.0), + np.clip(b, 0.0, 1.0), + ) + + def demosaic_raw16_to_rgb_linear( + self, + raw16: np.ndarray, + bit_depth: int = 10, + ) -> tuple[np.ndarray, np.ndarray, np.ndarray]: + + rgb_cfg = getattr(self, "rgb_processing_config", {}) or {} + algorithm = str(rgb_cfg.get("demosaic_algorithm", "ea")).lower() + + cv_code, _ = self._get_bayer_cv2_code( + bayer_pattern=self.bayer_pattern, + algorithm=algorithm, + ) + + raw16 = np.asarray(raw16) + if raw16.dtype != np.uint16: + raw16 = raw16.astype(np.uint16, copy=False) + + rgb16 = cv2.cvtColor(raw16, cv_code) + + rgb = rgb16.astype(np.float32) + rgb *= np.float32(1.0 / float((1 << bit_depth) - 1)) + np.clip(rgb, 0.0, 1.0, out=rgb) + + return rgb[:, :, 0], rgb[:, :, 1], rgb[:, :, 2] + + def build_training_rgb( + self, + raw16: np.ndarray, + output_dtype: str = "float32", + bit_depth: int = 10, + ) -> np.ndarray: + rgb_mode = str( + (getattr(self, "rgb_processing_config", {}) or {}).get("mode", "linear_demosaic") + ).lower() + + if rgb_mode in ("linear_demosaic", "demosaic", "full_res"): + r, g, b = self.demosaic_raw16_to_rgb_linear( + raw16, + bit_depth=bit_depth, + ) + + elif rgb_mode in ("bayer_planes", "bayer", "half_res"): + r, g, b = self.bayer_planes_to_rgb_linear( + raw16, + bit_depth=bit_depth, + ) + + else: + raise ValueError(f"rgb_processing.mode inválido: {rgb_mode}") + + rgb_cal = getattr(self, "rgb_calibration", {}) or {} + if rgb_cal.get("enabled", False): + gains = rgb_cal.get("gains", {}) or {} + r = r * float(gains.get("R", 1.0)) + g = g * float(gains.get("G", 1.0)) + b = b * float(gains.get("B", 1.0)) + + chw = np.stack([r, g, b], axis=0).astype(np.float32) + chw = np.clip(chw, 0.0, 1.0) + + if output_dtype == "float32": + return chw + + if output_dtype == "uint8": + return (chw * 255.0).clip(0, 255).astype(np.uint8) + + if output_dtype == "uint16": + return (chw * 65535.0).clip(0, 65535).astype(np.uint16) + + raise ValueError(f"output_dtype não suportado: {output_dtype}") + + def _channel_names_from_decoded(self, decoded): + names = ["R", "G", "B"] + + roles = { + data.get("role") or data.get("meta", {}).get("role"): cam_id + for cam_id, data in decoded.items() + } + + if "re" in roles: + names.append("RE") + if "nir" in roles: + names.append("NIR") + + return names + + def _find_cam_by_role(self, decoded, role): + role = str(role).lower() + + for cam_id, data in decoded.items(): + data_role = ( + data.get("role") or + data.get("meta", {}).get("role") or + "" + ) + if str(data_role).lower() == role: + return cam_id + + return None + + def decode_bins_cameras(self, bins_data, bins_meta): + decoded = {} + + for data, meta in zip(bins_data, bins_meta): + role = (meta.get("role") or "").strip().lower() + bit_depth = int(meta.get("bit_depth", 10)) + max_val = float((1 << bit_depth) - 1) + + cam_id = meta.get("cam_id") or meta.get("camera_id") or meta.get("id") or role + + if role == "rgb": + decoded[cam_id] = { + "name": "RGB", + "image": data.astype(np.float32) / max_val, + "meta": meta, + } + + elif role == "re": + decoded[cam_id] = { + "name": "RE", + "image": data.astype(np.float32) / max_val, + "meta": meta, + } + + elif role == "nir": + decoded[cam_id] = { + "name": "NIR", + "image": data.astype(np.float32) / max_val, + "meta": meta, + } + decoded[cam_id]["role"] = role + + return decoded + + def build_multispectral_tensor(self, bins_data, bins_meta, target_size=None): + decoded = self.decode_bins_cameras(bins_data, bins_meta) + + rgb_cam_id = self._find_cam_by_role(decoded, "rgb") + if rgb_cam_id is None: + raise RuntimeError("RGB obrigatório") + + channel_names = self._channel_names_from_decoded(decoded) + + tensor = self.fuse_multispec_cameras( + decoded, + meta=None, + channels_expected=len(channel_names), + ) + + tensor = self.resize_tensor_chw(tensor, target_size=target_size) + + if bool((self.patch_normalization_config or {}).get("enabled", False)): + tensor = self.apply_patch_normalization_to_tensor(tensor) + + self.last_frame_quality_result = self.evaluate_frame_quality(tensor) + + return tensor, channel_names + + def build_infer_tensor_from_stream(self, frame, meta, channels_expected, target_size=None): + frame_type = meta.get("frame_type") + + if frame_type == "RAW_BRUTO": + decoded = self.decode_stream_cameras(frame, meta) + tensor = self.fuse_multispec_cameras(decoded, meta, channels_expected) + tensor = self.resize_tensor_chw(tensor, target_size=target_size) + + # Sem cartões neste modo novo. + # patch_normalization deve ficar desligado no JSON. + # Se quiser manter compatibilidade futura, deixe gateado: + if bool((self.patch_normalization_config or {}).get("enabled", False)): + tensor = self.apply_patch_normalization_to_tensor(tensor) + + self.last_frame_quality_result = self.evaluate_frame_quality(tensor) + return tensor + + if frame_type in ("RGB", "MULTISPEC"): + if not isinstance(frame, np.ndarray): + raise RuntimeError(f"Frame {frame_type} esperado como ndarray") + + if frame.ndim != 3: + raise RuntimeError(f"Frame {frame_type} inválido: shape={frame.shape}") + + dtype_str = str(meta.get("dtype") or meta.get("output_dtype") or "float32").lower() + + if dtype_str == "uint8": + raw_np = frame.astype(np.float32) / 255.0 + elif dtype_str == "float32": + raw_np = frame.astype(np.float32) + elif dtype_str == "uint16": + raw_np = frame.astype(np.float32) / 65535.0 + else: + raise RuntimeError(f"dtype {frame_type} não suportado: {dtype_str}") + + if raw_np.shape[0] != channels_expected: + raise RuntimeError( + f"Frame {frame_type} com canais inesperados: " + f"{raw_np.shape[0]} | esperado={channels_expected}" + ) + + tensor = self.resize_tensor_chw(raw_np, target_size=target_size) + self.last_frame_quality_result = self.evaluate_frame_quality(tensor) + return tensor + + raise RuntimeError(f"frame_type não suportado para inferência: {frame_type}") + + 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 {} + camera_info = meta.get("camera_info", {}) or {} + + decoded = {} + + rgb_mode = str( + (getattr(self, "rgb_processing_config", {}) or {}).get("mode", "linear_demosaic") + ).lower() + + for cam_id, data in frame.items(): + cam_meta = camera_frames.get(cam_id) or camera_info.get(cam_id) or {} + + role = str(cam_meta.get("role", "")).lower() + if not role: + raise RuntimeError(f"Meta da câmera {cam_id} sem role. Esperado role='rgb', 'nir' ou 're'.") + + bit_depth = int(cam_meta.get("bit_depth", 10)) + raw_format = str(cam_meta.get("raw_format", "")).upper() + packed = bool(cam_meta.get("packed", False)) + is_raw10 = raw_format == "RAW10_PACKED" or packed or bit_depth == 10 + + if role == "rgb": + if is_raw10 and data.ndim == 2: + sensor_w = int(cam_meta.get("width", self.sensor_width)) + sensor_h = int(cam_meta.get("height", self.sensor_height)) + + bayer = ( + cam_meta.get("bayer_pattern") + or cam_meta.get("bayer") + or self.bayer_pattern + or "RGGB" + ) + + if rgb_mode in ("bayer_planes", "bayer", "half_res"): + rgb_hwc = self._raw10_rgb_bayer_planes_to_rgb_float01_aggressive( + data, + width=sensor_w, + height=sensor_h, + bayer_pattern=bayer, + bit_depth=bit_depth, + ) + + elif rgb_mode in ( + "linear_demosaic", + "demosaic", + "full_res", + "linear_demosaic_half", + "demosaic_half", + "full_demosaic_half", + ): + rgb_hwc = self._raw10_rgb_linear_demosaic_to_rgb_float01_fast( + data, + width=sensor_w, + height=sensor_h, + bayer_pattern=bayer, + bit_depth=bit_depth, + ) + + else: + raise ValueError(f"rgb_processing.mode inválido: {rgb_mode}") + + decoded[cam_id] = { + "name": "RGB", + "role": "rgb", + "image": rgb_hwc, + "meta": cam_meta, + } + + else: + # Caso preview/processado antigo: BGR HWC uint8. + if data.ndim != 3 or data.shape[2] != 3: + raise RuntimeError(f"{cam_id} RGB inválida: shape={data.shape}") + + rgb = data[:, :, ::-1].astype(np.float32) / 255.0 + + decoded[cam_id] = { + "name": "RGB", + "role": "rgb", + "image": np.clip(rgb, 0.0, 1.0), + "meta": cam_meta, + } + + elif role == "re": + decoded[cam_id] = { + "name": "RE", + "role": "re", + "image": self._decode_spectral_frame_to_float01(data, cam_meta), + "meta": cam_meta, + } + + elif role == "nir": + decoded[cam_id] = { + "name": "NIR", + "role": "nir", + "image": self._decode_spectral_frame_to_float01(data, cam_meta), + "meta": cam_meta, + } + + else: + raise RuntimeError(f"Role não suportada em {cam_id}: {role}") + + return decoded + + def _decode_spectral_frame_to_float01(self, data, cam_meta): + arr = data + + if arr.ndim == 3 and arr.shape[2] == 1: + arr = arr[:, :, 0] + + bit_depth = int(cam_meta.get("bit_depth", 8)) + raw_format = str(cam_meta.get("raw_format", "")).upper() + packed = bool(cam_meta.get("packed", False)) + channels = int(cam_meta.get("channels", 1)) if cam_meta.get("channels") is not None else 1 + + sensor_width = int(cam_meta.get("width", self.sensor_width)) + sensor_height = int(cam_meta.get("height", arr.shape[0])) + packed_width = int(cam_meta.get("packed_width", 0) or 0) + + looks_like_raw10_packed = ( + arr.ndim == 2 + and arr.dtype == np.uint8 + and arr.shape[0] == sensor_height + and ( + raw_format == "RAW10_PACKED" + or packed + or bit_depth == 10 + or (packed_width > 0 and arr.shape[1] == packed_width and packed_width != sensor_width) + or arr.shape[1] == int(sensor_width * 10 / 8) + ) + ) + + if looks_like_raw10_packed: + t_total0 = time.perf_counter() + + out = self._raw10_mono_to_float01_aggressive( + arr, + width=sensor_width, + height=sensor_height, + bit_depth=bit_depth, + ) + + role = str(cam_meta.get("role", "spectral")).lower() + + self._set_decode_perf(role, { + "mode": "mono_raw10_aggressive", + "width": int(sensor_width), + "height": int(sensor_height), + "numba": bool(_HAS_NUMBA), + "total_ms": (time.perf_counter() - t_total0) * 1000.0, + "out_shape": list(out.shape), + "out_dtype": str(out.dtype), + }) + + return out + + # Caso preview/processado: mono já vem uint8 normal. + if arr.ndim == 2 and arr.dtype == np.uint8: + return np.clip(arr.astype(np.float32) / 255.0, 0.0, 1.0) + + if arr.ndim == 2 and arr.dtype == np.uint16: + max_val = float((1 << bit_depth) - 1) if bit_depth > 0 and bit_depth <= 16 else 65535.0 + return np.clip(arr.astype(np.float32) / max_val, 0.0, 1.0) + + arr01 = arr.astype(np.float32) + if arr01.max() > 1.5: + arr01 /= 255.0 + + return np.clip(arr01, 0.0, 1.0) + + def fuse_multispec_cameras(self, decoded, meta, channels_expected): + t_total0 = time.perf_counter() + + t0 = time.perf_counter() + decoded = self.apply_dark_to_decoded(decoded) + t_dark_ms = (time.perf_counter() - t0) * 1000.0 + + t0 = time.perf_counter() + decoded = self.normalize_decoded_by_capture_controls(decoded, meta) + t_radnorm_ms = (time.perf_counter() - t0) * 1000.0 + + flat_cfg = self.flatfield_config or {} + flat_apply_space = str(flat_cfg.get("apply_space", "native_camera_space")).lower() + + apply_native_flat = ( + bool(flat_cfg.get("enabled", False)) + and flat_apply_space != "final_tensor_space" + ) + + t0 = time.perf_counter() + + if apply_native_flat: + decoded = self.apply_flat_gain_to_decoded(decoded) + + t_flat_native_ms = (time.perf_counter() - t0) * 1000.0 + + # Caminho espacial otimizado: direto para o target final. + tensor, direct_perf = self._fuse_multispec_direct_to_target_fast( + decoded, + meta, + channels_expected, + ) + + t_flat_ms = float(t_flat_native_ms + float(direct_perf.get("final_flat_ms", 0.0))) + + t0 = time.perf_counter() + if tensor.shape[0] != channels_expected: + raise RuntimeError( + f"Tensor fundido com canais inesperados: {tensor.shape[0]} | " + f"esperado={channels_expected}" + ) + + out = tensor.astype(np.float32, copy=False) + t_final_ms = (time.perf_counter() - t0) * 1000.0 + + t_total_ms = (time.perf_counter() - t_total0) * 1000.0 + + # Mantém contrato de perf do benchmark. + t_prepare_ms = float(direct_perf.get("prepare_ms", 0.0)) + t_rgb_ms = float(direct_perf.get("rgb_crop_resize_ms", 0.0)) + t_warp_total_ms = float(direct_perf.get("warp_total_ms", 0.0)) + warp_details = direct_perf.get("warp_details_ms", {}) or {} + + # Aqui crop_resize_ms representa somente RGB crop/resize no modo direto. + # O custo espacial total útil para analisar é: + # spatial_direct_ms = rgb_crop_resize_ms + warp_total_ms + t_crop_resize_ms = float(direct_perf.get("crop_resize_ms", t_rgb_ms)) + t_concat_ms = 0.0 + + if self.last_fusion_result is None: + self.last_fusion_result = {} + + self.last_fusion_result["roles"] = ["R", "G", "B", "RE", "NIR"] + self.last_fusion_result["output_shape"] = list(out.shape) + self.last_fusion_result["perf"] = { + "dark_ms": t_dark_ms, + "radnorm_ms": t_radnorm_ms, + "flat_ms": t_flat_ms, + "prepare_ms": t_prepare_ms, + "rgb_crop_resize_ms": t_rgb_ms, + "warp_total_ms": t_warp_total_ms, + "warp_details_ms": warp_details, + "crop_resize_ms": t_crop_resize_ms, + "concat_ms": t_concat_ms, + "spatial_direct_ms": float(t_rgb_ms + t_warp_total_ms), + "final_ms": t_final_ms, + "total_ms": t_total_ms, + "decode_perf": getattr(self, "last_decode_perf", {}), + } + + #if not hasattr(self, "_last_perf_log_ts"): + # self._last_perf_log_ts = 0.0 + #now = time.time() + #if now - self._last_perf_log_ts >= 1.0: + # self._last_perf_log_ts = now + # print( + # "[PERF][CORE_FUSE] " + # f"dark={t_dark_ms:.1f}ms " + # f"radnorm={t_radnorm_ms:.1f}ms " + # f"flat={t_flat_ms:.1f}ms " + # f"prepare={t_prepare_ms:.1f}ms " + # f"rgb_resize={t_rgb_ms:.1f}ms " + # f"warp={t_warp_total_ms:.1f}ms " + # f"warp_re={warp_details.get('re', -1):.1f}ms " + # f"warp_nir={warp_details.get('nir', -1):.1f}ms " + # f"spatial={t_rgb_ms + t_warp_total_ms:.1f}ms " + # f"crop_resize={t_crop_resize_ms:.1f}ms " + # f"concat={t_concat_ms:.1f}ms " + # f"final={t_final_ms:.1f}ms " + # f"total={t_total_ms:.1f}ms " + # f"shape={out.shape}" + # ) + + #if not hasattr(self, "_last_remap_perf_log_ts"): + # self._last_remap_perf_log_ts = 0.0 + #now = time.time() + #if now - self._last_remap_perf_log_ts >= 1.0: + # self._last_remap_perf_log_ts = now + # print( + # "[PERF][REMAP] " + # f"enabled={direct_perf.get('remap_enabled')} " + # f"hit={direct_perf.get('remap_cache_hit')} " + # f"cache={direct_perf.get('remap_cache_ms', -1):.2f}ms " + # f"rgb={direct_perf.get('rgb_crop_resize_ms', -1):.2f}ms " + # f"warp={direct_perf.get('warp_total_ms', -1):.2f}ms " + # f"re={(direct_perf.get('warp_details_ms') or {}).get('re', -1):.2f}ms " + # f"nir={(direct_perf.get('warp_details_ms') or {}).get('nir', -1):.2f}ms " + # f"spatial={direct_perf.get('spatial_direct_ms', -1):.2f}ms " + # f"hits={direct_perf.get('remap_cache_hits')} " + # f"misses={direct_perf.get('remap_cache_misses')}" + # ) + + return out + + def _shift_image(self, img, dx, dy): + h, w = img.shape[:2] + M = np.float32([[1, 0, dx], [0, 1, dy]]) + return cv2.warpAffine( + img, M, (w, h), + flags=cv2.INTER_LINEAR, + borderMode=cv2.BORDER_CONSTANT, + borderValue=0 + ) + + def _affine_image(self, img, dx, dy, theta_deg): + 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 + + return cv2.warpAffine( + img, + M, + (w, h), + flags=cv2.INTER_LINEAR, + borderMode=cv2.BORDER_CONSTANT, + borderValue=0 + ) + + def _warp_with_valid_mask(self, img, role, ref_shape, meta): + ref_h, ref_w = ref_shape + + if img.shape[:2] != (ref_h, ref_w): + img = cv2.resize(img, (ref_w, ref_h), interpolation=cv2.INTER_LINEAR) + + cfg = self.fusion_config + mode = cfg.get("alignment_mode", "identity") + + mask = np.ones((ref_h, ref_w), dtype=np.uint8) * 255 + + if mode == "identity": + warped = img + warped_mask = mask + + elif mode == "manual_offset": + offs = cfg.get("manual_offsets", {}).get(role, {}) + dx = int(offs.get("dx", 0)) + dy = int(offs.get("dy", 0)) + + warped = self._shift_image(img, dx, dy) + warped_mask = self._shift_image(mask, dx, dy) + + elif mode == "manual_affine": + offs = cfg.get("manual_offsets", {}).get(role, {}) + dx = int(offs.get("dx", 0)) + dy = int(offs.get("dy", 0)) + theta_deg = float(offs.get("theta_deg", 0.0)) + + warped = self._affine_image(img, dx, dy, theta_deg) + warped_mask = self._affine_image(mask, dx, dy, theta_deg) + + elif mode == "homography": + H = cfg.get("homographies", {}).get(f"{role}_to_rgb") + + if H is None: + raise RuntimeError( + f"fusion_config.alignment_mode='homography', " + f"mas homografia '{role}_to_rgb' está ausente. " + f"Isso deixaria o canal {role.upper()} sem alinhamento." + ) + + calib_size = cfg.get("homography_calibration_size", None) + + H = self._scale_homography_to_runtime( + H, + calib_size=calib_size, + runtime_size=(ref_w, ref_h), + ) + + if H.shape != (3, 3): + raise RuntimeError(f"Homografia inválida para {role}: shape={H.shape}") + + warped = cv2.warpPerspective( + img, H, (ref_w, ref_h), + flags=cv2.INTER_LINEAR, + borderMode=cv2.BORDER_CONSTANT, + borderValue=0 + ) + + warped_mask = cv2.warpPerspective( + mask, H, (ref_w, ref_h), + flags=cv2.INTER_NEAREST, + borderMode=cv2.BORDER_CONSTANT, + borderValue=0 + ) + + else: + raise RuntimeError(f"alignment_mode inválido: {mode}") + + warped_mask = (warped_mask > 0).astype(np.uint8) + return warped, warped_mask + + def _compute_common_crop_box(self, masks): + if not masks: + return None + + common = masks[0].copy() + for m in masks[1:]: + common = np.logical_and(common > 0, m > 0) + + ys, xs = np.where(common) + if len(xs) == 0 or len(ys) == 0: + return None + + x0 = int(xs.min()) + x1 = int(xs.max()) + 1 + y0 = int(ys.min()) + y1 = int(ys.max()) + 1 + + return x0, y0, x1, y1 + + def _crop_and_resize_channels(self, channels, crop_box, ref_shape): + x0, y0, x1, y1 = crop_box + ref_h, ref_w = ref_shape + + cropped = [ch[:, y0:y1, x0:x1] for ch in channels] + + cfg = self.fusion_config + if not cfg.get("resize_after_crop", False): + return cropped + + target_size = cfg.get("target_size", None) + if target_size is None: + target_w, target_h = ref_w, ref_h + else: + target_w, target_h = target_size + + resized = [] + for ch in cropped: + ch_resized = np.stack([ + cv2.resize( + ch_i, + (target_w, target_h), + interpolation=cv2.INTER_LINEAR + ) + for ch_i in ch + ], axis=0) + resized.append(ch_resized) + + return resized + + def resize_tensor_chw(self, tensor, target_size=None): + if target_size is None: + return tensor + + target_w, target_h = target_size + + if tensor.ndim != 3: + raise RuntimeError(f"Tensor esperado em CHW. Veio shape={tensor.shape}") + + _, h, w = tensor.shape + + if (w, h) == (target_w, target_h): + return tensor.astype(np.float32, copy=False) + + interp = cv2.INTER_AREA if target_w < w or target_h < h else cv2.INTER_LINEAR + + chans = [] + for ch in tensor: + ch_res = cv2.resize(ch, (target_w, target_h), interpolation=interp) + chans.append(ch_res.astype(np.float32)) + + return np.stack(chans, axis=0) + + def _scale_homography_to_runtime(self, H, calib_size, runtime_size): + """ + Ajusta uma homografia calculada em calib_size para ser aplicada em runtime_size. + + H original: + ponto_spec_calib -> ponto_rgb_calib + + H runtime: + ponto_spec_runtime -> ponto_rgb_runtime + """ + if H is None: + return None + + H = np.asarray(H, dtype=np.float32) + + if calib_size is None: + return H + + calib_w, calib_h = calib_size + runtime_w, runtime_h = runtime_size + + calib_w = float(calib_w) + calib_h = float(calib_h) + runtime_w = float(runtime_w) + runtime_h = float(runtime_h) + + if calib_w <= 0 or calib_h <= 0: + return H + + sx = runtime_w / calib_w + sy = runtime_h / calib_h + + S = np.array([ + [sx, 0.0, 0.0], + [0.0, sy, 0.0], + [0.0, 0.0, 1.0], + ], dtype=np.float32) + + S_inv = np.array([ + [1.0 / sx, 0.0, 0.0], + [0.0, 1.0 / sy, 0.0], + [0.0, 0.0, 1.0], + ], dtype=np.float32) + + H_runtime = S @ H @ S_inv + + if abs(H_runtime[2, 2]) > 1e-9: + H_runtime = H_runtime / H_runtime[2, 2] + + return H_runtime.astype(np.float32) + + + def apply_patch_normalization_to_tensor(self, tensor: np.ndarray) -> np.ndarray: + self.last_patch_normalization_result = None + + cfg = self.patch_normalization_config or {} + + result = { + "enabled": bool(cfg.get("enabled", False)), + "applied": False, + "method": cfg.get("method", "gray_scale_with_white_guard"), + "space": cfg.get("space", "multispec_tensor"), + "warnings": [], + "scales": {}, + "patch_stats": {}, + "channel_debug": {}, + "tensor_stats_before": {}, + "tensor_stats_after": {}, + "summary": {}, + } + + if not cfg.get("enabled", False): + result["warnings"].append("patch_normalization_disabled") + self.last_patch_normalization_result = result + return tensor + + rad_cfg = self.radiometric_config or {} + + if cfg.get("apply_when_metering_mode") == "reference_patches": + if rad_cfg.get("metering_mode") != "reference_patches": + result["warnings"].append( + f"metering_mode_not_reference_patches: {rad_cfg.get('metering_mode')}" + ) + self.last_patch_normalization_result = result + return tensor + + if tensor is None or tensor.ndim != 3 or tensor.shape[0] < 5: + result["warnings"].append(f"invalid_tensor_shape: {None if tensor is None else tensor.shape}") + self.last_patch_normalization_result = result + return tensor + + patches = rad_cfg.get("reference_patches", []) or [] + patch_by_type = { + str(p.get("type", "")).lower(): p + for p in patches + if isinstance(p, dict) + } + + gray = patch_by_type.get("gray") + white = patch_by_type.get("white") + black = patch_by_type.get("black") + + if gray is None: + result["warnings"].append("missing_gray_patch") + if cfg.get("require_valid_gray", True): + self.last_patch_normalization_result = result + return tensor + + targets_by_patch_channel = self._resolve_targets_by_patch_channel(cfg, rad_cfg) + + scale_min = float(cfg.get("scale_min", 0.35)) + scale_max = float(cfg.get("scale_max", 2.50)) + white_guard_max_global = float(cfg.get("white_guard_max", 0.92)) + white_guard_by_channel = cfg.get("white_guard_max_by_channel", {}) or {} + clip_output = bool(cfg.get("clip_output", True)) + + channel_names = ["R", "G", "B", "RE", "NIR"] + + out = tensor.astype(np.float32).copy() + h, w = out.shape[1], out.shape[2] + result["tensor_stats_before"] = self._tensor_channel_stats(out, channel_names) + + + + # Guarda anti-roxo para normalização por patches. + # Usa uma máscara comum RGB calculada antes de qualquer escala. + patch_sat_guard_enabled = bool(cfg.get("rgb_saturation_guard_enabled", True)) + patch_sat_guard_mode = str(cfg.get("rgb_saturation_guard_mode", "fade_strength")).lower() + patch_sat_soft_start = float(cfg.get("rgb_saturation_soft_start", 0.88)) + patch_sat_hard = float(cfg.get("rgb_saturation_hard", 0.97)) + patch_sat_threshold = float(cfg.get("rgb_saturation_threshold", 0.97)) + + rgb_original = out[:3].copy() if out.shape[0] >= 3 else None + rgb_sat_mask = None + rgb_strength_mask = None + + if patch_sat_guard_enabled and rgb_original is not None: + rgb_max = np.max(rgb_original, axis=0) + + rgb_sat_mask = rgb_max >= patch_sat_threshold + + denom = max(patch_sat_hard - patch_sat_soft_start, 1e-6) + t = (rgb_max - patch_sat_soft_start) / denom + t = np.clip(t, 0.0, 1.0) + + # 1.0 = aplica normalização normal + # 0.0 = preserva original + rgb_strength_mask = 1.0 - t + + result["rgb_saturation_guard"] = { + "enabled": True, + "mode": patch_sat_guard_mode, + "soft_start": patch_sat_soft_start, + "hard": patch_sat_hard, + "threshold": patch_sat_threshold, + "sat_pct": float(np.mean(rgb_sat_mask) * 100.0), + "mean_strength": float(np.mean(rgb_strength_mask)), + } + else: + result["rgb_saturation_guard"] = { + "enabled": False + } + + # Para o tensor final fusionado, a geometria de referência é o espaço do RGB. + # Como RE/NIR são alinhados por homografia para casar no RGB, as ROIs usadas + # na normalização final devem ser as ROIs da role rgb, com suporte a múltiplas + # ROIs por cor/cartão. + gray_rois = self._resolve_patch_tensor_rois(gray, w, h, reference_role="rgb") + white_rois = self._resolve_patch_tensor_rois(white, w, h, reference_role="rgb") + black_rois = self._resolve_patch_tensor_rois(black, w, h, reference_role="rgb") + + result["roi_source"] = "roi_list_by_role.rgb" + result["roi_counts"] = { + "black": len(black_rois), + "gray": len(gray_rois), + "white": len(white_rois), + } + + if not gray_rois: + result["warnings"].append("missing_or_invalid_gray_rois") + self.last_patch_normalization_result = result + return tensor + + for ci, ch_name in enumerate(channel_names): + ch = out[ci] + input_stats = self._array01_stats(ch) + + gray_target = float(targets_by_patch_channel["gray"][ch_name]) + white_target = float(targets_by_patch_channel["white"][ch_name]) + + # ----------------------------- + # Stats do gray + # ----------------------------- + gray_stats = self._measure_patch_rois_for_channel(ch, gray_rois) + + if not gray_stats: + result["warnings"].append(f"{ch_name}: empty_gray_rois") + continue + + result["patch_stats"].setdefault("gray", {})[ch_name] = gray_stats + gray_p50 = float(gray_stats["p50"]) + + if gray_p50 <= 1e-6: + result["warnings"].append(f"{ch_name}: gray_p50_too_low") + continue + + scale_raw = float(gray_target / gray_p50) + scale = scale_raw + scale_after_white_guard = float(scale) + + white_guard_ch = white_guard_max_global + if isinstance(white_guard_by_channel, dict) and ch_name in white_guard_by_channel: + try: + white_guard_ch = float(white_guard_by_channel[ch_name]) + except Exception: + pass + + # ----------------------------- + # Stats do white + guarda + # ----------------------------- + if white_rois: + white_stats = self._measure_patch_rois_for_channel(ch, white_rois) + + if white_stats: + result["patch_stats"].setdefault("white", {})[ch_name] = white_stats + + white_p50 = float(white_stats["p50"]) + white_sat = float(white_stats["sat_pct"]) + + if white_sat > 0.5: + result["warnings"].append(f"{ch_name}: white_patch_saturated_{white_sat:.2f}%") + + if white_p50 > 1e-6: + max_scale_by_white = white_guard_ch / white_p50 + if scale > max_scale_by_white: + result["warnings"].append( + f"{ch_name}: scale_limited_by_white_guard " + f"{scale:.3f}->{max_scale_by_white:.3f}" + ) + scale = min(scale, max_scale_by_white) + scale_after_white_guard = float(scale) + + # ----------------------------- + # Stats do black, só diagnóstico + # ----------------------------- + if black_rois: + black_stats = self._measure_patch_rois_for_channel(ch, black_rois) + if black_stats: + result["patch_stats"].setdefault("black", {})[ch_name] = black_stats + + scale_before_clip = float(scale) + scale = float(np.clip(scale, scale_min, scale_max)) + + if abs(scale - scale_before_clip) > 1e-6: + result["warnings"].append( + f"{ch_name}: scale_clipped {scale_before_clip:.3f}->{scale:.3f}" + ) + + scaled = ch * scale + output_before_clip_stats = self._array01_stats(scaled) + would_clip_low_pct = float((scaled < 0.0).mean() * 100.0) + would_clip_high_pct = float((scaled > 1.0).mean() * 100.0) + would_clip_pct = would_clip_low_pct + would_clip_high_pct + + if clip_output: + out_ch = np.clip(scaled, 0.0, 1.0) + else: + out_ch = scaled + + output_after_clip_stats = self._array01_stats(out_ch) + out[ci] = out_ch + + # Anti-roxo: não deixa patch_normalization recolorir pixels RGB saturados. + if ( + patch_sat_guard_enabled + and ci < 3 + and rgb_original is not None + and rgb_strength_mask is not None + ): + original_ch = rgb_original[ci] + + if patch_sat_guard_mode == "skip": + if rgb_sat_mask is not None: + out_ch = np.where(rgb_sat_mask, original_ch, out_ch) + + elif patch_sat_guard_mode == "fade_strength": + # Mistura entre canal original e canal normalizado. + # Em região normal: strength=1 -> usa out_ch. + # Em saturação: strength=0 -> preserva original. + s = rgb_strength_mask.astype(np.float32) + out_ch = original_ch * (1.0 - s) + out_ch * s + + result["scales"][ch_name] = { + "scale": scale, + "scale_raw_gray": scale_raw, + "scale_after_white_guard": scale_after_white_guard, + "scale_before_clip_limits": scale_before_clip, + "scale_min": scale_min, + "scale_max": scale_max, + "gray_target": gray_target, + "gray_measured_p50": gray_p50, + "white_target": white_target, + "white_guard_max": white_guard_ch, + "would_clip_pct": would_clip_pct, + "would_clip_high_pct": would_clip_high_pct, + "would_clip_low_pct": would_clip_low_pct, + } + + result["channel_debug"][ch_name] = { + "input_stats": input_stats, + "output_before_clip_stats": output_before_clip_stats, + "output_after_clip_stats": output_after_clip_stats, + "gray_target": gray_target, + "white_target": white_target, + "white_guard_max": white_guard_ch, + "gray_p50": gray_p50, + "scale_raw_gray": scale_raw, + "scale_after_white_guard": scale_after_white_guard, + "scale_before_clip_limits": scale_before_clip, + "scale_final": scale, + "would_clip_pct": would_clip_pct, + "would_clip_high_pct": would_clip_high_pct, + "would_clip_low_pct": would_clip_low_pct, + } + + if clip_output: + out = np.clip(out, 0.0, 1.0) + + result["tensor_stats_after"] = self._tensor_channel_stats(out, channel_names) + result["summary"] = self._summarize_patch_normalization_result(result) + + result["applied"] = True + result["valid"] = bool(len(result["scales"]) == len(channel_names)) + result["clip_output"] = clip_output + result["shape"] = list(out.shape) + result["channel_names"] = channel_names + + self.last_patch_normalization_result = result + return out.astype(np.float32, copy=False) + + def _array01_stats(self, arr: np.ndarray) -> dict: + """ + Estatísticas compactas para debug radiométrico de arrays float. + Mantém tudo serializável em JSON e leve o bastante para log por frame. + """ + if arr is None: + return {} + + vals = np.asarray(arr, dtype=np.float32).reshape(-1) + if vals.size <= 0: + return {} + + finite = vals[np.isfinite(vals)] + if finite.size <= 0: + return {"count": int(vals.size), "finite_count": 0} + + return { + "count": int(vals.size), + "finite_count": int(finite.size), + "min": float(np.min(finite)), + "p01": float(np.percentile(finite, 1)), + "p05": float(np.percentile(finite, 5)), + "p50": float(np.percentile(finite, 50)), + "p95": float(np.percentile(finite, 95)), + "p99": float(np.percentile(finite, 99)), + "max": float(np.max(finite)), + "mean": float(np.mean(finite)), + "std": float(np.std(finite)), + "sat_pct": float((finite >= 0.98).mean() * 100.0), + "dark_pct": float((finite <= 0.02).mean() * 100.0), + "over_1_pct": float((finite > 1.0).mean() * 100.0), + "under_0_pct": float((finite < 0.0).mean() * 100.0), + } + + def _tensor_channel_stats(self, tensor: np.ndarray, channel_names: list) -> dict: + if tensor is None or tensor.ndim != 3: + return {} + + stats = {} + for i, ch_name in enumerate(channel_names): + if i >= tensor.shape[0]: + break + stats[ch_name] = self._array01_stats(tensor[i]) + return stats + + def _summarize_patch_normalization_result(self, result: dict) -> dict: + scales = result.get("scales", {}) or {} + patch_stats = result.get("patch_stats", {}) or {} + + scale_values = [float(v.get("scale", 1.0)) for v in scales.values() if isinstance(v, dict)] + clip_values = [float(v.get("would_clip_pct", 0.0)) for v in scales.values() if isinstance(v, dict)] + + white_stats = patch_stats.get("white", {}) if isinstance(patch_stats, dict) else {} + gray_stats = patch_stats.get("gray", {}) if isinstance(patch_stats, dict) else {} + + max_white_sat = 0.0 + if isinstance(white_stats, dict): + vals = [float(v.get("sat_pct", 0.0)) for v in white_stats.values() if isinstance(v, dict)] + max_white_sat = max(vals) if vals else 0.0 + + max_gray_sat = 0.0 + if isinstance(gray_stats, dict): + vals = [float(v.get("sat_pct", 0.0)) for v in gray_stats.values() if isinstance(v, dict)] + max_gray_sat = max(vals) if vals else 0.0 + + return { + "scale_min_applied": float(min(scale_values)) if scale_values else None, + "scale_max_applied": float(max(scale_values)) if scale_values else None, + "max_would_clip_pct": float(max(clip_values)) if clip_values else 0.0, + "max_white_sat_pct": float(max_white_sat), + "max_gray_sat_pct": float(max_gray_sat), + "warning_count": int(len(result.get("warnings", []) or [])), + "valid_channel_count": int(len(scales)), + } + + def evaluate_frame_quality(self, tensor: np.ndarray | None = None) -> dict: + """ + Avalia a qualidade do tensor final gerado pelo RawProcessorCore. + + Esta avaliação pertence ao processamento do tensor, não à captura RAW_BRUTO. + Ela combina: + - estatísticas do tensor final por canal; + - resumo do patch_normalization; + - warnings radiométricos/normalização. + + Status: + good -> tensor utilizável normalmente; + warning -> tensor utilizável, mas com cautela/auditoria; + bad -> tensor não recomendado para treino/inferência crítica. + """ + channel_names = ["R", "G", "B", "RE", "NIR"] + quality = { + "status": "good", + "usable_for_training": True, + "usable_for_inference": True, + "reasons": [], + "metrics": {}, + "thresholds": { + "tensor_sat_warning_pct": 0.5, + "tensor_sat_bad_pct": 5.0, + "tensor_dark_warning_pct": 45.0, + "tensor_dark_bad_pct": 75.0, + "patch_white_sat_warning_pct": 0.5, + "patch_white_sat_bad_pct": 20.0, + "patch_clip_warning_pct": 0.5, + "patch_clip_bad_pct": 5.0, + }, + } + + def mark(level: str, reason: str): + ranks = {"good": 0, "warning": 1, "bad": 2} + if ranks.get(level, 0) > ranks.get(quality["status"], 0): + quality["status"] = level + if reason not in quality["reasons"]: + quality["reasons"].append(reason) + + if tensor is None or not isinstance(tensor, np.ndarray) or tensor.ndim != 3: + mark("bad", "invalid_tensor") + quality["usable_for_training"] = False + quality["usable_for_inference"] = False + return quality + + stats = self._tensor_channel_stats(tensor, channel_names) + quality["metrics"]["tensor_stats"] = stats + + sat_values = [float(v.get("sat_pct", 0.0)) for v in stats.values() if isinstance(v, dict)] + dark_values = [float(v.get("dark_pct", 0.0)) for v in stats.values() if isinstance(v, dict)] + over_values = [float(v.get("over_1_pct", 0.0)) for v in stats.values() if isinstance(v, dict)] + under_values = [float(v.get("under_0_pct", 0.0)) for v in stats.values() if isinstance(v, dict)] + + max_tensor_sat = max(sat_values) if sat_values else 0.0 + max_tensor_dark = max(dark_values) if dark_values else 0.0 + max_tensor_over = max(over_values) if over_values else 0.0 + max_tensor_under = max(under_values) if under_values else 0.0 + + quality["metrics"]["max_tensor_sat_pct"] = float(max_tensor_sat) + quality["metrics"]["max_tensor_dark_pct"] = float(max_tensor_dark) + quality["metrics"]["max_tensor_over_1_pct"] = float(max_tensor_over) + quality["metrics"]["max_tensor_under_0_pct"] = float(max_tensor_under) + + th = quality["thresholds"] + if max_tensor_sat >= th["tensor_sat_bad_pct"]: + mark("bad", f"tensor_saturation_high:{max_tensor_sat:.2f}%") + elif max_tensor_sat >= th["tensor_sat_warning_pct"]: + mark("warning", f"tensor_saturation_warning:{max_tensor_sat:.2f}%") + + if max_tensor_dark >= th["tensor_dark_bad_pct"]: + mark("bad", f"tensor_too_dark:{max_tensor_dark:.2f}%") + elif max_tensor_dark >= th["tensor_dark_warning_pct"]: + mark("warning", f"tensor_dark_warning:{max_tensor_dark:.2f}%") + + patch_result = self.last_patch_normalization_result or {} + quality["metrics"]["patch_normalization_summary"] = patch_result.get("summary", {}) if isinstance(patch_result, dict) else {} + + if isinstance(patch_result, dict): + if patch_result.get("enabled", False) and not patch_result.get("applied", False): + mark("bad", "patch_normalization_not_applied") + + summary = patch_result.get("summary", {}) or {} + max_white_sat = float(summary.get("max_white_sat_pct", 0.0) or 0.0) + max_clip = float(summary.get("max_would_clip_pct", 0.0) or 0.0) + warning_count = int(summary.get("warning_count", 0) or 0) + valid_channel_count = int(summary.get("valid_channel_count", 0) or 0) + + quality["metrics"]["max_patch_white_sat_pct"] = max_white_sat + quality["metrics"]["max_patch_would_clip_pct"] = max_clip + quality["metrics"]["patch_warning_count"] = warning_count + quality["metrics"]["patch_valid_channel_count"] = valid_channel_count + + if max_white_sat >= th["patch_white_sat_bad_pct"]: + mark("bad", f"patch_white_saturation_high:{max_white_sat:.2f}%") + elif max_white_sat >= th["patch_white_sat_warning_pct"]: + mark("warning", f"patch_white_saturation_warning:{max_white_sat:.2f}%") + + if max_clip >= th["patch_clip_bad_pct"]: + mark("bad", f"patch_output_clip_high:{max_clip:.2f}%") + elif max_clip >= th["patch_clip_warning_pct"]: + mark("warning", f"patch_output_clip_warning:{max_clip:.2f}%") + + if warning_count > 0: + mark("warning", f"patch_warnings:{warning_count}") + + if valid_channel_count not in (0, len(channel_names)): + mark("bad", f"patch_valid_channels_incomplete:{valid_channel_count}") + + warnings = patch_result.get("warnings", []) or [] + hard_warning_tokens = ( + "missing_or_invalid_gray_rois", + "missing_gray_patch", + "invalid_tensor_shape", + "gray_p50_too_low", + "empty_gray_rois", + ) + for w in warnings: + ws = str(w) + if any(tok in ws for tok in hard_warning_tokens): + mark("bad", f"patch_error:{ws}") + + if quality["status"] == "bad": + quality["usable_for_training"] = False + quality["usable_for_inference"] = False + elif quality["status"] == "warning": + quality["usable_for_training"] = True + quality["usable_for_inference"] = True + quality["requires_review"] = True + else: + quality["requires_review"] = False + + return quality + + def _resolve_targets_by_patch_channel(self, cfg: dict, rad_cfg: dict) -> dict: + """ + Resolve os alvos radiométricos por tipo de referência e por canal CHW. + + Contrato atual: + patch_normalization.targets_by_patch_channel + + Formato esperado: + { + "black": {"R": 0.06, "G": 0.06, "B": 0.06, "RE": 0.06, "NIR": 0.06}, + "gray": {"R": 0.34, "G": 0.34, "B": 0.34, "RE": 0.24, "NIR": 0.30}, + "white": {"R": 0.78, "G": 0.78, "B": 0.78, "RE": 0.78, "NIR": 0.78} + } + + Observação: + Por enquanto, o método gray_scale_with_white_guard usa principalmente: + - gray: alvo principal para calcular escala + - white: alvo/guarda para limitar escala + - black: reservado para futura correção com offset ou linearização + """ + channel_names = ["R", "G", "B", "RE", "NIR"] + + # Valores seguros caso algo falte no JSON atual. + default_targets = { + "black": { + "R": 0.06, + "G": 0.06, + "B": 0.06, + "RE": 0.06, + "NIR": 0.06, + }, + "gray": { + "R": 0.34, + "G": 0.34, + "B": 0.34, + "RE": 0.24, + "NIR": 0.30, + }, + "white": { + "R": 0.78, + "G": 0.78, + "B": 0.78, + "RE": 0.78, + "NIR": 0.78, + }, + } + + out = { + patch_type: dict(values) + for patch_type, values in default_targets.items() + } + + explicit = (cfg or {}).get("targets_by_patch_channel", {}) or {} + + if isinstance(explicit, dict): + for patch_type in ("black", "gray", "white"): + patch_targets = explicit.get(patch_type, {}) or {} + if not isinstance(patch_targets, dict): + continue + + for ch in channel_names: + if ch not in patch_targets: + continue + try: + out[patch_type][ch] = float(patch_targets[ch]) + except Exception: + pass + + return out + + def _resolve_patch_tensor_rois(self, patch: dict, w: int, h: int, reference_role: str = "rgb") -> list: + """ + Resolve as ROIs usadas pelo patch_normalization no tensor final. + + Contrato atual escolhido: + - O tensor final é alinhado no espaço do RGB. + - Portanto, as ROIs dos cartões no tensor final vêm de: + reference_patches[type].roi_list_by_role["rgb"] + - Suporta N ROIs por cor/cartão. + """ + if not isinstance(patch, dict): + return [] + + roi_list_by_role = patch.get("roi_list_by_role", {}) or {} + roi_items = roi_list_by_role.get(reference_role, []) or [] + + if not isinstance(roi_items, list): + roi_items = [] + + rois = [] + for idx, item in enumerate(roi_items): + if not isinstance(item, dict): + continue + + if not bool(item.get("enabled", True)): + continue + + roi_pct = item.get("roi_pct", {}) or {} + if not isinstance(roi_pct, dict): + continue + + roi_px = self._roi_pct_to_pixels_from_patch(roi_pct, w, h) + x0, y0, x1, y1 = roi_px + + if x1 <= x0 or y1 <= y0: + continue + + rois.append({ + "name": str(item.get("name") or f"{reference_role}_roi_{idx + 1:02d}"), + "roi_pct": dict(roi_pct), + "roi_px": roi_px, + }) + + return rois + + def _measure_patch_rois_for_channel(self, ch: np.ndarray, rois: list) -> dict | None: + """ + Mede uma lista de ROIs no canal CHW já fusionado e reduz de forma robusta. + + Com múltiplas ROIs, usamos mediana dos p50/p05/p95 e máximo de saturação/dark + para manter a normalização estável sem ignorar ROI problemática. + """ + roi_results = [] + + for roi in rois or []: + x0, y0, x1, y1 = roi["roi_px"] + vals = ch[y0:y1, x0:x1].reshape(-1) + + if vals.size <= 0: + continue + + stats = { + "name": roi.get("name"), + "roi_px": list(roi["roi_px"]), + "p05": float(np.percentile(vals, 5)), + "p50": float(np.percentile(vals, 50)), + "p95": float(np.percentile(vals, 95)), + "sat_pct": float((vals >= 0.98).mean() * 100.0), + "dark_pct": float((vals <= 0.02).mean() * 100.0), + } + roi_results.append(stats) + + if not roi_results: + return None + + return { + "roi_count": len(rois or []), + "valid_roi_count": len(roi_results), + "p05": float(np.median([r["p05"] for r in roi_results])), + "p50": float(np.median([r["p50"] for r in roi_results])), + "p95": float(np.median([r["p95"] for r in roi_results])), + "sat_pct": float(max(r["sat_pct"] for r in roi_results)), + "dark_pct": float(max(r["dark_pct"] for r in roi_results)), + "roi_results": roi_results, + } + + def _roi_pct_to_pixels_from_patch(self, roi_pct: dict, w: int, h: int): + x0 = int(float(roi_pct.get("x0", 0.0)) * w) + y0 = int(float(roi_pct.get("y0", 0.0)) * h) + x1 = int(float(roi_pct.get("x1", 1.0)) * w) + y1 = int(float(roi_pct.get("y1", 1.0)) * h) + + x0 = max(0, min(w - 1, x0)) + x1 = max(x0 + 1, min(w, x1)) + y0 = max(0, min(h - 1, y0)) + y1 = max(y0 + 1, min(h, y1)) + + return x0, y0, x1, y1 + + + def extract_camera_meta(self, meta_json: dict, cam_id: str) -> dict: + cam_frames = meta_json.get("camera_frames", {}) or meta_json.get("stream_meta", {}).get("camera_frames", {}) + + cam = cam_frames.get(cam_id) + if not cam: + raise ValueError(f"Camera {cam_id} não encontrada no meta") + + # Detecta RAW10 packed mono + if int(cam.get("channels", 1)) == 1 and int(cam.get("bit_depth", 10)) == 10: + packed_width = int(cam.get("width")) + height = int(cam.get("height")) + + # 🔥 converte packed → real + real_width = int((packed_width * 8) / 10) + + return { + "camera_id": cam_id, + "width": real_width, + "height": height, + "channels": 1, + "bit_depth": 10, + "shape": [height, packed_width], # packed shape + "role": cam.get("role") + } + + # RGB + else: + width = int(cam.get("width")) + height = int(cam.get("height")) + channels = int(cam.get("channels", 3)) + + return { + "camera_id": cam_id, + "width": width, + "height": height, + "channels": channels, + "bit_depth": int(cam.get("bit_depth", 8)), + "shape": [height, width, channels], # 🔥 AQUI está a correção + "role": cam.get("role") + } + + # ============================================================ + # RAW10 PACKED + # ============================================================ + + def packed_width_for_raw10(self, sensor_width: int = None) -> int: + width = sensor_width if sensor_width is not None else self.sensor_width + return math.ceil(width * 10 / 8) + + def pack_raw10_packed(self, raw16: np.ndarray) -> np.ndarray: + h, w = raw16.shape + + if w % 4 != 0: + raise ValueError(f"Width precisa ser múltiplo de 4 para pack otimizado. Veio {w}") + + raw16 = np.clip(raw16, 0, 1023).astype(np.uint16) + + p0 = raw16[:, 0::4] + p1 = raw16[:, 1::4] + p2 = raw16[:, 2::4] + p3 = raw16[:, 3::4] + + b0 = (p0 >> 2).astype(np.uint8) + b1 = (p1 >> 2).astype(np.uint8) + b2 = (p2 >> 2).astype(np.uint8) + b3 = (p3 >> 2).astype(np.uint8) + + b4 = ( + ((p0 & 0x03) << 0) | + ((p1 & 0x03) << 2) | + ((p2 & 0x03) << 4) | + ((p3 & 0x03) << 6) + ).astype(np.uint8) + + packed = np.empty((h, w // 4, 5), dtype=np.uint8) + packed[:, :, 0] = b0 + packed[:, :, 1] = b1 + packed[:, :, 2] = b2 + packed[:, :, 3] = b3 + packed[:, :, 4] = b4 + + return packed.reshape(h, w // 4 * 5) + + def load_raw10_packed_file(self, path: str, width: int, height: int) -> np.ndarray: + packed_width = self.packed_width_for_raw10(width) + + expected_size = height * packed_width + actual_size = os.path.getsize(path) + + if actual_size != expected_size: + raise ValueError( + f"Tamanho inválido RAW10: {actual_size}, esperado {expected_size} em {path}" + ) + + packed = np.fromfile(path, dtype=np.uint8).reshape(height, packed_width) + return self.unpack_raw10_packed(packed, sensor_width=width, sensor_height=height) + + def save_raw10_packed_file(self, path: str, raw16: np.ndarray): + packed = self.pack_raw10_packed(raw16) + packed.tofile(path) + + # ============================================================ + # RGB UINT8 + # ============================================================ + + def load_rgb_u8_file(self, path: str, shape) -> np.ndarray: + arr = np.fromfile(path, dtype=np.uint8) + + expected = np.prod(shape) + if arr.size != expected: + raise ValueError( + f"Tamanho inválido RGB: {arr.size}, esperado {expected} em {path}" + ) + + return arr.reshape(shape) + + def save_rgb_u8_file(self, path: str, arr: np.ndarray): + arr.astype(np.uint8).tofile(path) + + + # ============================================================ + # DISPATCHER (O MAIS IMPORTANTE) + # ============================================================ + + def load_native_bin(self, path: str, cam_meta: dict) -> np.ndarray: + """ + Decide automaticamente como carregar o .bin baseado no meta. + """ + + channels = int(cam_meta.get("channels", 1)) + bit_depth = int(cam_meta.get("bit_depth", 10)) + shape = cam_meta.get("shape") + + if channels == 1 and bit_depth == 10: + width = int(cam_meta["width"]) + height = int(cam_meta["height"]) + return self.load_raw10_packed_file(path, width, height) + + elif channels == 3 and bit_depth == 8: + return self.load_rgb_u8_file(path, shape) + + else: + raise ValueError( + f"Formato não suportado: channels={channels}, bit_depth={bit_depth}" + ) + + + def save_native_bin(self, path: str, arr: np.ndarray, cam_meta: dict): + """ + Salva no formato correto baseado no meta. + """ + + channels = int(cam_meta.get("channels", 1)) + bit_depth = int(cam_meta.get("bit_depth", 10)) + + if channels == 1 and bit_depth == 10: + self.save_raw10_packed_file(path, arr) + + elif channels == 3 and bit_depth == 8: + self.save_rgb_u8_file(path, arr) + + else: + raise ValueError( + f"Formato não suportado para salvar: channels={channels}, bit_depth={bit_depth}" + ) + + + def load_config_json(self, path: str): + if not path or not os.path.isfile(path): + raise FileNotFoundError(f"Arquivo de calibração não encontrado: {path}") + + with open(path, "r", encoding="utf-8") as f: + data = json.load(f) + + self.bayer_pattern = data.get("bayer_pattern", self.bayer_pattern) + + rgb_proc = data.get("rgb_processing") + if isinstance(rgb_proc, dict): + self.rgb_processing_config = self._merge_config(self.rgb_processing_config, rgb_proc) + + fusion = data.get("fusion_config") + if isinstance(fusion, dict): + self.fusion_config = self._merge_config(self.fusion_config, fusion) + else: + print("[WARN] JSON sem fusion_config. Mantendo config padrão.") + + rgb_cal = data.get("rgb_calibration") + if isinstance(rgb_cal, dict): + self.rgb_calibration = self._merge_config(self.rgb_calibration, rgb_cal) + + flatfield = data.get("flatfield_config") + if isinstance(flatfield, dict): + self.flatfield_config = self._merge_config(self.flatfield_config, flatfield) + self.load_flatfield_maps() + else: + self.flatfield_config["enabled"] = False + self.flatfield_maps = {} + self.flatfield_loaded = False + + radiometric = data.get("radiometric_config") + if isinstance(radiometric, dict): + self.radiometric_config = self._merge_config(self.radiometric_config, radiometric) + + rad_norm_config = data.get("radiometric_normalization") + if isinstance(rad_norm_config, dict): + self.radiometric_normalization_config = self._merge_config(self.radiometric_normalization_config, rad_norm_config) + + patch_norm = data.get("patch_normalization") + if isinstance(patch_norm, dict): + self.patch_normalization_config = self._merge_config(self.patch_normalization_config, patch_norm) + + cam_set = data.get("camera_settings") + if isinstance(cam_set, dict): + self.camera_settings = self._merge_config(self.camera_settings, cam_set) + + def _merge_config(self, default_cfg: dict, loaded_cfg: dict) -> dict: + cfg = json.loads(json.dumps(default_cfg)) + + def merge(dst: dict, src: dict): + for key, value in src.items(): + if isinstance(value, dict) and isinstance(dst.get(key), dict): + merge(dst[key], value) + else: + dst[key] = value + + if isinstance(loaded_cfg, dict): + merge(cfg, loaded_cfg) + + return cfg + + + def _resolve_calibration_path(self, path: str) -> str: + if not path: + return "" + + path = str(path).replace("\\", "/") + + if os.path.isabs(path): + return path + + # Primeiro tenta relativo ao diretório de execução. + if os.path.isfile(path): + return path + + # Depois tenta relativo ao diretório do module_params.json. + candidate = os.path.join(self.calibration_base_dir, path) + if os.path.isfile(candidate): + return candidate + + # Por fim, se o path já começa com "calibration/", tenta relativo ao pai da pasta calibration. + base_parent = os.path.dirname(self.calibration_base_dir) + candidate = os.path.join(base_parent, path) + if os.path.isfile(candidate): + return candidate + + return path + + def load_flatfield_maps(self): + cfg = self.flatfield_config or {} + + if not cfg.get("enabled", False): + self.flatfield_maps = {} + self.flatfield_loaded = False + return False + + npz_file = cfg.get("npz_file") + if not npz_file: + print("[WARN] flatfield_config habilitado, mas sem npz_file.") + self.flatfield_maps = {} + self.flatfield_loaded = False + return False + + npz_path = self._resolve_calibration_path(npz_file) + + if not os.path.isfile(npz_path): + print(f"[WARN] Arquivo flat-field não encontrado: {npz_file} -> {npz_path}") + self.flatfield_maps = {} + self.flatfield_loaded = False + return False + + data = np.load(npz_path) + + maps = {} + channel_maps = cfg.get("channel_maps", {}) or {} + channels = cfg.get("channels", ["R", "G", "B", "RE", "NIR"]) + + for ch in channels: + ch = str(ch).upper() + ch_cfg = channel_maps.get(ch, {}) or {} + + gain_key = ch_cfg.get("gain_key", f"gain_{ch}") + dark_key = ch_cfg.get("dark_median_key", f"dark_median_{ch}") + + if gain_key not in data: + print(f"[WARN] Flat-field sem chave {gain_key} para canal {ch}.") + continue + + entry = { + "gain": data[gain_key].astype(np.float32), + "gain_key": gain_key, + } + + if dark_key and dark_key in data: + entry["dark"] = data[dark_key].astype(np.float32) + entry["dark_key"] = dark_key + + maps[ch] = entry + + self.flatfield_maps = maps + self.flatfield_loaded = len(maps) > 0 + + if self.flatfield_loaded: + print(f"[OK] Flat-field carregado: {npz_path} | canais={list(maps.keys())}") + else: + print(f"[WARN] Flat-field habilitado, mas nenhum mapa foi carregado: {npz_path}") + + return self.flatfield_loaded + + def apply_dark_to_decoded(self, decoded: dict) -> dict: + cfg = self.flatfield_config or {} + + if not cfg.get("enabled", False): + return decoded + + subtract_dark = bool(cfg.get("subtract_dark", True)) + if not subtract_dark: + return decoded + + if not self.flatfield_loaded: + self.load_flatfield_maps() + + if not self.flatfield_loaded: + return decoded + + corrected = {} + + for cam_id, item in decoded.items(): + role = str(item.get("role") or item.get("meta", {}).get("role") or "").lower() + img = item.get("image") + + if img is None: + corrected[cam_id] = item + continue + + new_item = dict(item) + new_meta = dict(item.get("meta", {}) or {}) + + if role == "rgb": + if img.ndim != 3 or img.shape[2] < 3: + corrected[cam_id] = item + continue + + out = img.astype(np.float32).copy() + + for idx, ch in enumerate(("R", "G", "B")): + out[:, :, idx] = self._subtract_dark_single_channel( + out[:, :, idx], + ch, + ) + + new_item["image"] = out + + elif role in ("re", "nir"): + ch = "RE" if role == "re" else "NIR" + + new_item["image"] = self._subtract_dark_single_channel( + img.astype(np.float32), + ch, + ) + + else: + corrected[cam_id] = item + continue + + new_meta["dark_applied"] = True + new_item["meta"] = new_meta + corrected[cam_id] = new_item + + return corrected + + def apply_flat_gain_to_decoded(self, decoded: dict) -> dict: + cfg = self.flatfield_config or {} + + if not cfg.get("enabled", False): + return decoded + + if not self.flatfield_loaded: + self.load_flatfield_maps() + + if not self.flatfield_loaded: + return decoded + + clip_output = bool(cfg.get("clip_output", True)) + corrected = {} + + # Guarda anti-roxo / anti-artefato em saturação + sat_guard_enabled = bool(cfg.get("saturation_guard_enabled", True)) + sat_mode = str(cfg.get("saturation_guard_mode", "fade_strength")).lower() + sat_threshold = float(cfg.get("saturation_guard_threshold", 0.97)) + + for cam_id, item in decoded.items(): + role = str(item.get("role") or item.get("meta", {}).get("role") or "").lower() + img = item.get("image") + + if img is None: + corrected[cam_id] = item + continue + + new_item = dict(item) + new_meta = dict(item.get("meta", {}) or {}) + + if role == "rgb": + if img.ndim != 3 or img.shape[2] < 3: + corrected[cam_id] = item + continue + + out = img.astype(np.float32).copy() + + # Máscara comum RGB: + # se qualquer canal estiver perto de saturar, tratamos os 3 canais juntos. + # Isso evita R/G/B receberem correções diferentes e criarem magenta/roxo. + saturation_mask = None + if sat_guard_enabled: + rgb_max = np.max(out[:, :, :3], axis=2) + saturation_mask = rgb_max >= sat_threshold + + for idx, ch in enumerate(("R", "G", "B")): + out[:, :, idx] = self._apply_flat_gain_single_channel( + out[:, :, idx], + ch, + clip_output=clip_output, + saturation_mask=saturation_mask, + ) + + new_item["image"] = out + + elif role in ("re", "nir"): + ch = "RE" if role == "re" else "NIR" + + # Para RE/NIR a guarda pode ser por canal mesmo. + # Não existe cor roxa aqui, mas ainda evita mexer em pixels clipados. + saturation_mask = None + img_f = img.astype(np.float32) + if sat_guard_enabled: + saturation_mask = img_f >= sat_threshold + + new_item["image"] = self._apply_flat_gain_single_channel( + img_f, + ch, + clip_output=clip_output, + saturation_mask=saturation_mask, + ) + + else: + corrected[cam_id] = item + continue + + new_meta["flatfield_applied"] = True + new_meta["flatfield_map_type"] = cfg.get("map_type", "gain") + new_meta["flatfield_saturation_guard"] = { + "enabled": sat_guard_enabled, + "mode": sat_mode, + "threshold": sat_threshold, + } + + new_item["meta"] = new_meta + corrected[cam_id] = new_item + + return corrected + + def _subtract_dark_single_channel( + self, + img: np.ndarray, + channel_name: str, + ) -> np.ndarray: + ch = str(channel_name).upper() + entry = self.flatfield_maps.get(ch) + + if not entry: + return img.astype(np.float32, copy=False) + + dark = entry.get("dark") + if dark is None: + return img.astype(np.float32, copy=False) + + base = img.astype(np.float32) + + dark = dark.astype(np.float32) + if dark.shape[:2] != base.shape[:2]: + dark = cv2.resize( + dark, + (base.shape[1], base.shape[0]), + interpolation=cv2.INTER_LINEAR, + ) + + out = np.maximum(base - dark, 0.0) + return out.astype(np.float32, copy=False) + + def _apply_flat_gain_single_channel( + self, + img: np.ndarray, + channel_name: str, + clip_output: bool = True, + saturation_mask: np.ndarray | None = None, + ) -> np.ndarray: + t_total0 = time.perf_counter() + t_cache_ms = 0.0 + t_gain_eff_ms = 0.0 + t_mul_clip_ms = 0.0 + + ch = str(channel_name).upper() + cfg = self.flatfield_config or {} + + base = img.astype(np.float32, copy=False) + + fast_runtime = bool(cfg.get("fast_runtime", False)) + sat_guard_enabled = bool(cfg.get("saturation_guard_enabled", True)) + + if fast_runtime and not sat_guard_enabled: + t0 = time.perf_counter() + + gain_eff = self._get_runtime_gain_eff_map(ch, base.shape, cfg) + + if gain_eff is None: + return base + + out = self._apply_flat_gain_simple_aggressive( + base, + gain_eff, + clip_output=clip_output, + ) + + t_total_ms = (time.perf_counter() - t_total0) * 1000.0 + if not hasattr(self, "_last_flat_ch_perf_log_ts"): + self._last_flat_ch_perf_log_ts = 0.0 + now = time.time() + if now - self._last_flat_ch_perf_log_ts >= 1.0: + self._last_flat_ch_perf_log_ts = now + print( + "[PERF][FLAT_CH_FAST] " + f"ch={ch} " + f"shape={base.shape} " + f"total={t_total_ms:.2f}ms " + f"clip={clip_output} " + f"strength={cfg.get('strength_by_channel', {}).get(ch, cfg.get('strength', 1.0))}" + ) + + return out.astype(np.float32, copy=False) + + # ============================================================ + # Gain map runtime cacheado + # ============================================================ + t0 = time.perf_counter() + gain = self._get_runtime_gain_map(ch, base.shape, cfg) + t_cache_ms = (time.perf_counter() - t0) * 1000.0 + + if gain is None: + return base + + runtime_smooth_ksize = int(cfg.get("runtime_smooth_ksize", 0) or 0) + if runtime_smooth_ksize >= 3 and runtime_smooth_ksize % 2 == 0: + runtime_smooth_ksize += 1 + + # Importante: + # NÃO aplicar GaussianBlur aqui. + # O gain já veio pronto/cacheado de _get_runtime_gain_map(). + + # ============================================================ + # Intensidade global e por canal + # ============================================================ + strength = float(cfg.get("strength", 1.0)) + strength_by_channel = cfg.get("strength_by_channel", {}) or {} + if ch in strength_by_channel: + try: + strength = float(strength_by_channel[ch]) + except Exception: + pass + + gain_min_runtime = float(cfg.get("gain_min_runtime", 0.0)) + gain_max_runtime = float(cfg.get("gain_max_runtime", 999.0)) + + # ============================================================ + # Guarda de saturação + # ============================================================ + sat_guard_enabled = bool(cfg.get("saturation_guard_enabled", True)) + sat_mode = str(cfg.get("saturation_guard_mode", "fade_strength")).lower() + + sat_soft_start = float(cfg.get("saturation_guard_soft_start", 0.90)) + sat_hard = float(cfg.get("saturation_guard_hard", 0.98)) + + if saturation_mask is None and sat_guard_enabled: + sat_threshold = float(cfg.get("saturation_guard_threshold", 0.97)) + saturation_mask = base >= sat_threshold + + # ============================================================ + # Calcula ganho efetivo + # ============================================================ + t0 = time.perf_counter() + + if sat_guard_enabled and sat_mode == "fade_strength": + denom = max(sat_hard - sat_soft_start, 1e-6) + + t = (base - sat_soft_start) / denom + t = np.clip(t, 0.0, 1.0) + + strength_mask = 1.0 - t + + if saturation_mask is not None: + strength_mask = np.where(saturation_mask, 0.0, strength_mask) + + gain_eff = 1.0 + (strength * strength_mask) * (gain - 1.0) + + else: + gain_eff = 1.0 + strength * (gain - 1.0) + + gain_eff = np.clip(gain_eff, gain_min_runtime, gain_max_runtime) + + t_gain_eff_ms = (time.perf_counter() - t0) * 1000.0 + + # ============================================================ + # Aplica ganho + # ============================================================ + t0 = time.perf_counter() + + out = base * gain_eff + + if sat_guard_enabled and sat_mode == "skip" and saturation_mask is not None: + out = np.where(saturation_mask, base, out) + + if clip_output: + out = np.clip(out, 0.0, 1.0) + + t_mul_clip_ms = (time.perf_counter() - t0) * 1000.0 + + t_total_ms = (time.perf_counter() - t_total0) * 1000.0 + + if not hasattr(self, "_last_flat_ch_perf_log_ts"): + self._last_flat_ch_perf_log_ts = 0.0 + + now = time.time() + if now - self._last_flat_ch_perf_log_ts >= 1.0: + self._last_flat_ch_perf_log_ts = now + print( + "[PERF][FLAT_CH] " + f"ch={ch} " + f"shape={base.shape} " + f"cache={t_cache_ms:.1f}ms " + f"gain_eff={t_gain_eff_ms:.1f}ms " + f"mul_clip={t_mul_clip_ms:.1f}ms " + f"total={t_total_ms:.1f}ms " + f"ksize={runtime_smooth_ksize} " + f"strength={strength:.2f}" + ) + + return out.astype(np.float32, copy=False) + + + def normalize_decoded_by_capture_controls(self, decoded: dict, meta: dict | None = None) -> dict: + cfg = self.radiometric_normalization_config or {} + self.last_radiometric_normalization_result = None + + result = { + "enabled": bool(cfg.get("enabled", False)), + "applied": False, + "method": cfg.get("method", "oak_ae_frame_controls_v1"), + "warnings": [], + "by_role": {}, + "by_camera": {}, + "summary": {}, + } + + if not cfg.get("enabled", False): + result["warnings"].append("radiometric_normalization_disabled") + self.last_radiometric_normalization_result = result + return decoded + + method = str(cfg.get("method", "oak_ae_frame_controls_v1")).lower() + if method not in ("oak_ae_frame_controls_v1", "exposure_iso_reference"): + result["warnings"].append(f"unsupported_method:{method}") + self.last_radiometric_normalization_result = result + return decoded + + controls_by_role, controls_by_cam = self._extract_frame_controls_from_meta_by_role(meta) + + if not controls_by_role: + result["warnings"].append("missing_frame_controls") + self.last_radiometric_normalization_result = result + + if str(cfg.get("missing_controls_policy", "skip")).lower() == "raise": + raise RuntimeError("radiometric_normalization ativo, mas meta.frame_controls ausente.") + + return decoded + + reference_controls = cfg.get("reference_controls", {}) or {} + clip_output = bool(cfg.get("clip_output", False)) + + # Cache por role para não recalcular factor/scale 3x quando RGB tem 3 canais no mesmo item. + scale_by_role = {} + debug_by_role = {} + + normalized = {} + + for cam_id, item in decoded.items(): + role = str(item.get("role") or item.get("meta", {}).get("role") or "").lower() + img = item.get("image") + + if img is None or not role: + normalized[cam_id] = item + result["warnings"].append(f"{cam_id}:missing_image_or_role") + continue + + if role in scale_by_role: + scale = scale_by_role[role] + debug = dict(debug_by_role[role]) + debug["camera_id"] = cam_id + else: + actual_ctrl = controls_by_role.get(role, {}) or {} + ref_ctrl = reference_controls.get(role, {}) or {} + + actual_factor = self._radiometric_factor_from_controls(actual_ctrl, cfg) + ref_factor = self._radiometric_factor_from_controls(ref_ctrl, cfg) + + if actual_factor <= 0 or ref_factor <= 0: + normalized[cam_id] = item + result["warnings"].append( + f"{role}:invalid_factor actual={actual_factor:.6g} ref={ref_factor:.6g}" + ) + + if str(cfg.get("invalid_controls_policy", "skip")).lower() == "raise": + raise RuntimeError(f"Controles radiométricos inválidos para role={role}: {actual_ctrl}") + + continue + + raw_scale = float(ref_factor / actual_factor) + scale = self._clip_radiometric_scale(raw_scale, role, cfg) + + debug = self._build_radiometric_debug( + method=method, + role=role, + cam_id=cam_id, + actual_ctrl=actual_ctrl, + ref_ctrl=ref_ctrl, + actual_factor=actual_factor, + ref_factor=ref_factor, + raw_scale=raw_scale, + scale=scale, + clip_output=clip_output, + ) + + scale_by_role[role] = scale + debug_by_role[role] = dict(debug) + + out, reused = self._apply_radiometric_scale_inplace( + img, + scale=scale, + clip_output=clip_output, + ) + + new_item = dict(item) + new_meta = dict(item.get("meta", {}) or {}) + + debug["inplace_reused_input"] = bool(reused) + debug["image_shape"] = list(out.shape) if hasattr(out, "shape") else None + debug["image_dtype"] = str(out.dtype) if hasattr(out, "dtype") else None + + new_meta["radiometric_normalization"] = debug + new_meta["radiometric_normalization_applied"] = True + new_meta["radiometric_normalization_scale"] = float(scale) + + new_item["image"] = out + new_item["meta"] = new_meta + normalized[cam_id] = new_item + + result["applied"] = True + result["by_camera"][cam_id] = debug + result["by_role"][role] = debug + + if result["applied"]: + scales = [v.get("scale_applied") for v in result["by_camera"].values() if isinstance(v, dict)] + scales = [float(s) for s in scales if s is not None] + + if scales: + result["summary"] = { + "scale_min": float(np.min(scales)), + "scale_max": float(np.max(scales)), + "scale_mean": float(np.mean(scales)), + "num_normalized": int(len(scales)), + "inplace_fast_path": True, + } + + self.last_radiometric_normalization_result = result + return normalized + + def _extract_frame_controls_from_meta_by_role(self, meta: dict | None) -> tuple[dict, dict]: + """ + Retorna: + controls_by_role = { + "rgb": {...}, + "re": {...}, + "nir": {...} + } + + controls_by_cam = { + "CAM_A": {...}, + "CAM_B": {...}, + "CAM_C": {...} + } + + Fonte principal: + meta["frame_controls"] + + Também aceita: + meta["stream_meta"]["frame_controls"] + """ + if not isinstance(meta, dict): + return {}, {} + + stream_meta = meta.get("stream_meta") if isinstance(meta.get("stream_meta"), dict) else None + + frame_controls = meta.get("frame_controls") + if not isinstance(frame_controls, dict) and stream_meta is not None: + frame_controls = stream_meta.get("frame_controls") + + if not isinstance(frame_controls, dict) or not frame_controls: + return {}, {} + + camera_info = meta.get("camera_info") + if not isinstance(camera_info, dict) and stream_meta is not None: + camera_info = stream_meta.get("camera_info") + + camera_info = camera_info if isinstance(camera_info, dict) else {} + + controls_by_cam = {} + controls_by_role = {} + + for cam_id, ctrl in frame_controls.items(): + if not isinstance(ctrl, dict): + continue + + cam_id = str(cam_id) + controls_by_cam[cam_id] = dict(ctrl) + + role = str( + (camera_info.get(cam_id, {}) or {}).get("role", "") + ).lower() + + if not role: + # fallback defensivo caso algum meta antigo venha sem camera_info + role = self._role_from_cam_id_fallback(cam_id) + + if role: + controls_by_role[role] = dict(ctrl) + + return controls_by_role, controls_by_cam + + def _role_from_cam_id_fallback(self, cam_id: str) -> str: + """ + Fallback fraco. Só usado se o meta não tiver camera_info. + No fluxo novo, camera_info sempre deve existir. + """ + cam_id = str(cam_id).upper() + + role_map = { + "CAM_A": "rgb", + "CAM_B": "re", + "CAM_C": "nir", + } + + return role_map.get(cam_id, "") + + def _radiometric_factor_from_controls(self, ctrl: dict, cfg: dict) -> float: + """ + Modelo físico simples: + fator = exposure_time_us * (sensitivity_iso / iso_base) + + Esse fator representa a amplificação aproximada do sinal causada pela câmera. + """ + if not isinstance(ctrl, dict): + return 0.0 + + iso_base = float(cfg.get("iso_base", 100.0) or 100.0) + + exp = ctrl.get("exposure_time_us", None) + iso = ctrl.get("sensitivity_iso", None) + + # Compatibilidade com contrato antigo + gain = ctrl.get("analogue_gain", None) + gain_est = ctrl.get("analogue_gain_est", None) + + try: + exp = float(exp) + except Exception: + exp = 0.0 + + if exp <= 0: + return 0.0 + + try: + if iso is not None: + gain_factor = float(iso) / iso_base + elif gain_est is not None: + gain_factor = float(gain_est) + elif gain is not None: + gain_factor = float(gain) + else: + gain_factor = 1.0 + except Exception: + gain_factor = 1.0 + + if gain_factor <= 0: + gain_factor = 1.0 + + return float(exp * gain_factor) + + def _clip_radiometric_scale(self, scale: float, role: str, cfg: dict) -> float: + limits = cfg.get("scale_limits", {}) or {} + + role_limits = limits.get(role) + if not isinstance(role_limits, dict): + role_limits = limits.get("default", {}) or {} + + scale_min = float(role_limits.get("min", 0.15)) + scale_max = float(role_limits.get("max", 6.0)) + + if scale_min <= 0: + scale_min = 0.001 + + if scale_max < scale_min: + scale_max = scale_min + + return float(np.clip(float(scale), scale_min, scale_max)) + + + def _extract_actual_controls_from_meta(self, meta: dict | None) -> dict: + if not meta: + return {} + + for key in ("actual_camera_controls", "camera_controls", "startup_camera_controls"): + controls = meta.get(key) + if isinstance(controls, dict) and controls: + return controls + + stream_meta = meta.get("stream_meta") + if isinstance(stream_meta, dict): + for key in ("actual_camera_controls", "camera_controls", "startup_camera_controls"): + controls = stream_meta.get(key) + if isinstance(controls, dict) and controls: + return controls + + return {} + + def _exposure_gain_factor(self, ctrl: dict) -> float: + if not isinstance(ctrl, dict): + return 0.0 + + exp = ctrl.get("exposure_time_us", None) + gain = ctrl.get("analogue_gain", None) + + try: + exp = float(exp) + except Exception: + exp = 0.0 + + try: + gain = float(gain) + except Exception: + gain = 1.0 + + if exp <= 0: + return 0.0 + + if gain <= 0: + gain = 1.0 + + return float(exp * gain) + + + def get_last_radiometric_normalization_result(self): + return self.last_radiometric_normalization_result + + def _get_runtime_gain_map(self, channel_name: str, base_shape: tuple, cfg: dict): + ch = str(channel_name).upper() + entry = self.flatfield_maps.get(ch) + + if not entry: + return None + + gain = entry.get("gain") + if gain is None: + return None + + h, w = base_shape[:2] + runtime_smooth_ksize = int(cfg.get("runtime_smooth_ksize", 0) or 0) + + if runtime_smooth_ksize >= 3 and runtime_smooth_ksize % 2 == 0: + runtime_smooth_ksize += 1 + + cache_key = ( + ch, + h, + w, + runtime_smooth_ksize, + ) + + cached = self._flatfield_runtime_cache.get(cache_key) + if cached is not None: + return cached + + print(f"[FLAT_CACHE] criando gain cache ch={ch} shape=({h},{w}) ksize={runtime_smooth_ksize}") + + gain_rt = gain.astype(np.float32, copy=False) + + if gain_rt.shape[:2] != (h, w): + gain_rt = cv2.resize( + gain_rt, + (w, h), + interpolation=cv2.INTER_LINEAR, + ) + + if runtime_smooth_ksize >= 3: + gain_rt = cv2.GaussianBlur( + gain_rt, + (runtime_smooth_ksize, runtime_smooth_ksize), + 0, + ) + + gain_rt = gain_rt.astype(np.float32, copy=False) + self._flatfield_runtime_cache[cache_key] = gain_rt + + return gain_rt + + + + def _raw10_expected_packed_width_fast(self, width: int) -> int: + """ + RAW10 packed: 4 pixels em 5 bytes. + Para OV9782/OV9282 em 1280 px, packed_width = 1600. + """ + return int(math.ceil(int(width) * 10 / 8)) + + def _prepare_raw10_packed_view_fast(self, packed_frame: np.ndarray, width: int, height: int) -> np.ndarray: + """ + Valida e retorna uma view útil do RAW10 packed, cortando padding se existir. + Não copia quando não precisa. + """ + arr = packed_frame + + if arr.ndim == 3 and arr.shape[2] == 1: + arr = arr[:, :, 0] + + if arr.ndim != 2: + raise ValueError(f"RAW10 packed esperado 2D. Veio shape={arr.shape}") + + width = int(width) + height = int(height) + + if width % 4 != 0: + raise ValueError(f"Largura {width} não é múltipla de 4 para RAW10 packed") + + expected_packed_width = self._raw10_expected_packed_width_fast(width) + + actual_h, actual_w = arr.shape[:2] + padding = actual_w - expected_packed_width + + if actual_h != height: + raise ValueError( + f"[ERRO FRAME] Altura RAW10 packed inesperada: {arr.shape}, esperado altura={height}" + ) + + if actual_w < expected_packed_width: + raise ValueError( + f"[ERRO FRAME] Largura RAW10 packed menor que esperada: {arr.shape}, " + f"esperado pelo menos ({height}, {expected_packed_width})" + ) + + if padding > 64: + raise ValueError( + f"[ERRO FRAME] Padding excessivo no RAW10 packed: {arr.shape}, " + f"esperado útil ({height}, {expected_packed_width}), padding={padding}" + ) + + return arr[:, :expected_packed_width] + + def _raw10_groups_view_fast(self, packed_frame: np.ndarray, width: int, height: int) -> np.ndarray: + """ + Retorna view em grupos RAW10: + shape = (height, width//4, 5) + sem converter tudo para uint16 de uma vez. + """ + arr = self._prepare_raw10_packed_view_fast(packed_frame, width, height) + return arr.reshape(int(height), int(width) // 4, 5) + + def _raw10_unpack_mono_to_float01_fast( + self, + packed_frame: np.ndarray, + width: int, + height: int, + bit_depth: int = 10, + ) -> np.ndarray: + """ + RAW10 packed mono -> float32 0..1. + + Otimização em relação ao caminho antigo: + antigo: RAW10 packed -> raw16 uint16 -> float32 / max + novo : RAW10 packed -> float32 0..1 direto + + Ainda gera imagem full-res, porque RE/NIR continuam full-res antes da fusão. + """ + width = int(width) + height = int(height) + bit_depth = int(bit_depth) + + groups = self._raw10_groups_view_fast(packed_frame, width, height) + + # Conversões por byte. Evita groups.astype(uint16) completo. + b0 = groups[:, :, 0].astype(np.uint16, copy=False) + b1 = groups[:, :, 1].astype(np.uint16, copy=False) + b2 = groups[:, :, 2].astype(np.uint16, copy=False) + b3 = groups[:, :, 3].astype(np.uint16, copy=False) + b4 = groups[:, :, 4].astype(np.uint16, copy=False) + + max_val = float((1 << bit_depth) - 1) + scale = np.float32(1.0 / max_val) + + out = np.empty((height, width), dtype=np.float32) + + # 4 pixels por grupo de 5 bytes. + out[:, 0::4] = ((b0 << 2) | ((b4 >> 0) & 0x03)).astype(np.float32) * scale + out[:, 1::4] = ((b1 << 2) | ((b4 >> 2) & 0x03)).astype(np.float32) * scale + out[:, 2::4] = ((b2 << 2) | ((b4 >> 4) & 0x03)).astype(np.float32) * scale + out[:, 3::4] = ((b3 << 2) | ((b4 >> 6) & 0x03)).astype(np.float32) * scale + + # RAW10 já está no intervalo, mas mantém defesa contra metadado errado. + return np.clip(out, 0.0, 1.0) + + def _raw10_unpack_cols_parity_to_float01_fast( + self, + groups_rows: np.ndarray, + width: int, + parity: int, + scale: np.float32, + ) -> np.ndarray: + """ + Desempacota somente colunas pares ou ímpares de um conjunto de linhas RAW10. + + groups_rows: + shape = (n_rows, width//4, 5) + + parity: + 0 -> colunas x = 0,2,4,6,... + 1 -> colunas x = 1,3,5,7,... + + Retorna: + shape = (n_rows, width//2), float32 0..1 + + Isso é a chave para o RGB bayer_planes sem raw16 full-res. + """ + width = int(width) + n_rows = groups_rows.shape[0] + + b0 = groups_rows[:, :, 0].astype(np.uint16, copy=False) + b1 = groups_rows[:, :, 1].astype(np.uint16, copy=False) + b2 = groups_rows[:, :, 2].astype(np.uint16, copy=False) + b3 = groups_rows[:, :, 3].astype(np.uint16, copy=False) + b4 = groups_rows[:, :, 4].astype(np.uint16, copy=False) + + out = np.empty((n_rows, width // 2), dtype=np.float32) + + if int(parity) == 0: + # Colunas pares: p0, p2 em cada grupo. + out[:, 0::2] = ((b0 << 2) | ((b4 >> 0) & 0x03)).astype(np.float32) * scale + out[:, 1::2] = ((b2 << 2) | ((b4 >> 4) & 0x03)).astype(np.float32) * scale + else: + # Colunas ímpares: p1, p3 em cada grupo. + out[:, 0::2] = ((b1 << 2) | ((b4 >> 2) & 0x03)).astype(np.float32) * scale + out[:, 1::2] = ((b3 << 2) | ((b4 >> 6) & 0x03)).astype(np.float32) * scale + + return out + + def _raw10_rgb_bayer_planes_to_rgb_float01_fast( + self, + packed_frame: np.ndarray, + width: int, + height: int, + bayer_pattern: str | None = None, + bit_depth: int = 10, + ) -> np.ndarray: + """ + RGB Bayer RAW10 packed -> RGB HWC float32 0..1 usando bayer_planes. + + Retorna half-res, exatamente como bayer_planes_to_rgb_linear() fazia: + shape = (height//2, width//2, 3) + canais = RGB + + Vantagem: + evita criar raw16 full-res e evita fatiar raw16 depois. + + Padrões suportados: + RGGB, BGGR, GRBG, GBRG + """ + width = int(width) + height = int(height) + bit_depth = int(bit_depth) + p = str(bayer_pattern or self.bayer_pattern or "RGGB").upper() + + if height % 2 != 0 or width % 2 != 0: + raise ValueError(f"Bayer planes exige width/height pares. Veio {width}x{height}") + + groups = self._raw10_groups_view_fast(packed_frame, width, height) + + even_rows = groups[0::2] + odd_rows = groups[1::2] + + max_val = float((1 << bit_depth) - 1) + scale = np.float32(1.0 / max_val) + + even_even = self._raw10_unpack_cols_parity_to_float01_fast(even_rows, width, parity=0, scale=scale) + even_odd = self._raw10_unpack_cols_parity_to_float01_fast(even_rows, width, parity=1, scale=scale) + odd_even = self._raw10_unpack_cols_parity_to_float01_fast(odd_rows, width, parity=0, scale=scale) + odd_odd = self._raw10_unpack_cols_parity_to_float01_fast(odd_rows, width, parity=1, scale=scale) + + if p == "RGGB": + r = even_even + g1 = even_odd + g2 = odd_even + b = odd_odd + elif p == "BGGR": + b = even_even + g1 = even_odd + g2 = odd_even + r = odd_odd + elif p == "GRBG": + g1 = even_even + r = even_odd + b = odd_even + g2 = odd_odd + elif p == "GBRG": + g1 = even_even + b = even_odd + r = odd_even + g2 = odd_odd + else: + raise ValueError(f"Padrão Bayer não suportado: {p}") + + g = (g1 + g2) * np.float32(0.5) + + rgb = np.empty((height // 2, width // 2, 3), dtype=np.float32) + rgb[:, :, 0] = r + rgb[:, :, 1] = g + rgb[:, :, 2] = b + + rgb_cal = getattr(self, "rgb_calibration", {}) or {} + if rgb_cal.get("enabled", False): + gains = rgb_cal.get("gains", {}) or {} + rgb[:, :, 0] *= float(gains.get("R", 1.0)) + rgb[:, :, 1] *= float(gains.get("G", 1.0)) + rgb[:, :, 2] *= float(gains.get("B", 1.0)) + + return np.clip(rgb, 0.0, 1.0).astype(np.float32, copy=False) + + + + def _radiometric_get_writable_float32_image(self, img): + """ + Garante uma imagem float32 gravável. + + Se já for float32 e writeable, usa a própria referência. + Se não, faz uma única cópia/conversão. + """ + if img is None: + return None, False + + if img.dtype == np.float32 and img.flags.writeable: + return img, True + + return img.astype(np.float32, copy=True), False + + def _apply_radiometric_scale_inplace(self, img, scale: float, clip_output: bool): + """ + Aplica escala radiométrica com o mínimo possível de alocação. + + Retorna: + out, reused_input + """ + out, reused = self._radiometric_get_writable_float32_image(img) + + if out is None: + return img, False + + scale = float(scale) + + # Se a escala é praticamente 1 e não precisa clipar, não faz nada. + if abs(scale - 1.0) <= 1e-6 and not clip_output: + return out, reused + + # Multiplicação in-place. + if abs(scale - 1.0) > 1e-6: + np.multiply(out, np.float32(scale), out=out, casting="unsafe") + + # Clip in-place, se configurado. + if clip_output: + np.clip(out, 0.0, 1.0, out=out) + + return out, reused + + def _build_radiometric_debug(self, method, role, cam_id, actual_ctrl, ref_ctrl, actual_factor, ref_factor, raw_scale, scale, clip_output): + return { + "applied": True, + "method": method, + "role": role, + "camera_id": cam_id, + "actual_controls": dict(actual_ctrl), + "reference_controls": dict(ref_ctrl), + "actual_factor": float(actual_factor), + "reference_factor": float(ref_factor), + "scale_raw": float(raw_scale), + "scale_applied": float(scale), + "clip_output": bool(clip_output), + } + + + + def _bayer_pattern_to_code_fast(self, bayer_pattern: str | None) -> int: + p = str(bayer_pattern or self.bayer_pattern or "RGGB").upper() + if p == "RGGB": + return 0 + if p == "BGGR": + return 1 + if p == "GRBG": + return 2 + if p == "GBRG": + return 3 + raise ValueError(f"Padrão Bayer não suportado: {p}") + + def _get_rgb_calibration_gains_fast(self): + rgb_cal = getattr(self, "rgb_calibration", {}) or {} + if not rgb_cal.get("enabled", False): + return 1.0, 1.0, 1.0 + + gains = rgb_cal.get("gains", {}) or {} + return ( + float(gains.get("R", 1.0)), + float(gains.get("G", 1.0)), + float(gains.get("B", 1.0)), + ) + + def _prepare_raw10_packed_view_numba(self, packed_frame: np.ndarray, width: int, height: int) -> np.ndarray: + """ + Retorna uma view/cópia contígua do RAW10 packed útil. + O Numba gosta de array C-contiguous. + """ + arr = self._prepare_raw10_packed_view_fast(packed_frame, width, height) + + if arr.dtype != np.uint8: + arr = arr.astype(np.uint8, copy=False) + + if not arr.flags.c_contiguous: + arr = np.ascontiguousarray(arr) + + return arr + + def _raw10_mono_to_float01_aggressive( + self, + packed_frame: np.ndarray, + width: int, + height: int, + bit_depth: int = 10, + ) -> np.ndarray: + """ + RAW10 mono -> float32 0..1. + Usa Numba se disponível, fallback para NumPy fast. + """ + if not _HAS_NUMBA: + return self._raw10_unpack_mono_to_float01_fast( + packed_frame, + width=width, + height=height, + bit_depth=bit_depth, + ) + + width = int(width) + height = int(height) + bit_depth = int(bit_depth) + + packed = self._prepare_raw10_packed_view_numba(packed_frame, width, height) + + max_val = float((1 << bit_depth) - 1) + scale = np.float32(1.0 / max_val) + + out = np.empty((height, width), dtype=np.float32) + _raw10_mono_to_float32_numba(packed, out, width, height, scale) + + return out + + def _raw10_rgb_bayer_planes_to_rgb_float01_aggressive( + self, + packed_frame: np.ndarray, + width: int, + height: int, + bayer_pattern: str | None = None, + bit_depth: int = 10, + ) -> np.ndarray: + """ + RAW10 RGB Bayer -> RGB HWC float32 0..1 usando bayer_planes. + Usa Numba se disponível, fallback para NumPy fast. + """ + if not _HAS_NUMBA: + return self._raw10_rgb_bayer_planes_to_rgb_float01_fast( + packed_frame, + width=width, + height=height, + bayer_pattern=bayer_pattern, + bit_depth=bit_depth, + ) + + width = int(width) + height = int(height) + bit_depth = int(bit_depth) + + if width % 2 != 0 or height % 2 != 0: + raise ValueError(f"Bayer planes exige width/height pares. Veio {width}x{height}") + + packed = self._prepare_raw10_packed_view_numba(packed_frame, width, height) + + max_val = float((1 << bit_depth) - 1) + scale = np.float32(1.0 / max_val) + + pattern_code = self._bayer_pattern_to_code_fast(bayer_pattern) + gain_r, gain_g, gain_b = self._get_rgb_calibration_gains_fast() + + out = np.empty((height // 2, width // 2, 3), dtype=np.float32) + + _raw10_bayer_planes_to_rgb_numba( + packed, + out, + width, + height, + scale, + int(pattern_code), + float(gain_r), + float(gain_g), + float(gain_b), + ) + + return out + + def _raw10_to_raw16_aggressive( + self, + packed_frame: np.ndarray, + width: int, + height: int, + ) -> np.ndarray: + """ + RAW10 packed -> raw16 uint16. + Usa Numba quando disponível. + É pensado para o linear_demosaic, porque cv2.cvtColor precisa de raw16. + """ + width = int(width) + height = int(height) + + if not _HAS_NUMBA: + return self.unpack_raw10_packed( + packed_frame, + sensor_width=width, + sensor_height=height, + ) + + packed = self._prepare_raw10_packed_view_numba( + packed_frame, + width, + height, + ) + + out = np.empty((height, width), dtype=np.uint16) + _raw10_to_raw16_numba(packed, out, width, height) + + return out + + def _rgb16_to_float32_gain_clip_aggressive( + self, + rgb16: np.ndarray, + bit_depth: int, + ) -> np.ndarray: + """ + RGB uint16 -> RGB float32 0..1 com calibração e clip em uma passada. + """ + h, w = rgb16.shape[:2] + + gain_r, gain_g, gain_b = self._get_rgb_calibration_gains_fast() + scale = np.float32(1.0 / float((1 << int(bit_depth)) - 1)) + + if not _HAS_NUMBA: + rgb = rgb16.astype(np.float32) + rgb[:, :, 0] *= np.float32(scale * gain_r) + rgb[:, :, 1] *= np.float32(scale * gain_g) + rgb[:, :, 2] *= np.float32(scale * gain_b) + np.clip(rgb, 0.0, 1.0, out=rgb) + return rgb.astype(np.float32, copy=False) + + rgb16_c = rgb16 + if not rgb16_c.flags.c_contiguous: + rgb16_c = np.ascontiguousarray(rgb16_c) + + out = np.empty((h, w, 3), dtype=np.float32) + + _rgb16_to_float32_gain_clip_numba( + rgb16_c, + out, + int(h), + int(w), + np.float32(scale), + float(gain_r), + float(gain_g), + float(gain_b), + ) + + return out + + def _apply_flat_gain_simple_aggressive( + self, + img: np.ndarray, + gain_eff: np.ndarray, + clip_output: bool = True, + ) -> np.ndarray: + base = img.astype(np.float32, copy=False) + + if gain_eff.shape[:2] != base.shape[:2]: + gain_eff = cv2.resize( + gain_eff.astype(np.float32, copy=False), + (base.shape[1], base.shape[0]), + interpolation=cv2.INTER_LINEAR, + ) + + if not _HAS_NUMBA: + out = base * gain_eff + if clip_output: + np.clip(out, 0.0, 1.0, out=out) + return out.astype(np.float32, copy=False) + + if not base.flags.c_contiguous: + base = np.ascontiguousarray(base) + + gain_c = gain_eff.astype(np.float32, copy=False) + if not gain_c.flags.c_contiguous: + gain_c = np.ascontiguousarray(gain_c) + + h, w = base.shape[:2] + out = np.empty((h, w), dtype=np.float32) + + _apply_flat_gain_numba( + base, + gain_c, + out, + int(h), + int(w), + bool(clip_output), + ) + + return out + + + + def _direct_fusion_get_target_size_fast(self, ref_size): + """ + Resolve target_size final como (target_w, target_h). + Usa fusion_config.target_size se existir, senão usa tamanho de referência. + """ + ref_h, ref_w = int(ref_size[0]), int(ref_size[1]) + fusion = getattr(self, "fusion_config", {}) or {} + + target_size = fusion.get("target_size", None) + if isinstance(target_size, (list, tuple)) and len(target_size) == 2: + return int(target_size[0]), int(target_size[1]) + + return int(ref_w), int(ref_h) + + def _direct_fusion_get_crop_box_fast(self, valid_masks, ref_size): + """ + Calcula crop_box comum se crop_valid_common=true. + Se não houver crop, usa frame inteiro da referência. + """ + ref_h, ref_w = int(ref_size[0]), int(ref_size[1]) + cfg = getattr(self, "fusion_config", {}) or {} + + if cfg.get("crop_valid_common", False): + crop_box = self._compute_common_crop_box(valid_masks) + if crop_box is not None: + return tuple(int(v) for v in crop_box), True + + return (0, 0, ref_w, ref_h), False + + def _direct_fusion_crop_to_target_matrix_fast(self, crop_box, target_size): + """ + Matriz C que leva coordenadas do espaço RGB/ref para a saída final. + + crop_box está no espaço da imagem de referência: + x0,y0,x1,y1 + + Queremos: + x=x0 -> 0 + x=x1 -> target_w + y=y0 -> 0 + y=y1 -> target_h + + Retorna C_ref_to_target. + """ + x0, y0, x1, y1 = [float(v) for v in crop_box] + target_w, target_h = int(target_size[0]), int(target_size[1]) + + crop_w = max(1.0, x1 - x0) + crop_h = max(1.0, y1 - y0) + + sx = float(target_w) / crop_w + sy = float(target_h) / crop_h + + C = np.array( + [ + [sx, 0.0, -x0 * sx], + [0.0, sy, -y0 * sy], + [0.0, 0.0, 1.0], + ], + dtype=np.float32, + ) + + return C + + def _direct_fusion_scale_homography_for_ref_fast(self, H, meta, ref_size): + """ + Escala a homografia calibrada para o runtime da referência RGB. + + Usa a própria função existente _scale_homography_to_runtime() se existir. + Isso mantém compatibilidade com o contrato atual do core. + """ + fusion = getattr(self, "fusion_config", {}) or {} + ref_h, ref_w = int(ref_size[0]), int(ref_size[1]) + + calib_size = ( + fusion.get("homography_calibration_size") + or fusion.get("calibration_size") + or fusion.get("source_size") + or None + ) + + H = np.asarray(H, dtype=np.float32) + + if hasattr(self, "_scale_homography_to_runtime"): + try: + return self._scale_homography_to_runtime( + H, + calib_size=calib_size, + runtime_size=(ref_w, ref_h), + ).astype(np.float32) + except TypeError: + try: + return self._scale_homography_to_runtime( + H, + calib_size, + (ref_w, ref_h), + ).astype(np.float32) + except Exception: + pass + except Exception: + pass + + # Fallback local. + if calib_size is None: + if abs(float(H[2, 2])) > 1e-9: + H = H / H[2, 2] + return H.astype(np.float32) + + calib_w, calib_h = float(calib_size[0]), float(calib_size[1]) + if calib_w <= 0 or calib_h <= 0: + return H.astype(np.float32) + + sx = float(ref_w) / calib_w + sy = float(ref_h) / calib_h + + S = np.array([[sx, 0.0, 0.0], [0.0, sy, 0.0], [0.0, 0.0, 1.0]], dtype=np.float32) + S_inv = np.array([[1.0 / sx, 0.0, 0.0], [0.0, 1.0 / sy, 0.0], [0.0, 0.0, 1.0]], dtype=np.float32) + + H_runtime = S @ H @ S_inv + if abs(float(H_runtime[2, 2])) > 1e-9: + H_runtime = H_runtime / H_runtime[2, 2] + + return H_runtime.astype(np.float32) + + def _direct_fusion_get_role_homography_fast(self, role, meta, ref_size): + """ + Retorna H_role_to_rgb escalada para o espaço da referência RGB. + """ + role = str(role).lower() + fusion = getattr(self, "fusion_config", {}) or {} + homographies = fusion.get("homographies", {}) or {} + + key = f"{role}_to_rgb" + H = homographies.get(key) + + if H is None: + # Fallbacks para contratos diferentes. + H = homographies.get(role) + + if H is None: + raise RuntimeError(f"Homografia ausente para role={role}. Esperado fusion_config.homographies.{key}") + + return self._direct_fusion_scale_homography_for_ref_fast(H, meta, ref_size) + + def _direct_fusion_resize_spec_to_ref_if_needed_fast(self, img, ref_size): + """ + Mantém compatibilidade com o fluxo atual: + se RE/NIR não estão no mesmo shape do RGB de referência, redimensiona para ref. + """ + ref_h, ref_w = int(ref_size[0]), int(ref_size[1]) + + if img.shape[:2] == (ref_h, ref_w): + return img + + return cv2.resize( + img.astype(np.float32, copy=False), + (ref_w, ref_h), + interpolation=cv2.INTER_LINEAR, + ) + + def _direct_fusion_compute_valid_masks_fast(self, decoded, role_to_cam, ref_size, meta): + """ + Calcula máscaras válidas no espaço RGB/ref para crop comum. + Usa warpPerspective apenas em máscara uint8, que costuma ser barato. + """ + ref_h, ref_w = int(ref_size[0]), int(ref_size[1]) + + valid_masks = [np.ones((ref_h, ref_w), dtype=np.uint8)] + + base = np.ones((ref_h, ref_w), dtype=np.uint8) * 255 + + for role in ("re", "nir"): + if role not in role_to_cam: + continue + + H = self._direct_fusion_get_role_homography_fast(role, meta, ref_size) + mask = cv2.warpPerspective( + base, + H, + (ref_w, ref_h), + flags=cv2.INTER_NEAREST, + borderMode=cv2.BORDER_CONSTANT, + borderValue=0, + ) + valid_masks.append(mask) + + return valid_masks + + def _direct_fusion_write_rgb_fast(self, tensor, rgb, crop_box, target_size): + """ + Escreve RGB no tensor final. + RGB entra HWC float32, sai CHW no tensor[0:3]. + """ + target_w, target_h = int(target_size[0]), int(target_size[1]) + x0, y0, x1, y1 = [int(v) for v in crop_box] + + rgb_crop = rgb[y0:y1, x0:x1, :].astype(np.float32, copy=False) + + if rgb_crop.shape[1] != target_w or rgb_crop.shape[0] != target_h: + rgb_out = cv2.resize( + rgb_crop, + (target_w, target_h), + interpolation=cv2.INTER_LINEAR, + ) + else: + rgb_out = rgb_crop + + tensor[0] = rgb_out[:, :, 0] + tensor[1] = rgb_out[:, :, 1] + tensor[2] = rgb_out[:, :, 2] + + def _direct_fusion_write_spec_fast(self, tensor, channel_index, img, role, C_ref_to_target, ref_size, target_size, meta): + """ + Escreve RE ou NIR direto no tensor final, compondo: + M = C_ref_to_target @ H_role_to_rgb + + img é primeiro redimensionada para ref_size se necessário, para manter o mesmo + comportamento geométrico do fluxo atual. + """ + target_w, target_h = int(target_size[0]), int(target_size[1]) + + img_ref = self._direct_fusion_resize_spec_to_ref_if_needed_fast(img, ref_size) + + H_role_to_rgb = self._direct_fusion_get_role_homography_fast(role, meta, ref_size) + M_role_to_target = (C_ref_to_target @ H_role_to_rgb).astype(np.float32) + + out = cv2.warpPerspective( + img_ref.astype(np.float32, copy=False), + M_role_to_target, + (target_w, target_h), + flags=cv2.INTER_LINEAR, + borderMode=cv2.BORDER_CONSTANT, + borderValue=0.0, + ) + + tensor[int(channel_index)] = out + + def _fuse_multispec_direct_to_target_fast(self, decoded, meta, channels_expected): + """ + Fusão direta otimizada com cache de geometria fixa. + """ + t0 = time.perf_counter() + + rgb_cam_id = self._find_cam_by_role(decoded, "rgb") + if rgb_cam_id is None: + raise RuntimeError("Fusão direta requer câmera com role='rgb' como referência") + + rgb = decoded[rgb_cam_id]["image"] + if rgb.ndim != 3 or rgb.shape[2] != 3: + raise RuntimeError(f"RGB inválido para fusão direta: shape={rgb.shape}") + + ref_h, ref_w = rgb.shape[:2] + ref_size = (int(ref_h), int(ref_w)) + target_size = self._direct_fusion_get_target_size_fast(ref_size) + target_w, target_h = int(target_size[0]), int(target_size[1]) + + role_to_cam = { + item.get("role", data.get("meta", {}).get("role")): cam_id + for cam_id, data in decoded.items() + for item in [data] + } + + # Cache da geometria fixa. + geom = self._direct_fusion_get_geometry_cached_fast( + decoded=decoded, + role_to_cam=role_to_cam, + ref_size=ref_size, + target_size=target_size, + meta=meta, + ) + + crop_box = geom["crop_box"] + crop_applied = bool(geom["crop_applied"]) + + self.last_fusion_result = { + "ref_shape": [int(ref_h), int(ref_w)], + "target_size": [int(target_w), int(target_h)], + "crop_valid_common": bool((self.fusion_config or {}).get("crop_valid_common", False)), + "resize_after_crop": bool((self.fusion_config or {}).get("resize_after_crop", False)), + "crop_box": [int(v) for v in crop_box], + "crop_applied": bool(crop_applied), + "roles": ["R", "G", "B", "RE", "NIR"], + "direct_fusion_fast": True, + "geometry_cache_hit": bool(geom.get("prepare_cache_hit", False)), + "geometry_cache_hits": int(geom.get("cache_hits", 0)), + "geometry_cache_misses": int(geom.get("cache_misses", 0)), + } + + tensor = np.empty((int(channels_expected), target_h, target_w), dtype=np.float32) + + t_prepare_ms = (time.perf_counter() - t0) * 1000.0 + + # ------------------------------------------------------------ + # Cache de remap + # ------------------------------------------------------------ + use_remap_cache = bool((self.fusion_config or {}).get("use_remap_cache", True)) + use_remap_for_rgb = bool((self.fusion_config or {}).get("use_remap_for_rgb", False)) + use_remap_for_spec = bool((self.fusion_config or {}).get("use_remap_for_spec", True)) + + t0_remap_cache = time.perf_counter() + + if use_remap_cache and (use_remap_for_rgb or use_remap_for_spec): + remap_cache = self._direct_fusion_get_remap_cached_fast( + decoded=decoded, + role_to_cam=role_to_cam, + geom=geom, + ref_size=ref_size, + target_size=target_size, + ) + else: + remap_cache = { + "maps": {}, + "cache_hit": False, + "cache_hits": 0, + "cache_misses": 0, + } + + t_remap_cache_ms = (time.perf_counter() - t0_remap_cache) * 1000.0 + + # ------------------------------------------------------------ + # RGB via remap + # ------------------------------------------------------------ + t0_rgb = time.perf_counter() + + rgb_maps = remap_cache["maps"].get("rgb") + if use_remap_cache and use_remap_for_rgb and rgb_maps is not None: + self._direct_fusion_write_rgb_remap_fast( + tensor=tensor, + rgb=rgb, + remap_entry=rgb_maps, + ) + else: + self._direct_fusion_write_rgb_fast( + tensor, + rgb, + crop_box, + target_size, + ) + + t_rgb_ms = (time.perf_counter() - t0_rgb) * 1000.0 + + warp_details = {} + t_warp_total_ms = 0.0 + + # ------------------------------------------------------------ + # RE via remap + # ------------------------------------------------------------ + if "re" in role_to_cam: + t0w = time.perf_counter() + + re_img = decoded[role_to_cam["re"]]["image"] + re_maps = remap_cache["maps"].get("re") + + if use_remap_cache and use_remap_for_spec and re_maps is not None: + self._direct_fusion_write_spec_remap_fast( + tensor=tensor, + channel_index=3, + img=re_img, + remap_entry=re_maps, + ) + else: + self._direct_fusion_write_spec_cached_fast( + tensor=tensor, + channel_index=3, + img=re_img, + role="re", + geom=geom, + ref_size=ref_size, + target_size=target_size, + ) + + warp_details["re"] = (time.perf_counter() - t0w) * 1000.0 + t_warp_total_ms += warp_details["re"] + + # ------------------------------------------------------------ + # NIR via remap + # ------------------------------------------------------------ + if "nir" in role_to_cam: + t0w = time.perf_counter() + + nir_img = decoded[role_to_cam["nir"]]["image"] + nir_maps = remap_cache["maps"].get("nir") + + if use_remap_cache and use_remap_for_spec and nir_maps is not None: + self._direct_fusion_write_spec_remap_fast( + tensor=tensor, + channel_index=4, + img=nir_img, + remap_entry=nir_maps, + ) + else: + self._direct_fusion_write_spec_cached_fast( + tensor=tensor, + channel_index=4, + img=nir_img, + role="nir", + geom=geom, + ref_size=ref_size, + target_size=target_size, + ) + + warp_details["nir"] = (time.perf_counter() - t0w) * 1000.0 + t_warp_total_ms += warp_details["nir"] + + # ------------------------------------------------------------ + # Flat-field no espaço final do tensor + # ------------------------------------------------------------ + t0_final_flat = time.perf_counter() + final_flat_ms = 0.0 + final_flat_enabled = False + final_flat_cache_available = False + + flat_cfg = self.flatfield_config or {} + apply_final_flat = ( + bool(flat_cfg.get("enabled", False)) + and str(flat_cfg.get("apply_space", "native_camera_space")).lower() == "final_tensor_space" + ) + + if apply_final_flat: + final_flat_enabled = True + + gain_tensor = self._get_final_flat_gain_tensor_cached_fast( + decoded=decoded, + role_to_cam=role_to_cam, + geom=geom, + remap_cache=remap_cache, + crop_box=crop_box, + target_size=target_size, + ) + + if gain_tensor is not None: + final_flat_cache_available = True + tensor = self._apply_final_flat_gain_tensor_inplace( + tensor=tensor, + gain_tensor=gain_tensor, + clip_output=bool(flat_cfg.get("clip_output", True)), + ) + + final_flat_ms = (time.perf_counter() - t0_final_flat) * 1000.0 + + if tensor.shape[0] != channels_expected: + raise RuntimeError( + f"Tensor direto com canais inesperados: {tensor.shape[0]} | esperado={channels_expected}" + ) + + self.last_fusion_result["output_shape"] = list(tensor.shape) + + perf = { + "prepare_ms": float(t_prepare_ms), + "rgb_crop_resize_ms": float(t_rgb_ms), + "warp_total_ms": float(t_warp_total_ms), + "warp_details_ms": warp_details, + "crop_resize_ms": float(t_rgb_ms), + "concat_ms": 0.0, + "spatial_direct_ms": float(t_rgb_ms + t_warp_total_ms), + "geometry_cache_hit": bool(geom.get("prepare_cache_hit", False)), + "geometry_cache_hits": int(geom.get("cache_hits", 0)), + "geometry_cache_misses": int(geom.get("cache_misses", 0)), + "remap_cache_ms": float(t_remap_cache_ms), + "remap_cache_hit": bool(remap_cache.get("cache_hit", False)), + "remap_cache_hits": int(remap_cache.get("cache_hits", 0)), + "remap_cache_misses": int(remap_cache.get("cache_misses", 0)), + "remap_enabled": bool(use_remap_cache), + "remap_rgb_enabled": bool(use_remap_cache and use_remap_for_rgb), + "remap_spec_enabled": bool(use_remap_cache and use_remap_for_spec), + "final_flat_enabled": bool(final_flat_enabled), + "final_flat_cache_available": bool(final_flat_cache_available), + "final_flat_ms": float(final_flat_ms), + } + + return tensor, perf + + + + def _direct_fusion_get_geometry_cache_key_fast(self, ref_size, target_size, role_to_cam): + """ + Chave simples e estável para cache da geometria. + + A geometria depende de: + - tamanho do RGB de referência + - target final + - roles presentes + - crop_valid_common / resize_after_crop + - homografias e calibration_size + + Para evitar custo de serializar o JSON todo por frame, usamos uma versão + simples. Se você editar module_params em runtime, chame + clear_direct_fusion_geometry_cache(). + """ + ref_h, ref_w = int(ref_size[0]), int(ref_size[1]) + target_w, target_h = int(target_size[0]), int(target_size[1]) + + fusion = getattr(self, "fusion_config", {}) or {} + homographies = fusion.get("homographies", {}) or {} + + # Pequena assinatura numérica das homografias. + def h_sig(key): + H = homographies.get(key) + if H is None: + return None + arr = np.asarray(H, dtype=np.float32).reshape(-1) + # arredonda para evitar ruído float/json, mas detecta mudança real. + return tuple(np.round(arr, 8).tolist()) + + roles = tuple(sorted([str(r).lower() for r in role_to_cam.keys()])) + + return ( + ref_w, + ref_h, + target_w, + target_h, + roles, + bool(fusion.get("crop_valid_common", False)), + bool(fusion.get("resize_after_crop", False)), + tuple(fusion.get("homography_calibration_size") or fusion.get("calibration_size") or []), + h_sig("re_to_rgb"), + h_sig("nir_to_rgb"), + ) + + def clear_direct_fusion_geometry_cache(self): + """ + Chame se mudar fusion_config/module_params em runtime. + """ + self._direct_fusion_geometry_cache = {} + self._direct_fusion_geometry_cache_hits = 0 + self._direct_fusion_geometry_cache_misses = 0 + + self._direct_fusion_remap_cache = {} + self._direct_fusion_remap_cache_hits = 0 + self._direct_fusion_remap_cache_misses = 0 + + def _direct_fusion_get_geometry_cached_fast(self, decoded, role_to_cam, ref_size, target_size, meta): + """ + Retorna geometria cacheada para a fusão direta. + + Saída: + geom = { + key, + crop_box, + crop_applied, + C_ref_to_target, + H_role_to_rgb: {re,nir}, + M_role_to_target: {re,nir}, + valid_masks, # opcional/debug + prepare_cache_hit, + } + """ + if not hasattr(self, "_direct_fusion_geometry_cache"): + self.clear_direct_fusion_geometry_cache() + + key = self._direct_fusion_get_geometry_cache_key_fast(ref_size, target_size, role_to_cam) + cache = self._direct_fusion_geometry_cache + + if key in cache: + self._direct_fusion_geometry_cache_hits += 1 + geom = cache[key] + geom["prepare_cache_hit"] = True + geom["cache_hits"] = int(self._direct_fusion_geometry_cache_hits) + geom["cache_misses"] = int(self._direct_fusion_geometry_cache_misses) + return geom + + self._direct_fusion_geometry_cache_misses += 1 + + ref_h, ref_w = int(ref_size[0]), int(ref_size[1]) + + # ------------------------------------------------------------ + # Homografias escaladas para runtime. + # ------------------------------------------------------------ + H_role_to_rgb = {} + for role in ("re", "nir"): + if role in role_to_cam: + H_role_to_rgb[role] = self._direct_fusion_get_role_homography_fast(role, meta, ref_size) + + # ------------------------------------------------------------ + # Máscaras válidas e crop comum. + # Essa era uma das partes caras e totalmente fixa. + # ------------------------------------------------------------ + valid_masks = [np.ones((ref_h, ref_w), dtype=np.uint8)] + base = np.ones((ref_h, ref_w), dtype=np.uint8) * 255 + + for role in ("re", "nir"): + if role not in H_role_to_rgb: + continue + + mask = cv2.warpPerspective( + base, + H_role_to_rgb[role], + (ref_w, ref_h), + flags=cv2.INTER_NEAREST, + borderMode=cv2.BORDER_CONSTANT, + borderValue=0, + ) + valid_masks.append(mask) + + crop_box, crop_applied = self._direct_fusion_get_crop_box_fast(valid_masks, ref_size) + + # ------------------------------------------------------------ + # Matriz crop RGB/ref -> target final. + # ------------------------------------------------------------ + C_ref_to_target = self._direct_fusion_crop_to_target_matrix_fast(crop_box, target_size) + + # ------------------------------------------------------------ + # Matrizes compostas role original/ref -> target. + # ------------------------------------------------------------ + M_role_to_target = {} + for role, H in H_role_to_rgb.items(): + M_role_to_target[role] = (C_ref_to_target @ H).astype(np.float32) + + geom = { + "key": key, + "crop_box": tuple(int(v) for v in crop_box), + "crop_applied": bool(crop_applied), + "C_ref_to_target": C_ref_to_target.astype(np.float32), + "H_role_to_rgb": H_role_to_rgb, + "M_role_to_target": M_role_to_target, + "valid_masks": valid_masks, + "prepare_cache_hit": False, + "cache_hits": int(self._direct_fusion_geometry_cache_hits), + "cache_misses": int(self._direct_fusion_geometry_cache_misses), + } + + # Cache pequeno: normalmente só uma geometria. Se mudar resolução/config, + # evita crescimento infinito. + if len(cache) > 4: + cache.clear() + + cache[key] = geom + return geom + + def _direct_fusion_write_spec_cached_fast(self, tensor, channel_index, img, role, geom, ref_size, target_size): + """ + Escreve RE/NIR usando matriz composta cacheada. + """ + target_w, target_h = int(target_size[0]), int(target_size[1]) + + img_ref = self._direct_fusion_resize_spec_to_ref_if_needed_fast(img, ref_size) + + M_role_to_target = geom["M_role_to_target"].get(str(role).lower()) + if M_role_to_target is None: + raise RuntimeError(f"Matriz composta ausente para role={role}") + + out = cv2.warpPerspective( + img_ref.astype(np.float32, copy=False), + M_role_to_target, + (target_w, target_h), + flags=cv2.INTER_LINEAR, + borderMode=cv2.BORDER_CONSTANT, + borderValue=0.0, + ) + + tensor[int(channel_index)] = out + + def _direct_fusion_build_remap_from_src_to_dst_fast( + self, + M_src_to_dst: np.ndarray, + src_shape: tuple, + dst_size: tuple, + ref_shape_for_scaled_src: tuple | None = None, + ): + """ + Cria mapas cacheáveis para cv2.remap. + + M_src_to_dst: + matriz 3x3 que leva coordenadas da imagem fonte para o destino final. + + src_shape: + shape real da imagem fonte, ex: img.shape[:2] + + dst_size: + (target_w, target_h) + + ref_shape_for_scaled_src: + usado quando a matriz foi calculada no espaço ref, mas a imagem fonte + real tem outro tamanho. Ex: spec original 1280x800, ref RGB half 640x400. + """ + src_h, src_w = int(src_shape[0]), int(src_shape[1]) + target_w, target_h = int(dst_size[0]), int(dst_size[1]) + + M = np.asarray(M_src_to_dst, dtype=np.float32) + if M.shape != (3, 3): + raise RuntimeError(f"M_src_to_dst inválida: shape={M.shape}") + + M_inv = np.linalg.inv(M).astype(np.float32) + + xs, ys = np.meshgrid( + np.arange(target_w, dtype=np.float32), + np.arange(target_h, dtype=np.float32), + ) + + den = M_inv[2, 0] * xs + M_inv[2, 1] * ys + M_inv[2, 2] + den = np.where(np.abs(den) < 1e-9, 1e-9, den) + + map_x = (M_inv[0, 0] * xs + M_inv[0, 1] * ys + M_inv[0, 2]) / den + map_y = (M_inv[1, 0] * xs + M_inv[1, 1] * ys + M_inv[1, 2]) / den + + # Se a matriz foi calculada no espaço de referência, mas a imagem fonte + # real tem outro tamanho, converte coordenada ref -> coordenada fonte real. + if ref_shape_for_scaled_src is not None: + ref_h, ref_w = int(ref_shape_for_scaled_src[0]), int(ref_shape_for_scaled_src[1]) + + if (src_w, src_h) != (ref_w, ref_h): + sx = float(src_w) / float(ref_w) + sy = float(src_h) / float(ref_h) + + # Aproxima a convenção de resize do OpenCV: + # x_src = (x_ref + 0.5) * scale - 0.5 + map_x = (map_x + 0.5) * sx - 0.5 + map_y = (map_y + 0.5) * sy - 0.5 + + map_x = map_x.astype(np.float32, copy=False) + map_y = map_y.astype(np.float32, copy=False) + + # convertMaps deixa o remap mais barato em muitos casos. + map1, map2 = cv2.convertMaps(map_x, map_y, cv2.CV_16SC2) + + return { + "map_x": map_x, + "map_y": map_y, + "map1": map1, + "map2": map2, + "src_shape": [src_h, src_w], + "dst_size": [target_w, target_h], + } + + def _direct_fusion_get_remap_cache_key_fast( + self, + geom: dict, + decoded: dict, + role_to_cam: dict, + ref_size: tuple, + target_size: tuple, + ): + """ + Chave do cache de remap. + + Precisa considerar: + - geometria base; + - crop/homografia; + - tamanho real das imagens fonte; + - target final. + """ + role_shapes = {} + + for role, cam_id in role_to_cam.items(): + img = decoded[cam_id]["image"] + role_shapes[str(role).lower()] = tuple(int(v) for v in img.shape[:2]) + + return ( + geom.get("key"), + tuple(int(v) for v in ref_size), + tuple(int(v) for v in target_size), + tuple(sorted(role_shapes.items())), + ) + + def _direct_fusion_get_remap_cached_fast( + self, + decoded: dict, + role_to_cam: dict, + geom: dict, + ref_size: tuple, + target_size: tuple, + ): + """ + Retorna mapas de remap cacheados para RGB, RE e NIR. + """ + if not hasattr(self, "_direct_fusion_remap_cache"): + self._direct_fusion_remap_cache = {} + self._direct_fusion_remap_cache_hits = 0 + self._direct_fusion_remap_cache_misses = 0 + + key = self._direct_fusion_get_remap_cache_key_fast( + geom=geom, + decoded=decoded, + role_to_cam=role_to_cam, + ref_size=ref_size, + target_size=target_size, + ) + + cache = self._direct_fusion_remap_cache + + if key in cache: + self._direct_fusion_remap_cache_hits += 1 + remap = cache[key] + remap["cache_hit"] = True + remap["cache_hits"] = int(self._direct_fusion_remap_cache_hits) + remap["cache_misses"] = int(self._direct_fusion_remap_cache_misses) + return remap + + self._direct_fusion_remap_cache_misses += 1 + + target_w, target_h = int(target_size[0]), int(target_size[1]) + + remap = { + "key": key, + "maps": {}, + "cache_hit": False, + "cache_hits": int(self._direct_fusion_remap_cache_hits), + "cache_misses": int(self._direct_fusion_remap_cache_misses), + } + + # ------------------------------------------------------------ + # RGB: matriz C_ref_to_target leva RGB/ref -> target. + # ------------------------------------------------------------ + rgb_cam_id = role_to_cam.get("rgb") + if rgb_cam_id is not None: + rgb_img = decoded[rgb_cam_id]["image"] + C_ref_to_target = geom["C_ref_to_target"] + + remap["maps"]["rgb"] = self._direct_fusion_build_remap_from_src_to_dst_fast( + M_src_to_dst=C_ref_to_target, + src_shape=rgb_img.shape[:2], + dst_size=(target_w, target_h), + ref_shape_for_scaled_src=None, + ) + + # ------------------------------------------------------------ + # RE/NIR: matriz composta M_role_to_target leva role/ref -> target. + # Se a imagem fonte não tiver o mesmo tamanho do ref, escalamos o mapa. + # ------------------------------------------------------------ + for role in ("re", "nir"): + cam_id = role_to_cam.get(role) + if cam_id is None: + continue + + img = decoded[cam_id]["image"] + M_role_to_target = geom["M_role_to_target"].get(role) + + if M_role_to_target is None: + continue + + remap["maps"][role] = self._direct_fusion_build_remap_from_src_to_dst_fast( + M_src_to_dst=M_role_to_target, + src_shape=img.shape[:2], + dst_size=(target_w, target_h), + ref_shape_for_scaled_src=ref_size, + ) + + if len(cache) > 4: + cache.clear() + + cache[key] = remap + return remap + + def _direct_fusion_write_rgb_remap_fast( + self, + tensor: np.ndarray, + rgb: np.ndarray, + remap_entry: dict, + ): + """ + RGB HWC -> tensor CHW usando cv2.remap direto para target final. + """ + map1 = remap_entry["map1"] + map2 = remap_entry["map2"] + + rgb_out = cv2.remap( + rgb.astype(np.float32, copy=False), + map1, + map2, + interpolation=cv2.INTER_LINEAR, + borderMode=cv2.BORDER_CONSTANT, + borderValue=0.0, + ) + + tensor[0] = rgb_out[:, :, 0] + tensor[1] = rgb_out[:, :, 1] + tensor[2] = rgb_out[:, :, 2] + + def _direct_fusion_write_spec_remap_fast( + self, + tensor: np.ndarray, + channel_index: int, + img: np.ndarray, + remap_entry: dict, + ): + """ + RE/NIR -> tensor usando cv2.remap direto para target final. + """ + map1 = remap_entry["map1"] + map2 = remap_entry["map2"] + + out = cv2.remap( + img.astype(np.float32, copy=False), + map1, + map2, + interpolation=cv2.INTER_LINEAR, + borderMode=cv2.BORDER_CONSTANT, + borderValue=0.0, + ) + + tensor[int(channel_index)] = out + + + + def _raw10_rgb_linear_demosaic_to_rgb_float01_fast( + self, + packed_frame: np.ndarray, + width: int, + height: int, + bayer_pattern: str | None = None, + bit_depth: int = 10, + ) -> np.ndarray: + """ + RAW10 RGB Bayer packed -> RGB HWC float32 0..1 usando demosaic OpenCV. + + Agora com telemetria interna: + - unpack_ms + - cvtColor_ms + - float_ms + - calibration_ms + - clip_ms + - resize_half_ms + - total_ms + """ + t_total0 = time.perf_counter() + + width = int(width) + height = int(height) + bit_depth = int(bit_depth) + + rgb_cfg = getattr(self, "rgb_processing_config", {}) or {} + rgb_mode = str(rgb_cfg.get("mode", "linear_demosaic")).lower() + algorithm = str(rgb_cfg.get("demosaic_algorithm", "ea")).lower() + + perf = { + "mode": rgb_mode, + "algorithm": algorithm, + "width": width, + "height": height, + } + + # ------------------------------------------------------------ + # 1) Resolve código Bayer OpenCV + # ------------------------------------------------------------ + t0 = time.perf_counter() + + cv_code, algorithm_resolved = self._get_bayer_cv2_code( + bayer_pattern=bayer_pattern, + algorithm=algorithm, + ) + + perf["resolve_code_ms"] = (time.perf_counter() - t0) * 1000.0 + perf["algorithm_resolved"] = algorithm_resolved + + # ------------------------------------------------------------ + # 2) RAW10 packed -> raw16 full-res + # ------------------------------------------------------------ + t0 = time.perf_counter() + + raw16 = self._raw10_to_raw16_aggressive( + packed_frame, + width=width, + height=height, + ) + + perf["unpack_ms"] = (time.perf_counter() - t0) * 1000.0 + + # ------------------------------------------------------------ + # 3) Demosaic OpenCV + # ------------------------------------------------------------ + t0 = time.perf_counter() + + rgb16 = cv2.cvtColor(raw16, cv_code) + + perf["cvtColor_ms"] = (time.perf_counter() - t0) * 1000.0 + + # ------------------------------------------------------------ + # 4) uint16 -> float32 0..1 + # ------------------------------------------------------------ + t0 = time.perf_counter() + + rgb = self._rgb16_to_float32_gain_clip_aggressive( + rgb16, + bit_depth=bit_depth, + ) + + perf["float_calib_clip_ms"] = (time.perf_counter() - t0) * 1000.0 + perf["calibration_enabled"] = bool((getattr(self, "rgb_calibration", {}) or {}).get("enabled", False)) + perf["float_ms"] = 0.0 + perf["calibration_ms"] = 0.0 + perf["clip_ms"] = 0.0 + + # ------------------------------------------------------------ + # 7) Half mode, se habilitado + # ------------------------------------------------------------ + t0 = time.perf_counter() + + if rgb_mode in ("linear_demosaic_half", "demosaic_half", "full_demosaic_half"): + rgb = cv2.resize( + rgb, + (width // 2, height // 2), + interpolation=cv2.INTER_AREA, + ).astype(np.float32, copy=False) + perf["resize_half_applied"] = True + else: + perf["resize_half_applied"] = False + + perf["resize_half_ms"] = (time.perf_counter() - t0) * 1000.0 + + # ------------------------------------------------------------ + # Total + # ------------------------------------------------------------ + perf["total_ms"] = (time.perf_counter() - t_total0) * 1000.0 + perf["out_shape"] = list(rgb.shape) + perf["out_dtype"] = str(rgb.dtype) + + self._set_decode_perf("rgb", perf) + + return rgb.astype(np.float32, copy=False) + + def demosaic_raw16_to_rgb_linear_hwc_fast( + self, + raw16: np.ndarray, + bit_depth: int = 10, + ) -> np.ndarray: + rgb_cfg = getattr(self, "rgb_processing_config", {}) or {} + algorithm = str(rgb_cfg.get("demosaic_algorithm", "ea")).lower() + + cv_code, _ = self._get_bayer_cv2_code( + bayer_pattern=self.bayer_pattern, + algorithm=algorithm, + ) + + if raw16.dtype != np.uint16: + raw16 = raw16.astype(np.uint16, copy=False) + + rgb16 = cv2.cvtColor(raw16, cv_code) + + rgb = rgb16.astype(np.float32) + rgb *= np.float32(1.0 / float((1 << bit_depth) - 1)) + + np.clip(rgb, 0.0, 1.0, out=rgb) + + return rgb.astype(np.float32, copy=False) + + def _get_bayer_cv2_code(self, bayer_pattern: str | None, algorithm: str = "ea"): + """ + Retorna o código OpenCV para demosaic Bayer. + + algorithm: + - "ea": Edge-Aware, melhor qualidade, mais pesado + - "bilinear": mais rápido, menor custo + """ + p = str(bayer_pattern or self.bayer_pattern or "RGGB").upper() + algo = str(algorithm or "ea").lower() + + if algo in ("bilinear", "linear", "fast", "normal"): + code_map = { + "BGGR": cv2.COLOR_BayerRG2RGB, + "RGGB": cv2.COLOR_BayerBG2RGB, + "GRBG": cv2.COLOR_BayerGR2RGB, + "GBRG": cv2.COLOR_BayerGB2RGB, + } + elif algo in ("ea", "edge_aware", "edge-aware"): + code_map = { + "BGGR": cv2.COLOR_BayerRG2RGB_EA, + "RGGB": cv2.COLOR_BayerBG2RGB_EA, + "GRBG": cv2.COLOR_BayerGR2RGB_EA, + "GBRG": cv2.COLOR_BayerGB2RGB_EA, + } + else: + raise ValueError(f"demosaic_algorithm inválido: {algorithm}") + + if p not in code_map: + raise ValueError(f"Padrão Bayer não suportado para demosaic: {p}") + + return code_map[p], algo + + + + def _set_decode_perf(self, role: str, perf: dict, log_interval_s: float = 1.0): + """ + Guarda telemetria do decode por role e loga no máximo 1x por segundo. + """ + role = str(role or "unknown").lower() + + if not hasattr(self, "last_decode_perf") or self.last_decode_perf is None: + self.last_decode_perf = {} + + self.last_decode_perf[role] = dict(perf) + + now = time.time() + if not hasattr(self, "_last_decode_perf_log_ts"): + self._last_decode_perf_log_ts = 0.0 + + if now - self._last_decode_perf_log_ts < log_interval_s: + return + + self._last_decode_perf_log_ts = now + + #parts = [] + #for k, v in perf.items(): + # if isinstance(v, (int, float)): + # parts.append(f"{k}={float(v):.2f}ms") + # else: + # parts.append(f"{k}={v}") + #print(f"[PERF][DECODE][{role.upper()}] " + " ".join(parts)) + + + + def _get_runtime_gain_eff_map(self, channel_name: str, base_shape: tuple, cfg: dict): + ch = str(channel_name).upper() + + h, w = base_shape[:2] + + strength = float(cfg.get("strength", 1.0)) + strength_by_channel = cfg.get("strength_by_channel", {}) or {} + if ch in strength_by_channel: + strength = float(strength_by_channel[ch]) + + gain_min_runtime = float(cfg.get("gain_min_runtime", 0.0)) + gain_max_runtime = float(cfg.get("gain_max_runtime", 999.0)) + + runtime_smooth_ksize = int(cfg.get("runtime_smooth_ksize", 0) or 0) + if runtime_smooth_ksize >= 3 and runtime_smooth_ksize % 2 == 0: + runtime_smooth_ksize += 1 + + cache_key = ( + "gain_eff", + ch, + int(h), + int(w), + int(runtime_smooth_ksize), + round(float(strength), 6), + round(float(gain_min_runtime), 6), + round(float(gain_max_runtime), 6), + ) + + cached = self._flatfield_runtime_cache.get(cache_key) + if cached is not None: + return cached + + gain = self._get_runtime_gain_map(ch, base_shape, cfg) + if gain is None: + return None + + gain_eff = 1.0 + np.float32(strength) * (gain.astype(np.float32, copy=False) - 1.0) + gain_eff = np.clip(gain_eff, gain_min_runtime, gain_max_runtime).astype(np.float32, copy=False) + + self._flatfield_runtime_cache[cache_key] = gain_eff + return gain_eff + + + + def _apply_final_flat_gain_tensor_inplace( + self, + tensor: np.ndarray, + gain_tensor: np.ndarray, + clip_output: bool = True, + ) -> np.ndarray: + if tensor is None or gain_tensor is None: + return tensor + + if tensor.ndim != 3: + raise RuntimeError(f"Tensor CHW esperado. Veio shape={tensor.shape}") + + if gain_tensor.shape != tensor.shape: + raise RuntimeError( + f"Gain tensor shape inválido: gain={gain_tensor.shape} tensor={tensor.shape}" + ) + + if not tensor.flags.c_contiguous: + tensor = np.ascontiguousarray(tensor) + + gain_tensor = gain_tensor.astype(np.float32, copy=False) + if not gain_tensor.flags.c_contiguous: + gain_tensor = np.ascontiguousarray(gain_tensor) + + c, h, w = tensor.shape + + if _HAS_NUMBA: + _apply_tensor_flat_gain_chw_numba( + tensor, + gain_tensor, + int(c), + int(h), + int(w), + bool(clip_output), + ) + return tensor + + np.multiply(tensor, gain_tensor, out=tensor) + if clip_output: + np.clip(tensor, 0.0, 1.0, out=tensor) + + return tensor + + def _get_final_flat_gain_tensor_cached_fast( + self, + decoded: dict, + role_to_cam: dict, + geom: dict, + remap_cache: dict, + crop_box: tuple, + target_size: tuple, + ): + cfg = self.flatfield_config or {} + + if not cfg.get("enabled", False): + return None + + if str(cfg.get("apply_space", "native_camera_space")).lower() != "final_tensor_space": + return None + + if not self.flatfield_loaded: + self.load_flatfield_maps() + + if not self.flatfield_loaded: + return None + + target_w, target_h = int(target_size[0]), int(target_size[1]) + + strength = float(cfg.get("strength", 1.0)) + strength_by_channel = cfg.get("strength_by_channel", {}) or {} + + gain_min_runtime = float(cfg.get("gain_min_runtime", 0.0)) + gain_max_runtime = float(cfg.get("gain_max_runtime", 999.0)) + + runtime_smooth_ksize = int(cfg.get("runtime_smooth_ksize", 0) or 0) + if runtime_smooth_ksize >= 3 and runtime_smooth_ksize % 2 == 0: + runtime_smooth_ksize += 1 + + key = ( + "final_flat_gain_tensor", + geom.get("key"), + tuple(int(v) for v in crop_box), + int(target_w), + int(target_h), + int(runtime_smooth_ksize), + round(float(strength), 6), + tuple(sorted((str(k), round(float(v), 6)) for k, v in strength_by_channel.items())), + round(float(gain_min_runtime), 6), + round(float(gain_max_runtime), 6), + ) + + cached = self._flatfield_runtime_cache.get(key) + if cached is not None: + return cached + + gain_tensor = np.ones((5, target_h, target_w), dtype=np.float32) + + # ------------------------------------------------------------ + # RGB: usa crop + resize, igual ao RGB real. + # ------------------------------------------------------------ + rgb_cam = role_to_cam.get("rgb") + if rgb_cam is not None: + rgb_img = decoded[rgb_cam]["image"] + rgb_shape = rgb_img.shape[:2] + + x0, y0, x1, y1 = [int(v) for v in crop_box] + + for ci, ch in enumerate(("R", "G", "B")): + gain_eff = self._get_runtime_gain_eff_map(ch, rgb_shape, cfg) + + if gain_eff is None: + continue + + gain_crop = gain_eff[y0:y1, x0:x1].astype(np.float32, copy=False) + + if gain_crop.shape[1] != target_w or gain_crop.shape[0] != target_h: + gain_out = cv2.resize( + gain_crop, + (target_w, target_h), + interpolation=cv2.INTER_LINEAR, + ) + else: + gain_out = gain_crop + + gain_tensor[ci] = gain_out.astype(np.float32, copy=False) + + # ------------------------------------------------------------ + # RE/NIR: usa o mesmo remap cacheado da imagem real. + # ------------------------------------------------------------ + maps = (remap_cache or {}).get("maps", {}) or {} + + spec_map = { + "re": ("RE", 3), + "nir": ("NIR", 4), + } + + for role, (ch, ci) in spec_map.items(): + cam_id = role_to_cam.get(role) + if cam_id is None: + continue + + img = decoded[cam_id]["image"] + gain_eff = self._get_runtime_gain_eff_map(ch, img.shape[:2], cfg) + + if gain_eff is None: + continue + + remap_entry = maps.get(role) + + if remap_entry is not None: + gain_out = cv2.remap( + gain_eff.astype(np.float32, copy=False), + remap_entry["map1"], + remap_entry["map2"], + interpolation=cv2.INTER_LINEAR, + borderMode=cv2.BORDER_CONSTANT, + borderValue=1.0, + ) + else: + M_role_to_target = geom["M_role_to_target"].get(role) + + if M_role_to_target is None: + continue + + gain_out = cv2.warpPerspective( + gain_eff.astype(np.float32, copy=False), + M_role_to_target, + (target_w, target_h), + flags=cv2.INTER_LINEAR, + borderMode=cv2.BORDER_CONSTANT, + borderValue=1.0, + ) + + gain_tensor[ci] = gain_out.astype(np.float32, copy=False) + + self._flatfield_runtime_cache[key] = gain_tensor + return gain_tensor + diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/oak_fcc3_core/raw_processor_preview.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/oak_fcc3_core/raw_processor_preview.py new file mode 100644 index 000000000..a8d70227b --- /dev/null +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/oak_fcc3_core/raw_processor_preview.py @@ -0,0 +1,126 @@ +import cv2 +import numpy as np +from typing import Optional + + +class RawProcessorPreview: + 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.upper() + + def raw16_to_vis8( + self, raw16: np.ndarray, + black_level: Optional[int] = None, + white_level: Optional[int] = None, + gamma: float = 2.2, + bit_depth: int = 10 + ) -> np.ndarray: + """ + Conversão para visualização: + - auto-level + - gamma + """ + max_val = float((1 << bit_depth) - 1) + + raw = raw16.astype(np.float32) + + if black_level is None: + black_level = float(raw.min()) + if white_level is None: + white_level = float(raw.max()) + + if white_level <= black_level: + norm = raw / max_val + else: + norm = (raw - black_level) / (white_level - black_level) + + norm = np.clip(norm, 0.0, 1.0) + + if gamma is not None and gamma > 0: + norm = np.power(norm, 1.0 / gamma) + + return (norm * 255.0).clip(0, 255).astype(np.uint8) + + def _debayer_code(self): + mapping = { + "RGGB": cv2.COLOR_BayerRG2RGB_EA, + "BGGR": cv2.COLOR_BayerBG2RGB_EA, + "GRBG": cv2.COLOR_BayerGR2RGB_EA, + "GBRG": cv2.COLOR_BayerGB2RGB_EA, + } + + if self.bayer_pattern not in mapping: + raise ValueError(f"Padrão Bayer não suportado: {self.bayer_pattern}") + + return mapping[self.bayer_pattern] + + def apply_preview_white_balance(self, bgr: np.ndarray, strength: float = 1.0) -> np.ndarray: + """ + Gray-world simples para deixar o preview mais agradável. + Não usar no raw de treino. + """ + img = bgr.astype(np.float32) + + mean_b = float(img[:, :, 0].mean()) + mean_g = float(img[:, :, 1].mean()) + mean_r = float(img[:, :, 2].mean()) + + mean_gray = (mean_b + mean_g + mean_r) / 3.0 + + eps = 1e-6 + gain_b = mean_gray / max(mean_b, eps) + gain_g = mean_gray / max(mean_g, eps) + gain_r = mean_gray / max(mean_r, eps) + + # strength=1 aplica total, strength=0 não aplica + gain_b = 1.0 + (gain_b - 1.0) * strength + gain_g = 1.0 + (gain_g - 1.0) * strength + gain_r = 1.0 + (gain_r - 1.0) * strength + + img[:, :, 0] *= gain_b + img[:, :, 1] *= gain_g + img[:, :, 2] *= gain_r + + return np.clip(img, 0, 255).astype(np.uint8) + + def apply_preview_contrast(self, bgr: np.ndarray, alpha: float = 1.08, beta: float = 0.0) -> np.ndarray: + """ + Ajuste leve de contraste/brilho para preview. + """ + out = cv2.convertScaleAbs(bgr, alpha=alpha, beta=beta) + return out + + def raw16_to_preview_bgr( + self, + raw16: np.ndarray, + gamma: float = 2.2, + wb_strength: float = 0.8, + apply_wb: bool = True, + apply_contrast: bool = True, + bit_depth: int = 10, + ) -> np.ndarray: + """ + Pipeline de preview bonito: + 1. auto-level + gamma no mosaico + 2. demosaic + 3. white balance simples + 4. leve contraste final + """ + vis8 = self.raw16_to_vis8(raw16, gamma=gamma, bit_depth=bit_depth) + bgr = cv2.cvtColor(vis8, self._debayer_code()) + + if apply_wb: + bgr = self.apply_preview_white_balance(bgr, strength=wb_strength) + + if apply_contrast: + bgr = self.apply_preview_contrast(bgr, alpha=1.08, beta=0.0) + + return bgr + + def raw16_to_preview_jpg_bytes(self, raw16: np.ndarray, jpeg_quality: int = 95) -> bytes: + bgr = self.raw16_to_preview_bgr(raw16) + ok, enc = cv2.imencode(".jpg", bgr, [int(cv2.IMWRITE_JPEG_QUALITY), int(jpeg_quality)]) + if not ok: + raise RuntimeError("Falha ao codificar preview JPG") + return enc.tobytes() diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/oak_fcc3_core/segformer_service.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/oak_fcc3_core/segformer_service.py new file mode 100644 index 000000000..514d92c35 --- /dev/null +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/oak_fcc3_core/segformer_service.py @@ -0,0 +1,1022 @@ +# camera_worker/multispec_segformer_service.py +# -*- coding: utf-8 -*- + +from __future__ import annotations + +import copy +import json +import time +from pathlib import Path +from typing import Dict, Optional, Sequence, Tuple, List + +import cv2 +import numpy as np +import torch +import torch.nn as nn +import torch.nn.functional as F +from transformers import SegformerConfig, SegformerForSemanticSegmentation + + +DEFAULT_HEADS = { + "semantic": { + "enabled": True, + "type": "multiclass", + "num_classes": 3, + "classes": {"chao": 0, "cana": 1, "erva": 2}, + "ignore_index": 255, + }, + "vegetation": { + "enabled": True, + "type": "binary", + "num_classes": 2, + "classes": {"background": 0, "vegetation": 1}, + "ignore_index": 255, + }, + "cana": { + "enabled": True, + "type": "binary", + "num_classes": 2, + "classes": {"not_cana": 0, "cana": 1}, + "ignore_index": 255, + }, + "target": { + "enabled": True, + "type": "binary", + "num_classes": 2, + "classes": {"background": 0, "target": 1}, + "ignore_index": 255, + }, +} + +SEMANTIC_COLORS_RGB = { + 0: (85, 85, 85), # chao + 1: (0, 190, 0), # cana + 2: (230, 55, 55), # erva +} + +BINARY_COLORS_RGB = { + 0: (30, 30, 30), + 1: (0, 220, 80), +} + +CANA_COLORS_RGB = { + 0: (30, 30, 30), + 1: (40, 210, 255), +} + +TARGET_COLORS_RGB = { + 0: (30, 30, 30), + 1: (255, 70, 30), +} + +DEFAULT_CHANNEL_ORDER = ["R", "G", "B", "RE", "NIR"] + + +def get_input_channel_names(config: dict) -> List[str]: + if "input_channels" in config: + names = [str(c).upper() for c in config["input_channels"]] + else: + n = int(config.get("channels", 5)) + names = DEFAULT_CHANNEL_ORDER[:n] + + invalid = [c for c in names if c not in DEFAULT_CHANNEL_ORDER] + if invalid: + raise RuntimeError(f"Canais inválidos em input_channels: {invalid}") + + return names + + +def get_input_channel_indices(config: dict) -> List[int]: + names = get_input_channel_names(config) + return [DEFAULT_CHANNEL_ORDER.index(c) for c in names] + + +def load_json(path: str | Path) -> dict: + with open(path, "r", encoding="utf-8") as f: + return json.load(f) + + +def merge_dict(dst: dict, src: dict) -> dict: + out = copy.deepcopy(dst) + + def rec(a, b): + for k, v in b.items(): + if isinstance(v, dict) and isinstance(a.get(k), dict): + rec(a[k], v) + else: + a[k] = v + + if isinstance(src, dict): + rec(out, src) + + return out + + +def build_heads_config(config: dict, ignore_index: int = 255) -> Dict[str, dict]: + cfg = merge_dict(DEFAULT_HEADS, config.get("heads", {}) or {}) + active = {} + + for name, hcfg in cfg.items(): + if not bool(hcfg.get("enabled", True)): + continue + + hcfg.setdefault("ignore_index", ignore_index) + hcfg["ignore_index"] = int(hcfg.get("ignore_index", ignore_index)) + hcfg["num_classes"] = int(hcfg.get("num_classes", 2)) + active[name] = hcfg + + for required in ("semantic", "vegetation", "cana"): + if required not in active: + raise RuntimeError(f"Head obrigatória ausente no config: {required}") + + return active + + +def patch_segformer_encoder_input_channels(segformer_encoder: nn.Module, in_ch: int): + if in_ch == 3: + return segformer_encoder + + proj = segformer_encoder.encoder.patch_embeddings[0].proj + + if proj.in_channels == in_ch: + return segformer_encoder + + old_weight = proj.weight.data.clone() + old_bias = proj.bias.data.clone() if proj.bias is not None else None + + new_proj = nn.Conv2d( + in_channels=in_ch, + out_channels=proj.out_channels, + kernel_size=proj.kernel_size, + stride=proj.stride, + padding=proj.padding, + dilation=proj.dilation, + groups=proj.groups, + bias=proj.bias is not None, + padding_mode=proj.padding_mode, + ) + + with torch.no_grad(): + if in_ch <= old_weight.shape[1]: + new_proj.weight.copy_(old_weight[:, :in_ch, :, :]) + else: + new_proj.weight[:, :old_weight.shape[1], :, :].copy_(old_weight) + extra = in_ch - old_weight.shape[1] + mean_w = old_weight.mean(dim=1, keepdim=True) + new_proj.weight[:, old_weight.shape[1]:, :, :].copy_(mean_w.repeat(1, extra, 1, 1)) + + if old_bias is not None: + new_proj.bias.copy_(old_bias) + + segformer_encoder.encoder.patch_embeddings[0].proj = new_proj + print(f"[MODEL] patch input channels: 3 -> {in_ch}") + return segformer_encoder + + +def replace_segformer_decode_classifier(decode_head: nn.Module, num_classes: int): + old = decode_head.classifier + if not isinstance(old, nn.Conv2d): + raise RuntimeError(f"decode_head.classifier não é Conv2d: {type(old)}") + + new = nn.Conv2d( + in_channels=old.in_channels, + out_channels=int(num_classes), + kernel_size=old.kernel_size, + stride=old.stride, + padding=old.padding, + dilation=old.dilation, + groups=old.groups, + bias=old.bias is not None, + padding_mode=old.padding_mode, + ) + decode_head.classifier = new + return decode_head + + +class MultiHeadSegFormer(nn.Module): + def __init__( + self, + backbone: str, + channels: int, + heads_config: Dict[str, dict], + semantic_id2label: Dict[int, str], + semantic_label2id: Dict[str, int], + ): + super().__init__() + + semantic_classes = int(heads_config["semantic"].get("num_classes", len(semantic_id2label))) + + base_config = SegformerConfig.from_pretrained( + backbone, + local_files_only=True + ) + + base_config.num_labels = semantic_classes + base_config.id2label = {int(k): str(v) for k, v in semantic_id2label.items()} + base_config.label2id = {str(k): int(v) for k, v in semantic_label2id.items()} + + base = SegformerForSemanticSegmentation(base_config) + + patch_segformer_encoder_input_channels(base.segformer, channels) + base.config.num_channels = int(channels) + + self.segformer = base.segformer + self.decode_heads = nn.ModuleDict() + self.heads_config = heads_config + + for head_name, hcfg in heads_config.items(): + h = copy.deepcopy(base.decode_head) + h = replace_segformer_decode_classifier(h, int(hcfg["num_classes"])) + self.decode_heads[head_name] = h + + self.config = base.config + + def forward(self, pixel_values: torch.Tensor, head_names=None) -> Dict[str, torch.Tensor]: + outputs = self.segformer( + pixel_values=pixel_values, + output_hidden_states=True, + return_dict=True, + ) + hidden_states = outputs.hidden_states + + if head_names is None: + selected = list(self.decode_heads.keys()) + else: + selected = [str(h) for h in head_names if str(h) in self.decode_heads] + + return {head_name: self.decode_heads[head_name](hidden_states) for head_name in selected} + + +class MultiSpecSegformerService: + """ + Service de inferência para tensor multiespectral: + input : CHW float32 [R,G,B,RE,NIR] 0..1 + output: dict de predictions multi-head + """ + + def __init__(self, model_config: dict, mostrar_log=print): + self.config = model_config or {} + self.mostrar_log = mostrar_log + + self.input_channel_names = get_input_channel_names(self.config) + self.input_channel_indices = get_input_channel_indices(self.config) + self.channels = len(self.input_channel_names) + + self.mostrar_log( + f"[MULTIHEAD][INPUT] selected_channels={self.input_channel_names} " + f"idx={self.input_channel_indices}" + ) + + self.ignore_id = int(self.config.get("ignore_index", 255)) + self.heads_config = build_heads_config(self.config, ignore_index=self.ignore_id) + + self.semantic_id2label = { + 0: "chao", + 1: "cana", + 2: "erva", + } + + self.semantic_label2id = { + "chao": 0, + "cana": 1, + "erva": 2, + } + + self.classes = dict(self.semantic_label2id) + + # Dict interno, bom para ids_to_rgb + self.colormap_rgb = { + 0: (85, 85, 85), + 1: (0, 190, 0), + 2: (230, 55, 55), + } + + # Lista externa, compatível com WeedDetector + self.colormap_list_rgb = [ + self.colormap_rgb[0], + self.colormap_rgb[1], + self.colormap_rgb[2], + ] + + self.device = torch.device("cuda" if torch.cuda.is_available() else "cpu") + self.use_amp = bool(self.config.get("amp", True)) and self.device.type == "cuda" + self.sync_for_timing = bool(self.config.get("sync_for_timing", False)) and self.device.type == "cuda" + + self.runtime_mode = str(self.config.get("runtime_mode", "semantic")).lower() + self.lowres_argmax = bool(self.config.get("lowres_argmax", True)) + self.trust_input = bool(self.config.get("trust_input", True)) + self.channels_last = bool(self.config.get("channels_last", True)) and self.device.type == "cuda" + self.model_half = bool(self.config.get("model_half", False)) and self.device.type == "cuda" + + if self.device.type == "cuda": + torch.backends.cudnn.benchmark = True + try: + torch.set_float32_matmul_precision("high") + except Exception: + pass + + self.mean, self.std = self._load_norm_stats_from_config() + + backbone = self.config.get("backbone", self.config.get("pretrained_model", "nvidia/mit-b1")) + ckpt_path = self._resolve_checkpoint_path() + + self.mostrar_log(f"[MULTIHEAD] device={self.device}") + self.mostrar_log(f"[MULTIHEAD] backbone={backbone}") + self.mostrar_log(f"[MULTIHEAD] ckpt={ckpt_path}") + + self.model = MultiHeadSegFormer( + backbone=backbone, + channels=self.channels, + heads_config=self.heads_config, + semantic_id2label=self.semantic_id2label, + semantic_label2id=self.semantic_label2id, + ) + + self._load_checkpoint(ckpt_path) + self.model.to(self.device) + self.model.eval() + + if self.model_half: + self.model.half() + if self.mean is not None: + self.mean = self.mean.half() + if self.std is not None: + self.std = self.std.half() + + if self.channels_last: + try: + self.model.to(memory_format=torch.channels_last) + except Exception: + self.channels_last = False + + if bool(self.config.get("fold_input_norm", False)): + self._fold_input_normalization_into_first_conv() + + if bool(self.config.get("torch_compile", False)): + try: + self.model = torch.compile( + self.model, + mode=str(self.config.get("torch_compile_mode", "reduce-overhead")), + fullgraph=False, + ) + self.mostrar_log("[MULTIHEAD][OPT] torch.compile habilitado") + except Exception as e: + self.mostrar_log(f"[MULTIHEAD][OPT] torch.compile falhou: {type(e).__name__}: {e}") + + self.mostrar_log( + f"[MULTIHEAD][FAST] runtime_mode={self.runtime_mode} " + f"lowres_argmax={self.lowres_argmax} trust_input={self.trust_input} " + f"channels_last={self.channels_last} model_half={self.model_half} amp={self.use_amp}" + ) + + self._ultimo_tensor = None + self._ultimo_predictions = None + self._ultimo_probs = None + + # ============================================================ + # Config/load + # ============================================================ + + def _resolve_checkpoint_path(self) -> Path: + candidates = [] + + for key in ("ckpt", "checkpoint", "ia_model_path", "model_path"): + value = self.config.get(key) + if value: + candidates.append(Path(value)) + + backup_root = self.config.get("backup_root") + modelo = self.config.get("modelo", "segformer_b1") + model_name = self.config.get("model_name", "target_teached") + fusion_mode = self.config.get("fusion_mode", "stacked") + + exp_tags = [ + f"{fusion_mode}_raw{self.channels}", + f"{fusion_mode}_raw{self.channels}_multihead", + ] + + ckpt_names = [ + "best_score.pt", + "best_target.pt", + "best_cana_head.pt", + "best_semantic_miou.pt", + "last.pt", + ] + + if backup_root: + for exp_tag in exp_tags: + for ckpt_name in ckpt_names: + candidates.append(Path(backup_root) / modelo / model_name / exp_tag / ckpt_name) + + for exp_tag in exp_tags: + for ckpt_name in ckpt_names: + candidates.append(Path("backup") / modelo / model_name / exp_tag / ckpt_name) + + for p in candidates: + p = p.resolve() if not p.is_absolute() else p + if p.is_file(): + return p + + raise FileNotFoundError( + "Checkpoint multi-head não encontrado. Procurei:\n" + + "\n".join(str(p) for p in candidates) + ) + + def _load_norm_stats_from_config(self): + path = None + + for key in ("norm_stats", "norm_stats_path", "ia_norm_stats_path"): + if self.config.get(key): + path = Path(self.config[key]) + break + + if path is None: + dataset_root = self.config.get("dataset_root") + ia_resolution = self.config.get("ia_resolution", [1024, 640]) + if dataset_root: + w, h = int(ia_resolution[0]), int(ia_resolution[1]) + path = Path(dataset_root) / f"{w}x{h}" / "group" / "norm_stats.json" + + if path is None or not path.is_file(): + self.mostrar_log("[MULTIHEAD][NORM] norm_stats não encontrado. Usando tensor 0..1 sem padronização.") + return None, None + + js = load_json(path) + mean = js.get("mean") + std = js.get("std") + names = js.get("channels", []) + + if mean is None or std is None: + raise RuntimeError(f"norm_stats inválido, faltando mean/std: {path}") + + max_idx = max(self.input_channel_indices) + + if len(mean) <= max_idx or len(std) <= max_idx: + raise RuntimeError( + f"norm_stats incompatível: precisa índices={self.input_channel_indices}, " + f"mean={len(mean)} std={len(std)} path={path}" + ) + + mean = [mean[i] for i in self.input_channel_indices] + std = [std[i] for i in self.input_channel_indices] + + if names: + names = [names[i] for i in self.input_channel_indices] + else: + names = self.input_channel_names + + self.mostrar_log(f"[MULTIHEAD][NORM] usando {path}") + self.mostrar_log(f"[MULTIHEAD][NORM] selected_channels={names}") + self.mostrar_log(f"[MULTIHEAD][NORM] mean={mean}") + self.mostrar_log(f"[MULTIHEAD][NORM] std ={std}") + + mean_t = torch.tensor(mean, dtype=torch.float32).view(1, self.channels, 1, 1).to(self.device) + std_t = torch.tensor(std, dtype=torch.float32).view(1, self.channels, 1, 1).to(self.device) + + return mean_t, std_t + + def _load_checkpoint(self, ckpt_path: Path): + ckpt = torch.load(str(ckpt_path), map_location="cpu", weights_only=False) + + if isinstance(ckpt, dict): + for key in ("model", "model_state", "model_state_dict", "state_dict"): + if key in ckpt and isinstance(ckpt[key], dict): + state = ckpt[key] + break + else: + state = ckpt + else: + raise RuntimeError(f"Checkpoint em formato inesperado: {type(ckpt)}") + + clean = {} + for k, v in state.items(): + nk = k + for prefix in ("module.", "model."): + if nk.startswith(prefix): + nk = nk[len(prefix):] + clean[nk] = v + + missing, unexpected = self.model.load_state_dict(clean, strict=False) + + self.mostrar_log( + f"[MULTIHEAD] load_state_dict strict=False | " + f"missing={len(missing)} unexpected={len(unexpected)}" + ) + + if missing: + self.mostrar_log(f"[MULTIHEAD] primeiros missing: {missing[:8]}") + if unexpected: + self.mostrar_log(f"[MULTIHEAD] primeiros unexpected: {unexpected[:8]}") + + def _select_input_channels(self, chw: np.ndarray) -> np.ndarray: + if chw.ndim != 3: + raise RuntimeError(f"Tensor inválido: esperado CHW 3D, veio shape={chw.shape}") + + # Se já veio no formato exato do modelo, mantém. + # Ex: modelo raw3 recebendo tensor [R,G,B] já fatiado. + if chw.shape[0] == self.channels: + return chw + + max_idx = max(self.input_channel_indices) + + if chw.shape[0] <= max_idx: + raise RuntimeError( + f"Tensor tem C={chw.shape[0]}, mas precisa acessar índice {max_idx} " + f"para canais {self.input_channel_names}" + ) + + return chw[self.input_channel_indices, :, :] + + # ============================================================ + # Inferência + # ============================================================ + + def _normalize(self, x: torch.Tensor) -> torch.Tensor: + if self.mean is not None and self.std is not None: + return (x - self.mean) / torch.clamp(self.std, min=1e-6) + return x + + @torch.inference_mode() + def infer_tensor(self, tensor5_chw: np.ndarray): + """ + Retorna predictions compatível com o WeedDetector. + + Por enquanto: + predictions = semantic mask uint8 HxW + + Também mantém: + self._ultimo_predictions_full = dict com semantic/vegetation/cana/target/probs. + """ + if tensor5_chw is None: + return None + + chw = np.asarray(tensor5_chw, dtype=np.float32) + chw = self._select_input_channels(chw) + + if chw.ndim != 3: + raise RuntimeError(f"Tensor inválido: esperado CHW 3D, veio shape={chw.shape}") + + if chw.shape[0] != self.channels: + raise RuntimeError(f"Tensor inválido: esperado C={self.channels}, veio shape={chw.shape}") + + chw = np.nan_to_num(chw, nan=0.0, posinf=1.0, neginf=0.0) + chw = np.clip(chw, 0.0, 1.0).astype(np.float32, copy=False) + chw = np.ascontiguousarray(chw) + + h, w = int(chw.shape[1]), int(chw.shape[2]) + + x = torch.from_numpy(chw).unsqueeze(0).to(self.device, non_blocking=True) + x = self._normalize(x) + + if self.device.type == "cuda": + torch.cuda.synchronize() + + t0 = time.perf_counter() + + with torch.autocast(device_type="cuda", dtype=torch.float16, enabled=self.use_amp): + logits_by_head = self.model(pixel_values=x) + + preds = {} + probs = {} + + for head_name, logits in logits_by_head.items(): + logits = F.interpolate(logits, size=(h, w), mode="bilinear", align_corners=False) + prob = torch.softmax(logits, dim=1)[0] + pred = torch.argmax(prob, dim=0) + + preds[head_name] = pred.detach().cpu().numpy().astype(np.uint8) + probs[head_name] = prob.detach().cpu().numpy().astype(np.float32) + + if self.device.type == "cuda": + torch.cuda.synchronize() + + t_ms = (time.perf_counter() - t0) * 1000.0 + + semantic = preds["semantic"] + vegetation = preds["vegetation"] + cana = preds["cana"] + + self._ultimo_tensor = chw + target_op = self.operational_target_mask(vegetation, cana, ignore_id=self.ignore_id) + target_head = preds.get("target") + + mode = str(getattr(self, "runtime_mode", "semantic")).lower() + + if mode in ("target_direct", "direct_target", "target_head") and target_head is not None: + output = target_head + elif mode in ("target", "spray", "operational"): + output = target_op + else: + output = semantic + + self._ultimo_predictions_full = { + "semantic": semantic, + "vegetation": vegetation, + "cana": cana, + "target": output if mode in ("target_direct", "direct_target", "target_head", "target", "spray", "operational") else target_op, + "target_head": target_head, + "target_op": target_op, + "probs": probs, + "infer_ms": t_ms, + "runtime_mode": mode, + } + + return output + + def infer_tensor_full(self, tensor5_chw: np.ndarray): + semantic = self.infer_tensor(tensor5_chw) + if semantic is None: + return None + return dict(self._ultimo_predictions_full) + + @torch.inference_mode() + def infer_tensor_fast(self, tensor5_chw: np.ndarray, keep_probs: bool = False): + if keep_probs: + return self.infer_tensor(tensor5_chw) + + return self.infer_tensor_ultrafast( + tensor5_chw, + return_full=bool(self.config.get("return_full_fast", False)), + ) + + # ============================================================ + # Preview/debug + # ============================================================ + + def preview_infer_cached(self, tensor5_chw=None, predictions=None, alpha=0.5): + """ + Compatível com o uso atual do weed_worker: + rgb_frame, seg_frame, overlay_frame, _, _ = preview_infer_cached(...) + + Retorna BGR para OpenCV/TCP. + """ + tensor = tensor5_chw if tensor5_chw is not None else self._ultimo_tensor + pred = predictions if predictions is not None else self._ultimo_predictions + + if tensor is None: + return None, None, None, None, None + + rgb = self.tensor_to_preview_rgb(tensor) + rgb_bgr = cv2.cvtColor(rgb, cv2.COLOR_RGB2BGR) + + if pred is None: + return rgb_bgr, None, rgb_bgr, None, None + + seg_rgb = self.ids_to_rgb(pred, self._colormap_for_current_output(), ignore_id=self.ignore_id) + overlay_rgb = cv2.addWeighted(rgb, 1.0 - alpha, seg_rgb, alpha, 0.0) + + seg_bgr = cv2.cvtColor(seg_rgb, cv2.COLOR_RGB2BGR) + overlay_bgr = cv2.cvtColor(overlay_rgb, cv2.COLOR_RGB2BGR) + + return rgb_bgr, seg_bgr, overlay_bgr, None, None + + def get_classes(self): + mode = str(getattr(self, "runtime_mode", "semantic")).lower() + + if mode in ("target_direct", "direct_target", "target_head", "target", "spray", "operational"): + return {"background": 0, "target": 1} + + return self.classes + + def get_colormap(self): + mode = str(getattr(self, "runtime_mode", "semantic")).lower() + + if mode in ("target_direct", "direct_target", "target_head", "target", "spray", "operational"): + return [ + TARGET_COLORS_RGB[0], + TARGET_COLORS_RGB[1], + ] + + return self.colormap_list_rgb + + def _colormap_for_current_output(self): + mode = str(getattr(self, "runtime_mode", "semantic")).lower() + + if mode in ("target_direct", "direct_target", "target_head", "target", "spray", "operational"): + return TARGET_COLORS_RGB + + return self.colormap_rgb + + @staticmethod + def operational_target_mask(veg_mask: np.ndarray, cana_mask: np.ndarray, ignore_id: int = 255) -> np.ndarray: + out = np.zeros_like(veg_mask, dtype=np.uint8) + ignore = (veg_mask == ignore_id) | (cana_mask == ignore_id) + out[(veg_mask == 1) & (cana_mask == 0)] = 1 + out[ignore] = ignore_id + return out + + @staticmethod + def tensor_to_preview_rgb(chw: np.ndarray, gamma: float = 0.85) -> np.ndarray: + c, h, w = chw.shape + + if c >= 3: + rgb = np.transpose(chw[:3], (1, 2, 0)).copy() + else: + one = chw[0] + rgb = np.stack([one, one, one], axis=-1) + + rgb = np.nan_to_num(rgb, nan=0.0, posinf=1.0, neginf=0.0) + + lo = np.percentile(rgb, 1.0) + hi = np.percentile(rgb, 99.0) + + if hi > lo: + rgb = (rgb - lo) / (hi - lo) + + rgb = np.clip(rgb, 0.0, 1.0) + + if gamma and gamma > 0: + rgb = np.power(rgb, gamma) + + return (rgb * 255.0).astype(np.uint8) + + @staticmethod + def ids_to_rgb(mask: np.ndarray, colormap_rgb: Dict[int, Tuple[int, int, int]], ignore_id: int = 255) -> np.ndarray: + h, w = mask.shape[:2] + out = np.zeros((h, w, 3), dtype=np.uint8) + + for cid, color in colormap_rgb.items(): + out[mask == cid] = color + + out[mask == ignore_id] = (0, 0, 0) + return out + + + + def _runtime_head_names(self): + mode = str(getattr(self, "runtime_mode", "semantic")).lower() + + if mode in ("semantic", "sem", "mask"): + return ["semantic"] + + if mode in ("target", "spray", "operational"): + return ["vegetation", "cana"] + + if mode in ("target_direct", "direct_target", "target_head"): + return ["target"] + + if mode in ("all", "full", "debug"): + return ["semantic", "vegetation", "cana", "target"] + + return ["semantic"] + + def _prepare_input_tensor_fast(self, tensor5_chw: np.ndarray): + if tensor5_chw is None: + return None, None, None + + if bool(getattr(self, "trust_input", True)): + chw = tensor5_chw + if not isinstance(chw, np.ndarray): + chw = np.asarray(chw, dtype=np.float32) + if chw.dtype != np.float32 or not chw.flags.c_contiguous: + chw = np.ascontiguousarray(chw, dtype=np.float32) + else: + chw = np.asarray(tensor5_chw, dtype=np.float32) + chw = np.nan_to_num(chw, nan=0.0, posinf=1.0, neginf=0.0) + chw = np.clip(chw, 0.0, 1.0).astype(np.float32, copy=False) + chw = np.ascontiguousarray(chw) + + # Sempre selecionar canais, independentemente de trust_input. + chw = self._select_input_channels(chw) + + if chw.ndim != 3: + raise RuntimeError(f"Tensor inválido: esperado CHW 3D, veio shape={chw.shape}") + + if chw.shape[0] != self.channels: + raise RuntimeError(f"Tensor inválido: esperado C={self.channels}, veio shape={chw.shape}") + + h, w = int(chw.shape[1]), int(chw.shape[2]) + + x = torch.from_numpy(chw).unsqueeze(0).to(self.device, non_blocking=True) + + if bool(getattr(self, "channels_last", False)): + try: + x = x.contiguous(memory_format=torch.channels_last) + except Exception: + pass + + x = self._normalize(x) + + if bool(getattr(self, "model_half", False)): + x = x.half() + + return chw, x, (h, w) + + def _logits_to_pred_numpy_fast(self, logits: torch.Tensor, out_hw, lowres_argmax: bool): + h, w = int(out_hw[0]), int(out_hw[1]) + + if lowres_argmax: + # Argmax no mapa pequeno, depois resize da máscara uint8. + # Bem mais barato que interpolar logits CxHxW em float. + pred_small = torch.argmax(logits, dim=1)[0] + pred_np = pred_small.detach().to("cpu", non_blocking=False).numpy().astype(np.uint8) + + if pred_np.shape[0] != h or pred_np.shape[1] != w: + pred_np = cv2.resize(pred_np, (w, h), interpolation=cv2.INTER_NEAREST) + + return pred_np + + # Caminho equivalente ao atual, mas sem softmax. + logits = F.interpolate(logits, size=(h, w), mode="bilinear", align_corners=False) + pred = torch.argmax(logits, dim=1)[0] + return pred.detach().to("cpu", non_blocking=False).numpy().astype(np.uint8) + + @torch.inference_mode() + def infer_tensor_ultrafast(self, tensor5_chw: np.ndarray, return_full: bool = False): + if tensor5_chw is None: + return None + + t_total0 = time.perf_counter() + + # ------------------------------------------------------------ + # Prepare: numpy -> torch/cuda + layout + normalização + # ------------------------------------------------------------ + t_prepare0 = time.perf_counter() + + chw, x, out_hw = self._prepare_input_tensor_fast(tensor5_chw) + + prepare_ms = (time.perf_counter() - t_prepare0) * 1000.0 + + if x is None: + return None + + head_names = self._runtime_head_names() + lowres_argmax = bool(getattr(self, "lowres_argmax", True)) + runtime_mode = str(getattr(self, "runtime_mode", "semantic")).lower() + output_mask_fullres = bool(self.config.get("output_mask_fullres", True)) + + if bool(getattr(self, "sync_for_timing", False)) and self.device.type == "cuda": + torch.cuda.synchronize() + + # ------------------------------------------------------------ + # Forward: modelo UMA vez só + # ------------------------------------------------------------ + t_forward0 = time.perf_counter() + + with torch.autocast( + device_type="cuda", + dtype=torch.float16, + enabled=bool(getattr(self, "use_amp", True)) and self.device.type == "cuda", + ): + logits_by_head = self.model(pixel_values=x, head_names=head_names) + + if self.device.type == "cuda": + torch.cuda.synchronize() + + forward_ms = (time.perf_counter() - t_forward0) * 1000.0 + + # ------------------------------------------------------------ + # Post: argmax / target / cópia CPU + # ------------------------------------------------------------ + t_post0 = time.perf_counter() + + semantic = None + vegetation = None + cana = None + target = None + + if runtime_mode in ("target", "spray", "operational"): + logits_veg = logits_by_head.get("vegetation") + logits_cana = logits_by_head.get("cana") + + if logits_veg is None or logits_cana is None: + raise RuntimeError("runtime_mode=target requer heads vegetation e cana") + + target = self._target_from_logits_gpu_fast( + logits_veg, + logits_cana, + out_hw=out_hw, + fullres=output_mask_fullres, + ) + + output = target + + else: + preds = {} + + if "target" in logits_by_head: + target = self._logits_to_pred_numpy_fast( + logits_by_head["target"], + out_hw=out_hw, + lowres_argmax=lowres_argmax, + ) + + semantic = None + vegetation = None + cana = None + output = target + else: + for head_name, logits in logits_by_head.items(): + preds[head_name] = self._logits_to_pred_numpy_fast( + logits, + out_hw=out_hw, + lowres_argmax=lowres_argmax, + ) + + semantic = preds.get("semantic") + vegetation = preds.get("vegetation") + cana = preds.get("cana") + + if vegetation is not None and cana is not None: + target = self.operational_target_mask( + vegetation, + cana, + ignore_id=self.ignore_id, + ) + + output = semantic if semantic is not None else target + + post_ms = (time.perf_counter() - t_post0) * 1000.0 + total_ms = (time.perf_counter() - t_total0) * 1000.0 + + self._ultimo_tensor = chw + self._ultimo_predictions = output + self._ultimo_probs = None + self._ultimo_predictions_full = { + "semantic": semantic, + "vegetation": vegetation, + "cana": cana, + "target": target, + "probs": None, + "infer_ms": total_ms, + "prepare_ms": prepare_ms, + "forward_ms": forward_ms, + "post_ms": post_ms, + "runtime_mode": runtime_mode, + "lowres_argmax": lowres_argmax, + "heads": list(head_names), + "output_mask_fullres": output_mask_fullres, + } + + if return_full: + return dict(self._ultimo_predictions_full) + + return output + + + def _target_from_logits_gpu_fast(self, logits_veg, logits_cana, out_hw, fullres: bool = True): + h, w = int(out_hw[0]), int(out_hw[1]) + + veg = torch.argmax(logits_veg, dim=1)[0] + cana = torch.argmax(logits_cana, dim=1)[0] + + target = ((veg == 1) & (cana == 0)).to(torch.uint8) + + target_np = target.detach().to("cpu", non_blocking=False).numpy() + + if fullres and (target_np.shape[0] != h or target_np.shape[1] != w): + target_np = cv2.resize( + target_np, + (w, h), + interpolation=cv2.INTER_NEAREST, + ) + + return target_np.astype(np.uint8, copy=False) + + def _fold_input_normalization_into_first_conv(self): + if self.mean is None or self.std is None: + return False + + try: + proj = self.model.segformer.encoder.patch_embeddings[0].proj + except Exception: + return False + + if not isinstance(proj, nn.Conv2d): + return False + + with torch.no_grad(): + device = proj.weight.device + dtype = proj.weight.dtype + + mean = self.mean.detach().to(device=device, dtype=dtype).view(-1) + std = self.std.detach().to(device=device, dtype=dtype).view(-1) + std = torch.clamp(std, min=1e-6) + + w_old = proj.weight.data.clone() + b_old = proj.bias.data.clone() if proj.bias is not None else torch.zeros( + proj.out_channels, + device=device, + dtype=dtype, + ) + + # W'[:, c] = W[:, c] / std[c] + w_new = w_old / std.view(1, -1, 1, 1) + + # bias' = bias - sum(W[:,c,:,:] * mean[c] / std[c]) + offset = (w_old * (mean / std).view(1, -1, 1, 1)).sum(dim=(1, 2, 3)) + b_new = b_old - offset + + proj.weight.data.copy_(w_new) + + if proj.bias is None: + proj.bias = nn.Parameter(b_new) + else: + proj.bias.data.copy_(b_new) + + self.mean = None + self.std = None + + self.mostrar_log("[MULTIHEAD][OPT] normalização foldada na primeira conv") + return True + diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/oak_fcc3_core/test.json b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/oak_fcc3_core/test.json new file mode 100644 index 000000000..d7b210538 --- /dev/null +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/oak_fcc3_core/test.json @@ -0,0 +1,104 @@ +{ + "debug_visual": false, + "frames_consecutivos": 3, + "frames_histerese": 2, + "min_area_px": 400, + "max_area_frac": 0.2, + "ia_roi_begin": 0.0, + "ia_roi_size": 1.0, + "ia_resolution": [1024,640], + "ia_channels": 5, + "ia_use_ndvi": false, + "erva_top_band_frac": 0.30, + "erva_frac_ema": 0.3, + "erva_thresh_vel_gain": 0.4, + "min_frac_erva_global_on": 0.0020, + "min_frac_erva_global_off": 0.0015, + "min_frac_erva_top_on": 0.0015, + "min_frac_erva_top_off": 0.0010, + "min_frac_erva_por_bico": 0.02, + "usar_morfologia": true, + "kernel_morf": 3, + + "usar_radar_global_gate": true, + "max_frac_cana_por_bico": 0.009, + "ema_frac_bico": 0.35, + "on_frames_required": 3, + "off_frames_required": 2, + "cana_halo_px": 5, + "min_area_erva_px": 80, + "erva_thresh_vel_gain_local": 0.6, + "k_roi_shift_px_per_vnorm": 24.0, + + "analise_fps": 20, + "tensor_fps": 20, + + "fps": 20, + "module_params": "C:\\ZendionInc\\agrobot_base\\Python\\OAK\\datasets\\oak-fcc-3\\calibration\\module_params.json", + + "qtd_bicos": 7, + "velocidade_robo": 0, + "ia_model_path": "C:\\AgroBaseModels\\Ervas\\model-3_1.pt", + "ia_labelmap_path": "C:\\AgroBaseModels\\Ervas\\model-3_1.txt", + "ia_norm_stats_path": "C:\\AgroBaseModels\\Ervas\\model-1_1.json", + "ia_backbone": "nvidia/mit-b1", + + "faixa_atuacao_bicos": 0.7, + "area_atuacao_bicos": 0.1, + "min_frac_erva_por_bico_on": 0.02, + "min_frac_erva_por_bico_off": 0.01, + + "tipo_camera_solo": "multispectral", + "channels": 5, + "input_channels": ["R", "G", "B", "RE", "NIR"], + "backbone": "nvidia/mit-b1", + "ckpt": "C:\\ZendionInc\\agrobot_base\\Python\\OAK\\datasets\\oak-fcc-3\\backup\\segformer_b1\\target_teached\\stacked_raw5\\best_score.pt", + "norm_stats_path": "C:\\ZendionInc\\agrobot_base\\Python\\OAK\\datasets\\oak-fcc-3\\backup\\segformer_b1\\target_teached\\stacked_raw5\\norm_stats.json", + "module_calibration_json": "C:\\ZendionInc\\agrobot_base\\Python\\OAK\\datasets\\oak-fcc-3\\calibration\\module_params.json", + "camera_width": 1280, + "camera_height": 800, + "camera_fps": 40, + "amp": true, + "fold_input_norm": true, + "runtime_mode": "target_direct", + "prediction_contract": "target_binary", + "output_mask_fullres": false, + "lowres_argmax": true, + "trust_input": true, + "channels_last": false, + "model_half": true, + "sync_for_timing": false, + "torch_compile": false, + "torch_compile_mode": "reduce-overhead", + "heads": { + "semantic": { + "enabled": true, + "type": "multiclass", + "num_classes": 3, + "classes": {"chao": 0, "cana": 1, "erva": 2}, + "ignore_index": 255 + }, + "vegetation": { + "enabled": true, + "type": "binary", + "num_classes": 2, + "classes": {"background": 0, "vegetation": 1}, + "ignore_index": 255 + }, + "cana": { + "enabled": true, + "type": "binary", + "num_classes": 2, + "classes": {"not_cana": 0, "cana": 1}, + "ignore_index": 255 + }, + "target": { + "enabled": true, + "type": "binary", + "num_classes": 2, + "classes": {"background": 0, "target": 1}, + "ignore_index": 255 + } + } + + } \ No newline at end of file diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/segformer_runner.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/segformer_runner.py index 3145d01c6..e6a07982d 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/segformer_runner.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/segformer_runner.py @@ -1,11 +1,34 @@ import json import os import time -from PIL import Image import cv2 import numpy as np import torch -from transformers import SegformerForSemanticSegmentation +import torch.nn as nn +import torch.nn.functional as F +from transformers import SegformerConfig, SegformerForSemanticSegmentation + + +class LabelHead(nn.Module): + def __init__(self, feat_ch: int, num_seg_classes: int, num_label_classes: int, hidden: int = 256, dropout: float = 0.2): + super().__init__() + in_ch = feat_ch + num_seg_classes + self.pool = nn.AdaptiveAvgPool2d((1, 1)) + self.net = nn.Sequential( + nn.Linear(in_ch, hidden), + nn.ReLU(inplace=True), + nn.Dropout(dropout), + nn.Linear(hidden, num_label_classes), + ) + + def forward(self, feat: torch.Tensor, logits_seg: torch.Tensor) -> torch.Tensor: + if feat.shape[-2:] != logits_seg.shape[-2:]: + feat = F.interpolate(feat, size=logits_seg.shape[-2:], mode="bilinear", align_corners=False) + + x = torch.cat([feat, logits_seg], dim=1) + x = self.pool(x).flatten(1) + return self.net(x) + class SegformerNavRunner: IMAGENET_MEAN = [0.485, 0.456, 0.406] @@ -15,16 +38,30 @@ class SegformerNavRunner: from shared.utils import carregar_labelmap_completo self.device = torch.device(device if torch.cuda.is_available() else "cpu") - self.cor_para_id, self.colormap_rgb, self.classes, self.ignore_rgb = carregar_labelmap_completo(seg_config["ia_labelmap_path"]) - self.model = self.load_segformer_from_checkpoint(seg_config, self.device) - self.resolucao = tuple(seg_config["ia_resolution"]) + + self.use_amp = bool(seg_config.get("use_amp", True)) + self.use_channels_last = bool(seg_config.get("use_channels_last", True)) + self.use_compact_aux = bool(seg_config.get("use_compact_aux", True)) + self.debug_timing = bool(seg_config.get("debug_timing", False)) + self._last_aux_result = None + self._last_aux_ts = 0.0 + + self.cor_para_id, self.colormap_rgb, self.classes, self.ignore_rgb = carregar_labelmap_completo( + seg_config["ia_labelmap_path"] + ) + + self.resolucao = tuple(seg_config["ia_resolution"]) # [W,H] self.roi_inicio = seg_config["ia_roi_begin"] self.roi_tamanho = seg_config["ia_roi_size"] self.last_infer = None - # ========================== - # Normalização fixa (igual treino) - # ========================== + self.mode = seg_config.get("ia_mode", "dual_label") + self.model = None + self.aux_head = None + self.label_names = {} + + self._load_dual_checkpoint(seg_config) + norm_mean = self.IMAGENET_MEAN norm_std = self.IMAGENET_STD @@ -35,161 +72,344 @@ class SegformerNavRunner: stats_channels = norm_stats.get("channels", []) stats_mean = norm_stats.get("mean", []) - stats_std = norm_stats.get("std", []) - - print(f"[NORM] usando stats fixos de: {norm_stats_path}") - print(f"[NORM] channels={stats_channels}") - print(f"[NORM] mean={stats_mean}") - print(f"[NORM] std ={stats_std}") + stats_std = norm_stats.get("std", []) idx_by_name = {name: i for i, name in enumerate(stats_channels)} - m_R = stats_mean[idx_by_name["R"]] - m_G = stats_mean[idx_by_name["G"]] - m_B = stats_mean[idx_by_name["B"]] - - s_R = stats_std[idx_by_name["R"]] - s_G = stats_std[idx_by_name["G"]] - s_B = stats_std[idx_by_name["B"]] - - norm_mean = [m_R, m_G, m_B] - norm_std = [s_R, s_G, s_B] + if all(ch in idx_by_name for ch in ["R", "G", "B"]): + norm_mean = [ + stats_mean[idx_by_name["R"]], + stats_mean[idx_by_name["G"]], + stats_mean[idx_by_name["B"]], + ] + norm_std = [ + stats_std[idx_by_name["R"]], + stats_std[idx_by_name["G"]], + stats_std[idx_by_name["B"]], + ] + print(f"[NORM] usando stats fixos de: {norm_stats_path}") + else: + print("[NORM] norm_stats não contém R,G,B. Usando ImageNet.") else: - print(f"[NORM] norm_stats.json não encontrado em {norm_stats_path}. " - f"Usando normalização dinâmica por frame.") + print(f"[NORM] norm_stats não encontrado em {norm_stats_path}. Usando ImageNet.") self.set_norm_stats(norm_mean, norm_std) def set_norm_stats(self, mean, std): - mean = torch.tensor(mean, dtype=torch.float32).view(3, 1, 1) - std = torch.tensor(std, dtype=torch.float32).view(3, 1, 1).clamp_min(1e-6) - self._norm_mean = mean - self._norm_std = std + self._norm_mean = torch.tensor(mean, dtype=torch.float32, device=self.device).view(3, 1, 1) + self._norm_std = torch.tensor(std, dtype=torch.float32, device=self.device).view(3, 1, 1).clamp_min(1e-6) - def _extract_state_dict(self, ckpt): - """ - Aceita: - - state_dict puro (dict de tensores) - - checkpoint com chaves comuns: state_dict / model_state_dict / model - """ - if not isinstance(ckpt, dict): - return None + def normalize_img(self, img): + return (img - self._norm_mean) / self._norm_std - # caso já seja um state_dict puro - if any(isinstance(v, torch.Tensor) for v in ckpt.values()): - return ckpt - - for k in ("state_dict", "model_state_dict", "model"): - if k in ckpt and isinstance(ckpt[k], dict): - return ckpt[k] - - return None - - def load_segformer_from_checkpoint( - self, - seg_config: dict, - device: torch.device, - ): - """ - Carrega um SegFormer (B0, B1, B2, B3...) compatível com o treino: - - - Cria o modelo via from_pretrained(backbone, num_labels=num_classes) - - Carrega o state_dict salvo pelo script de treino - """ - pt_path = seg_config.get("ia_model_path") - backbone = seg_config.get("ia_backbone") - num_classes = len(self.classes) - ckpt = torch.load(pt_path, map_location="cpu", weights_only=True) - state_dict = self._extract_state_dict(ckpt) - if state_dict is None: - raise RuntimeError(f"Não consegui extrair state_dict de {pt_path}. keys={list(ckpt.keys())}") - - # limpar prefixos comuns - cleaned = {} - for k, v in state_dict.items(): - nk = k - if nk.startswith("model."): - nk = nk[len("model."):] - if nk.startswith("module."): - nk = nk[len("module."):] - cleaned[nk] = v - - model = SegformerForSemanticSegmentation.from_pretrained( + def _build_base_model(self, backbone: str, num_classes: int): + config = SegformerConfig.from_pretrained( backbone, - num_labels=num_classes, - ignore_mismatched_sizes=True, - use_safetensors=True + local_files_only=True ) - missing, unexpected = model.load_state_dict(cleaned, strict=False) - print(f"[load] missing={len(missing)} unexpected={len(unexpected)}") - if missing: - print("[load] missing sample:", missing[:10]) - if unexpected: - print("[load] unexpected sample:", unexpected[:10]) + config.num_labels = int(num_classes) + config.output_hidden_states = True - model.to(device).eval() + model = SegformerForSemanticSegmentation(config) return model - def normalize_img(self, img: torch.Tensor) -> torch.Tensor: - return (img - self._norm_mean.to(img.device)) / self._norm_std.to(img.device) + def _load_dual_checkpoint(self, seg_config): + pt_path = seg_config["ia_model_path"] + backbone = seg_config["ia_backbone"] + num_classes = len(self.classes) + + ckpt = torch.load(pt_path, map_location="cpu", weights_only=False) + self.model = self._build_base_model(backbone, num_classes) + self.model.load_state_dict(ckpt["model"], strict=True) + self.model.to(self.device).eval() + + if self.use_channels_last and self.device.type == "cuda": + self.model.to(memory_format=torch.channels_last) + + torch.backends.cudnn.benchmark = True + + extra = ckpt.get("extra", {}) or {} + label_names_raw = extra.get("label_name_by_id", {}) or {} + self.label_names = {int(k): str(v) for k, v in label_names_raw.items()} if label_names_raw else {} + + if self.mode == "dual_label": + aux_sd = ckpt.get("aux_head") + if aux_sd is None: + raise RuntimeError("Checkpoint não possui aux_head. Este arquivo parece não ser dual_head_label.") + + max_label_id = -1 + + for k, v in aux_sd.items(): + if k.endswith("net.3.weight") or k.endswith("net.3.bias"): + max_label_id = int(v.shape[0]) - 1 + break + + if max_label_id < 0: + raise RuntimeError("Não consegui inferir número de classes da LabelHead.") + + num_label_classes = max_label_id + 1 + + for i in range(num_label_classes): + self.label_names.setdefault(i, f"label_{i}") + + feat_ch = int(self.model.config.hidden_sizes[-1]) + + self.aux_head = LabelHead( + feat_ch=feat_ch, + num_seg_classes=num_classes, + num_label_classes=num_label_classes, + hidden=256, + dropout=0.2, + ) + + self.aux_head.load_state_dict(aux_sd, strict=True) + self.aux_head.to(self.device).eval() + if self.use_channels_last and self.device.type == "cuda": + # Linear não usa channels_last, mas manter aqui não atrapalha. + pass + + print(f"[DUAL] LabelHead carregada: classes={num_label_classes} names={self.label_names}") + + else: + self.aux_head = None + print("[SEG] Modo single carregado.") + + print(f"[MODEL] ckpt={pt_path}") + print(f"[MODEL] epoch={ckpt.get('epoch')} bests={ckpt.get('bests')}") def compute_roi_indices(self, H: int, zona_inicio: float, faixa_atuacao: float): y_inicio = int((1.0 - zona_inicio) * H) y_fim = int((1.0 - (zona_inicio + faixa_atuacao)) * H) + y_fim = max(0, min(H, y_fim)) y_inicio = max(0, min(H, y_inicio)) + if y_fim >= y_inicio: y_fim = max(0, y_inicio - 1) + return y_fim, y_inicio def resize_keep_width(self, img: np.ndarray, new_w: int, min_h: int, interpolation: int) -> np.ndarray: h, w = img.shape[:2] - new_h = int(round(new_w * (h / w))) + new_h = int(round(new_w * (h / max(1, w)))) + if min_h is not None and new_h < min_h: new_h = min_h + return cv2.resize(img, (new_w, new_h), interpolation=interpolation) - @torch.no_grad() - def segformer_predict_ids(self, img_tensor): - """ - img_tensor: [1,3,H,W] float32 normalizado. - retorna: pred_ids [H,W] (numpy int) - """ - out = self.model(pixel_values=img_tensor) - logits = out.logits # [B, C, h, w] (pode ser menor que input) - # Upsample logits para o tamanho do input - logits = torch.nn.functional.interpolate( - logits, - size=img_tensor.shape[-2:], - mode="bilinear", - align_corners=False + def _preprocess_roi(self, roi_rgb: np.ndarray): + roi_resized = self.resize_keep_width( + roi_rgb, + self.resolucao[0], + self.resolucao[1], + cv2.INTER_AREA, ) - pred = torch.argmax(logits, dim=1) # [B,H,W] - return pred.squeeze(0).cpu().numpy().astype(np.uint8) + # Garante array contínuo para reduzir cópia torta no torch.from_numpy + roi_resized = np.ascontiguousarray(roi_resized) + img_tensor = torch.from_numpy(roi_resized).to( + device=self.device, + dtype=torch.float32, + non_blocking=True + ) + # HWC -> NCHW + img_tensor = img_tensor.permute(2, 0, 1).unsqueeze(0) + img_tensor = img_tensor.div_(255.0) + + img_tensor = self.normalize_img(img_tensor) + + if self.use_channels_last and self.device.type == "cuda": + img_tensor = img_tensor.contiguous(memory_format=torch.channels_last) + + return img_tensor, roi_resized + + @torch.inference_mode() def infer_ids(self, frame_rgb): - if False: - img_rgb = np.array(Image.open(r"C:\ZendionInc\agrobot_base\t_cor\108_rgb.jpeg").convert("RGB")) - H, W = img_rgb.shape[:2] - y_fim, y_inicio = self.compute_roi_indices(H, self.roi_inicio, self.roi_tamanho) - roi = img_rgb[y_fim:y_inicio, 0:W] - else: - H, W = frame_rgb.shape[:2] - y_fim, y_inicio = self.compute_roi_indices(H, self.roi_inicio, self.roi_tamanho) - roi = frame_rgb[y_fim:y_inicio, 0:W] + if frame_rgb is None or not hasattr(frame_rgb, "shape") or frame_rgb.size == 0: + return None, None, None, None, None + t0 = time.perf_counter() + H, W = frame_rgb.shape[:2] + y_fim, y_inicio = self.compute_roi_indices(H, self.roi_inicio, self.roi_tamanho) - roi_resized = self.resize_keep_width(roi, self.resolucao[0], self.resolucao[1], cv2.INTER_AREA) - roi_norm = roi_resized.astype(np.float32) / 255.0 - img_tensor = torch.from_numpy(roi_norm).permute(2, 0, 1).unsqueeze(0).to(self.device) - img_tensor = self.normalize_img(img_tensor).float() + roi_rgb = frame_rgb[y_fim:y_inicio, 0:W] + t_crop = time.perf_counter() + if roi_rgb is None or roi_rgb.size == 0: + return None, None, None, (y_fim, y_inicio), None + img_tensor, roi_resized = self._preprocess_roi(roi_rgb) + t_pre = time.perf_counter() + + use_amp_now = self.use_amp and self.device.type == "cuda" + + with torch.autocast(device_type="cuda", dtype=torch.float16, enabled=use_amp_now): + out = self.model(pixel_values=img_tensor, output_hidden_states=True) + + t_model = time.perf_counter() + + logits_seg = out.logits + + t_arg0 = time.perf_counter() + pred_low = torch.argmax(logits_seg, dim=1)[0] + pred_low_np = pred_low.detach().cpu().numpy().astype(np.uint8) + pred_ids_np = cv2.resize( + pred_low_np, + (roi_resized.shape[1], roi_resized.shape[0]), + interpolation=cv2.INTER_NEAREST + ) + t_arg1 = time.perf_counter() + + aux_result = None + + t_aux0 = time.perf_counter() + if self.mode == "dual_label" and self.aux_head is not None: + feat = out.hidden_states[-1] + + if feat.shape[-2:] != logits_seg.shape[-2:]: + feat = F.interpolate( + feat, + size=logits_seg.shape[-2:], + mode="bilinear", + align_corners=False, + ) + + with torch.autocast(device_type="cuda", dtype=torch.float16, enabled=use_amp_now): + logits_label = self.aux_head(feat, logits_seg) + + probs = torch.softmax(logits_label, dim=1)[0].detach().cpu().numpy() + + label_id = int(np.argmax(probs)) + label_conf = float(probs[label_id]) + label_name = self.label_names.get(label_id, f"label_{label_id}") + + if self.use_compact_aux: + aux_result = { + "type": "label", + "label_id": label_id, + "label_name": label_name, + "label_conf": label_conf, + } + else: + aux_result = { + "type": "label", + "label_id": label_id, + "label_name": label_name, + "label_conf": label_conf, + "label_probs": probs.astype(float).tolist(), + "label_names": self.label_names, + } + if aux_result is not None: + self._last_aux_result = aux_result + self._last_aux_ts = time.time() + + t_aux1 = time.perf_counter() - pred_ids = self.segformer_predict_ids(img_tensor) # (H,W) self.last_infer = time.time() - return pred_ids, self.last_infer, roi_resized, (y_fim, y_inicio) + + t_end = time.perf_counter() + + if self.debug_timing: + print( + f"RUNNER | crop={(t_crop-t0)*1000:.1f}ms | " + f"pre={(t_pre-t_crop)*1000:.1f}ms | " + f"model={(t_model-t_pre)*1000:.1f}ms | " + f"arg={(t_arg1-t_arg0)*1000:.1f}ms | " + f"aux={(t_aux1-t_aux0)*1000:.1f}ms | " + f"total={(t_end-t0)*1000:.1f}ms" + ) + + return pred_ids_np, self.last_infer, roi_resized, (y_fim, y_inicio), aux_result + @torch.inference_mode() + def infer_ids_seg_only(self, frame_rgb): + """ + Inferência rápida apenas da cabeça de segmentação. + + Diferenças para infer_ids(): + - não pede hidden_states; + - não roda aux_head; + - reutiliza self._last_aux_result, se existir; + - mantém o mesmo formato de retorno: + pred_ids_np, ts, roi_resized, roi_info, aux_result + """ + if frame_rgb is None or not hasattr(frame_rgb, "shape") or frame_rgb.size == 0: + return None, None, None, None, None + + t0 = time.perf_counter() + + H, W = frame_rgb.shape[:2] + y_fim, y_inicio = self.compute_roi_indices( + H, + self.roi_inicio, + self.roi_tamanho + ) + + roi_rgb = frame_rgb[y_fim:y_inicio, 0:W] + t_crop = time.perf_counter() + + if roi_rgb is None or roi_rgb.size == 0: + return None, None, None, (y_fim, y_inicio), None + + img_tensor, roi_resized = self._preprocess_roi(roi_rgb) + t_pre = time.perf_counter() + + use_amp_now = getattr(self, "use_amp", True) and self.device.type == "cuda" + + # Aqui está o ponto principal do teste: + # NÃO pede hidden_states, então o modelo só precisa entregar logits de segmentação. + with torch.autocast( + device_type="cuda", + dtype=torch.float16, + enabled=use_amp_now + ): + out = self.model( + pixel_values=img_tensor, + output_hidden_states=False + ) + + t_model = time.perf_counter() + + logits_seg = out.logits + + t_arg0 = time.perf_counter() + + pred_low = torch.argmax(logits_seg, dim=1)[0] + pred_low_np = pred_low.detach().cpu().numpy().astype(np.uint8) + + pred_ids_np = cv2.resize( + pred_low_np, + (roi_resized.shape[1], roi_resized.shape[0]), + interpolation=cv2.INTER_NEAREST + ) + + t_arg1 = time.perf_counter() + + # Reaproveita o último status conhecido. + # Para o teste inicial, pode ser None mesmo. + aux_result = getattr(self, "_last_aux_result", None) + + if aux_result is not None: + aux_result = dict(aux_result) + aux_result["stale"] = True + aux_result["age_ms"] = ( + time.time() - getattr(self, "_last_aux_ts", time.time()) + ) * 1000.0 + + self.last_infer = time.time() + t_end = time.perf_counter() + + if getattr(self, "debug_timing", False): + print( + f"RUNNER_SEG_ONLY | " + f"crop={(t_crop - t0) * 1000:.1f}ms | " + f"pre={(t_pre - t_crop) * 1000:.1f}ms | " + f"model={(t_model - t_pre) * 1000:.1f}ms | " + f"arg={(t_arg1 - t_arg0) * 1000:.1f}ms | " + f"total={(t_end - t0) * 1000:.1f}ms" + ) + + return pred_ids_np, self.last_infer, roi_resized, (y_fim, y_inicio), aux_result - \ No newline at end of file diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/main.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/main.py index ba7dc09fb..85c06e9c4 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/main.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/main.py @@ -30,7 +30,7 @@ def main(): T_Code.Sen: ModuloSensoriamento(), T_Code.Atu: ModuloAtuador(), T_Code.Lra: ModuloLoRa(), - T_Code.Imu: IMUCamera(), + #T_Code.Imu: IMUCamera(), T_Code.Npc: ModuloPC(), T_Code.Lvx: ModuloLivox(), T_Code.Ipb: ModuloIPBribge(), diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/modulos/imu.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/modulos/imu.py index 5bdcd529a..24b2fab46 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/modulos/imu.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/modulos/imu.py @@ -9,13 +9,18 @@ from shared.enums import StatusModulo, T_Code from health_worker.modulos.base import ModuloDiagnosticoBase class IMUCamera(ModuloDiagnosticoBase): - def __init__(self, queue=None, freq=100, angulo_inicial=26.3): + def __init__(self, mx_id, queue=None, freq=100, angulo_inicial=26.3): + self.mx_id = mx_id self.freq = freq + self.ultima_saude = None + self.ativo = False self.last_packet_ts = 0.0 self.last_publish_ts = 0.0 self.ultimo_erro_ts = 0.0 + + self.last_data = None if queue is None: return @@ -75,6 +80,13 @@ class IMUCamera(ModuloDiagnosticoBase): self.ativo = False self.mostrar_log("Task parada") + try: + th = getattr(self, "imu_thread", None) + if th is not None and th.is_alive(): + th.join(timeout=1.0) + except Exception: + pass + def _mediana_curta(self, hist, valor): hist.append(valor) return float(np.median(hist)) @@ -177,6 +189,7 @@ class IMUCamera(ModuloDiagnosticoBase): self.estabilidade_idx = round(self.clamp(estabilidade_idx, 0.0, 100.0), 1) def imu_task_loop(self): + from camera_worker.manager import definir_imu_camera t0 = time.perf_counter() while self.ativo: @@ -308,26 +321,48 @@ class IMUCamera(ModuloDiagnosticoBase): ts_imu = (time.time() * 1000) - ContextoGlobalRedis.atualizar_ctx_dict( - ContextoGlobalRedis.ModKey(T_Code.Imu), - roll=round(-roll, 2), - pitch=round(pitch, 2), - yaw=round(yaw, 2), - roll_seg=round(-self.roll_seg, 2), - pitch_seg=round(self.pitch_seg, 2), - vel_mps=round(velocidade_mps, 3), - em_movimento=self.em_movimento, - movimento_idx=round(self.movimento_idx, 1), - rugosidade_idx=round(self.rugosidade_idx, 1), - impacto_idx=round(self.impacto_idx, 1), - estabilidade_idx=round(self.estabilidade_idx, 1), - timestamp=ts_imu, - last_packet_ts=self.last_packet_ts, - last_publish_ts=self.last_publish_ts, - ultimo_erro_ts=self.ultimo_erro_ts, - latencia=latencia, - frequencia=f_tick - ) + #ContextoGlobalRedis.atualizar_ctx_dict( + # ContextoGlobalRedis.ModKey(T_Code.Imu), + # roll=round(-roll, 2), + # pitch=round(pitch, 2), + # yaw=round(yaw, 2), + # roll_seg=round(-self.roll_seg, 2), + # pitch_seg=round(self.pitch_seg, 2), + # vel_mps=round(velocidade_mps, 3), + # em_movimento=self.em_movimento, + # movimento_idx=round(self.movimento_idx, 1), + # rugosidade_idx=round(self.rugosidade_idx, 1), + # impacto_idx=round(self.impacto_idx, 1), + # estabilidade_idx=round(self.estabilidade_idx, 1), + # timestamp=ts_imu, + # last_packet_ts=self.last_packet_ts, + # last_publish_ts=self.last_publish_ts, + # ultimo_erro_ts=self.ultimo_erro_ts, + # latencia=latencia, + # frequencia=f_tick + #) + + self.last_data = { + "roll": round(-roll, 2), + "pitch": round(pitch, 2), + "yaw": round(yaw, 2), + "roll_seg": round(-self.roll_seg, 2), + "pitch_seg": round(self.pitch_seg, 2), + "vel_mps": round(velocidade_mps, 3), + "em_movimento": self.em_movimento, + "movimento_idx": round(self.movimento_idx, 1), + "rugosidade_idx": round(self.rugosidade_idx, 1), + "impacto_idx": round(self.impacto_idx, 1), + "estabilidade_idx": round(self.estabilidade_idx, 1), + "timestamp": ts_imu, + "last_packet_ts": self.last_packet_ts, + "last_publish_ts": self.last_publish_ts, + "ultimo_erro_ts": self.ultimo_erro_ts, + "latencia": latencia, + "frequencia": f_tick + } + + definir_imu_camera(self.mx_id, self.last_data, self.ultima_saude) except Exception as e: self.ultimo_erro_ts = time.time() @@ -345,7 +380,7 @@ class IMUCamera(ModuloDiagnosticoBase): delay_corrigido = max(0.0, (1.0 / self.freq) - latencia) time.sleep(delay_corrigido) - def atualizar_saude(self): + def atualizar_saude_bkp(self): try: agora = time.time() modulo = ContextoGlobalRedis.get_modulo(T_Code.Imu) or {} @@ -469,5 +504,156 @@ class IMUCamera(ModuloDiagnosticoBase): except Exception as e: self.mostrar_log(f"Erro ao atualizar saude: {e}") + def atualizar_saude(self): + try: + agora = time.time() + + FREQ_BASE = float(self.freq) + FREQ_MIN = FREQ_BASE * 0.5 + SAUDE_MIN_ALERTA = 80 + SAUDE_MIN_FALHA = 45 + + data = self.get_dados() + + last_packet_ts = float(data.get("last_packet_ts", self.last_packet_ts) or 0.0) + last_publish_ts = float(data.get("last_publish_ts", self.last_publish_ts) or 0.0) + ultimo_erro_ts = float(data.get("ultimo_erro_ts", self.ultimo_erro_ts) or 0.0) + + freq_hz = float(data.get("frequencia", 0.0) or 0.0) + latencia = float(data.get("latencia", 0.0) or 0.0) + valido = bool(data.get("valido", False)) + + tempo_sem_packet = (agora - last_packet_ts) if last_packet_ts > 0 else 999.0 + tempo_sem_atualizar = (agora - last_publish_ts) if last_publish_ts > 0 else 999.0 + tempo_desde_erro = (agora - ultimo_erro_ts) if ultimo_erro_ts > 0 else 999.0 + + conectado = bool(self.ativo and tempo_sem_packet < 10.0) + + saude = 100 + motivos = [] + + if not self.ativo: + conectado = False + saude = 0 + motivos.append("Thread da IMU parada") + + elif not valido: + saude = 0 + motivos.append("IMU ainda sem leitura válida") + + elif not conectado: + saude = 0 + motivos.append(f"Sem packets da IMU há {tempo_sem_packet:.2f}s") + + else: + if tempo_sem_packet > 1.5: + saude -= 45 + motivos.append(f"Sem packets há {tempo_sem_packet:.2f}s") + elif tempo_sem_packet > 0.7: + saude -= 25 + motivos.append(f"Packets atrasados há {tempo_sem_packet:.2f}s") + elif tempo_sem_packet > 0.3: + saude -= 10 + motivos.append(f"Leve atraso de packets: {tempo_sem_packet:.2f}s") + + if tempo_sem_atualizar > 1.5: + saude -= 30 + motivos.append(f"Sem atualizar dados há {tempo_sem_atualizar:.2f}s") + elif tempo_sem_atualizar > 0.8: + saude -= 15 + motivos.append(f"Atualização atrasada há {tempo_sem_atualizar:.2f}s") + elif tempo_sem_atualizar > 0.3: + saude -= 5 + motivos.append(f"Leve atraso na atualização: {tempo_sem_atualizar:.2f}s") + + if freq_hz <= 0: + saude -= 20 + motivos.append("Frequência zerada") + elif freq_hz <= FREQ_MIN: + saude -= 35 + motivos.append(f"Frequência baixa: {freq_hz:.2f} Hz") + else: + f = min(freq_hz, FREQ_BASE) + erro = (FREQ_BASE - f) / FREQ_BASE + p = min(int(erro * 100), 20) + saude -= p + if p > 0: + motivos.append(f"Frequência abaixo do ideal: {freq_hz:.2f} Hz") + + if latencia > 1.0: + saude -= 25 + motivos.append(f"Latência muito alta: {latencia:.2f}s") + elif latencia > 0.5: + saude -= 15 + motivos.append(f"Latência alta: {latencia:.2f}s") + elif latencia > 0.2: + saude -= 5 + motivos.append(f"Latência moderada: {latencia:.2f}s") + + if tempo_desde_erro < 2.0: + saude -= 20 + motivos.append("Erro muito recente no loop") + elif tempo_desde_erro < 5.0: + saude -= 10 + motivos.append("Erro recente no loop") + + saude = max(0, min(100, saude)) + + status = StatusModulo.OPERANTE + if not conectado: + status = StatusModulo.DESCONECTADO + elif saude <= SAUDE_MIN_FALHA: + status = StatusModulo.FALHA + elif saude < SAUDE_MIN_ALERTA: + status = StatusModulo.ALERTA + + self.ultima_saude = { + "conectado": conectado, + "status": status.value, + "saude": saude, + "motivos": motivos, + "saude_individual": [], + } + + return self.ultima_saude + + except Exception as e: + self.mostrar_log(f"Erro ao atualizar saude: {e}") + self.ultima_saude = { + "conectado": False, + "status": StatusModulo.FALHA.value, + "saude": 0, + "motivos": [str(e)], + "saude_individual": [], + } + return self.ultima_saude + + def get_dados(self): + if self.last_data is None: + return { + "roll": 0.0, + "pitch": 0.0, + "yaw": 0.0, + "roll_seg": 0.0, + "pitch_seg": 0.0, + "vel_mps": 0.0, + "em_movimento": False, + "movimento_idx": 0.0, + "rugosidade_idx": 0.0, + "impacto_idx": 0.0, + "estabilidade_idx": 100.0, + "timestamp": 0.0, + "last_packet_ts": 0.0, + "last_publish_ts": 0.0, + "ultimo_erro_ts": 0.0, + "latencia": 0.0, + "frequencia": 0.0, + "valido": False + } + + data = dict(self.last_data) + data["valido"] = True + return data + def mostrar_log(self, mensagem): print(f"{time.time()} - [IMU][id={id(self)}] {mensagem}") \ No newline at end of file diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/modulos/pc.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/modulos/pc.py index 2ceb3440a..cc76f99c6 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/modulos/pc.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/modulos/pc.py @@ -1,75 +1,186 @@ import time -from typing import Dict, Any +from typing import Dict, Any, Optional, Tuple from shared.contexto_global_redis import ContextoGlobalRedis from shared.enums import StatusModulo, T_Code from health_worker.modulos.base import ModuloDiagnosticoBase -def clamp(v, lo, hi): + +def clamp(v, lo, hi): return lo if v < lo else hi if v > hi else v + def linmap(x, x0, x1, y0, y1): - # Mapeia linearmente x em [x0,x1] para [y0,y1] if x1 == x0: return (y0 + y1) * 0.5 t = (x - x0) / (x1 - x0) return y0 + t * (y1 - y0) + +def as_float(v, default: Optional[float] = None) -> Optional[float]: + try: + if v is None: + return default + return float(v) + except Exception: + return default + + class ModuloPC(ModuloDiagnosticoBase): """ - Monitor de saúde do PC, baseado nos campos publicados pelo C#: - - timestamp (seg unix ou string parseável) - - freq_base (Hz do loop C# - usado para staleness) - - cpu_load (%) - - gpu_load (%) - - hdd_load (% usado do disco atual relevante) - - ram_usage (%) - - cpu_temp (°C) - - gpu_temp (°C) - Persistência: - - Escreve 'saude' (payload completo) em ModKey(Mod) - - Mantém EMA por métrica em chave auxiliar 'pc_ema' + Diagnóstico de saúde do PC. + + Entrada esperada no Redis: + ContextoGlobalRedis.ModKey(T_Code.Npc) + + Campos principais: + timestamp + freq_base + + cpu_load_percent + cpu_temp_package_c + cpu_power_package_w + cpu_clock_avg_mhz + cpu_clock_max_mhz + + ram_usage_percent + ram_used_gb + ram_available_gb + ram_total_gb + + gpu_load_percent + gpu_core_load_percent + gpu_frame_buffer_load_percent + gpu_memory_load_percent + gpu_video_engine_load_percent + gpu_bus_interface_load_percent + gpu_temp_core_c + gpu_power_w + gpu_vram_used_mb + gpu_vram_total_mb + gpu_vram_used_percent + gpu_fan_percent + gpu_fan_rpm + + disk_used_space_percent + + process_thread_count + process_working_set_mb + process_private_memory_mb + process_virtual_memory_mb + + threadpool_used_worker_percent + threadpool_used_completion_port_percent + + Saída: + escreve 'saude' dentro do próprio ModKey(T_Code.Npc). """ + def __init__(self): self.t_code = T_Code.Npc self.nome = "PC" - # Rodamos por padrão a cada ~1s no scheduler; este timeout é semáforo lógico do módulo self.timeout = 5 - # Configurações (ajuste conforme a realidade do seu PC) self.CONFIG = { - # Janela e fator de histerese (EMA) - "ema_alpha": 0.3, # mais alto = reage mais rápido - "ema_decay_secs": 6.0, # rebaixa importância de EMA se ficar velho + # Suavização + "ema_alpha": 0.30, + "ema_decay_secs": 6.0, - # Staleness / latência de atualização do produtor (C#) - "stale_min_timeout": 5.0, # piso (segundos) - "stale_k_periods": 3.0, # k * (1/freq_base) + # Staleness + "stale_min_timeout": 5.0, + "stale_k_periods": 3.0, - # Thresholds (faixas típicas sob carga contínua) - "cpu_temp_ok": 70.0, "cpu_temp_warn": 85.0, "cpu_temp_fail": 90.0, - "gpu_temp_ok": 75.0, "gpu_temp_warn": 88.0, "gpu_temp_fail": 92.0, + # Temperaturas + "cpu_temp_ok": 72.0, + "cpu_temp_warn": 85.0, + "cpu_temp_fail": 92.0, - "cpu_load_ok": 85.0, "cpu_load_warn": 92.0, "cpu_load_fail": 98.0, - "gpu_load_ok": 95.0, "gpu_load_warn": 98.0, "gpu_load_fail": 100.0, # GPU alta pode ser normal + "gpu_temp_ok": 75.0, + "gpu_temp_warn": 84.0, + "gpu_temp_fail": 89.0, - "ram_ok": 70.0, "ram_warn": 85.0, "ram_fail": 95.0, # % - "hdd_ok": 80.0, "hdd_warn": 90.0, "hdd_fail": 95.0, # % de ocupação + # Loads + "cpu_load_ok": 82.0, + "cpu_load_warn": 92.0, + "cpu_load_fail": 98.0, - # Pesos na saúde global (somatório = 100 idealmente) - "w_cpu_temp": 22.0, - "w_gpu_temp": 16.0, - "w_cpu_load": 18.0, - "w_gpu_load": 10.0, # penaliza menos (GPU alta com temp ok é aceitável) - "w_ram": 18.0, - "w_hdd": 8.0, - "w_fresh": 8.0, # frescor do dado (staleness) + # GPU alta pode ser normal em inferência, então pontuação é mais permissiva + "gpu_load_ok": 90.0, + "gpu_load_warn": 97.0, + "gpu_load_fail": 100.0, + + "gpu_core_load_ok": 90.0, + "gpu_core_load_warn": 97.0, + "gpu_core_load_fail": 100.0, + + "gpu_memory_load_ok": 80.0, + "gpu_memory_load_warn": 92.0, + "gpu_memory_load_fail": 98.0, + + "gpu_frame_buffer_ok": 80.0, + "gpu_frame_buffer_warn": 92.0, + "gpu_frame_buffer_fail": 98.0, + + # Memória + "ram_ok": 72.0, + "ram_warn": 85.0, + "ram_fail": 95.0, + + "ram_available_warn_gb": 4.0, + "ram_available_fail_gb": 1.5, + + "vram_ok": 70.0, + "vram_warn": 85.0, + "vram_fail": 95.0, + + "vram_free_warn_mb": 1500.0, + "vram_free_fail_mb": 500.0, + + # Disco ocupado, não atividade do disco + "disk_used_ok": 80.0, + "disk_used_warn": 90.0, + "disk_used_fail": 96.0, + + # Energia/thermal throttling heurístico + "gpu_power_warn_w": 100.0, + "gpu_power_fail_w": 125.0, + + # Processo C# + "process_private_mem_warn_mb": 6000.0, + "process_private_mem_fail_mb": 10000.0, + "process_threads_warn": 120, + "process_threads_fail": 220, + + # ThreadPool + "threadpool_used_worker_warn": 30.0, + "threadpool_used_worker_fail": 70.0, + + # Pesos da saúde global + "w_fresh": 10.0, + + "w_cpu_temp": 14.0, + "w_cpu_load": 10.0, + "w_cpu_power_clock": 4.0, + + "w_gpu_temp": 14.0, + "w_gpu_load": 8.0, + "w_gpu_memory": 10.0, + "w_gpu_power": 4.0, + + "w_ram": 14.0, + "w_vram": 12.0, + "w_disk": 5.0, + + "w_process": 5.0, } - def _ema(self, key: str, val: float, now: float, ema_state: Dict[str, Any]) -> float: - """EMA com decaimento se a leitura anterior estiver velha.""" + # ========================================================== + # EMA + # ========================================================== + + def _ema(self, key: str, val: Optional[float], now: float, ema_state: Dict[str, Any]) -> Optional[float]: if val is None: - return ema_state.get(key, val) + return ema_state.get(key, None) alpha = self.CONFIG["ema_alpha"] last_ts = ema_state.get(f"{key}_ts", None) @@ -79,267 +190,813 @@ class ModuloPC(ModuloDiagnosticoBase): ema = val else: age = max(0.0, now - float(last_ts)) - # se ficou velho demais, aproxime do valor atual (reduz “memória”) - if age > self.CONFIG["ema_decay_secs"]: - alpha_eff = 0.7 - else: - alpha_eff = alpha + alpha_eff = 0.70 if age > self.CONFIG["ema_decay_secs"] else alpha ema = alpha_eff * val + (1.0 - alpha_eff) * prev ema_state[key] = ema ema_state[f"{key}_ts"] = now return ema - # ---------- scoring por métrica ---------- + # ========================================================== + # Scoring básico + # ========================================================== - def _score_temp(self, tC: float, ok: float, warn: float, fail: float) -> (float, StatusModulo): - if tC is None: - return 100.0, StatusModulo.DESCONECTADO - # 100 em <= ok ; 0 em >= fail ; decaimento linear - if tC <= ok: + def _score_high_bad( + self, + value: Optional[float], + ok: float, + warn: float, + fail: float, + missing_status=StatusModulo.DESCONECTADO, + ) -> Tuple[float, StatusModulo]: + if value is None: + return 100.0, missing_status + + value = float(value) + + if value <= ok: return 100.0, StatusModulo.OPERANTE - if tC >= fail: + if value >= fail: return 0.0, StatusModulo.FALHA - # entre ok e fail, com marca de aviso - status = StatusModulo.ALERTA if tC >= warn else StatusModulo.OPERANTE - score = linmap(tC, ok, fail, 100.0, 0.0) + + status = StatusModulo.ALERTA if value >= warn else StatusModulo.OPERANTE + score = linmap(value, ok, fail, 100.0, 0.0) return clamp(score, 0.0, 100.0), status - def _score_load(self, pct: float, ok: float, warn: float, fail: float, treat_high_as_normal=False) -> (float, StatusModulo): - if pct is None: - return 100.0, StatusModulo.DESCONECTADO - pct = clamp(pct, 0.0, 100.0) - if treat_high_as_normal: - # penaliza pouco até warn; zera só próximo do fail E se temperatura também alta (checagem fora) - if pct <= ok: - return 100.0, StatusModulo.OPERANTE - if pct >= fail: - return 40.0, StatusModulo.ALERTA # não zera sozinha - status = StatusModulo.ALERTA if pct >= warn else StatusModulo.OPERANTE - score = linmap(pct, ok, fail, 100.0, 40.0) - return clamp(score, 40.0, 100.0), status - else: - if pct <= ok: - return 100.0, StatusModulo.OPERANTE - if pct >= fail: - return 0.0, StatusModulo.FALHA - status = StatusModulo.ALERTA if pct >= warn else StatusModulo.OPERANTE - score = linmap(pct, ok, fail, 100.0, 0.0) - return clamp(score, 0.0, 100.0), status + def _score_low_bad( + self, + value: Optional[float], + ok_min: float, + warn_min: float, + fail_min: float, + missing_status=StatusModulo.DESCONECTADO, + ) -> Tuple[float, StatusModulo]: + """ + Para métricas em que baixo é ruim, exemplo RAM livre / VRAM livre. + ok_min: acima disso está ok. + warn_min: abaixo disso alerta. + fail_min: abaixo disso falha. + """ + if value is None: + return 100.0, missing_status - def _score_usage(self, pct: float, ok: float, warn: float, fail: float) -> (float, StatusModulo): - if pct is None: - return 100.0, StatusModulo.DESCONECTADO - pct = clamp(pct, 0.0, 100.0) - if pct <= ok: + value = float(value) + + if value >= ok_min: return 100.0, StatusModulo.OPERANTE - if pct >= fail: + if value <= fail_min: return 0.0, StatusModulo.FALHA - status = StatusModulo.ALERTA if pct >= warn else StatusModulo.OPERANTE - score = linmap(pct, ok, fail, 100.0, 0.0) + + status = StatusModulo.ALERTA if value <= warn_min else StatusModulo.OPERANTE + score = linmap(value, fail_min, ok_min, 0.0, 100.0) return clamp(score, 0.0, 100.0), status - def _score_freshness(self, age_s: float, timeout_s: float) -> (float, StatusModulo): - # 100 se age <= timeout/3 ; 0 se age >= timeout - if age_s is None or timeout_s is None: + def _score_gpu_load(self, load: Optional[float], temp: Optional[float]) -> Tuple[float, StatusModulo]: + """ + GPU alta é normal durante IA. + Penaliza mais quando GPU alta vem acompanhada de temperatura alta. + """ + C = self.CONFIG + + if load is None: return 100.0, StatusModulo.DESCONECTADO + + load = clamp(float(load), 0.0, 100.0) + temp = as_float(temp, 0.0) or 0.0 + + if load <= C["gpu_load_ok"]: + return 100.0, StatusModulo.OPERANTE + + if temp < C["gpu_temp_warn"]: + # GPU alta, mas termicamente ok: alerta leve, não falha. + if load >= C["gpu_load_fail"]: + return 70.0, StatusModulo.ALERTA + score = linmap(load, C["gpu_load_ok"], C["gpu_load_fail"], 100.0, 70.0) + return clamp(score, 70.0, 100.0), StatusModulo.OPERANTE if load < C["gpu_load_warn"] else StatusModulo.ALERTA + + # GPU alta + quente: penaliza de verdade. + if load >= C["gpu_load_fail"]: + return 20.0, StatusModulo.ALERTA + score = linmap(load, C["gpu_load_ok"], C["gpu_load_fail"], 90.0, 20.0) + return clamp(score, 20.0, 90.0), StatusModulo.ALERTA + + def _score_freshness(self, age_s: Optional[float], timeout_s: float) -> Tuple[float, StatusModulo]: + if age_s is None: + return 0.0, StatusModulo.FALHA + lo = timeout_s / 3.0 hi = timeout_s + if age_s <= lo: return 100.0, StatusModulo.OPERANTE if age_s >= hi: return 0.0, StatusModulo.FALHA - score = linmap(age_s, lo, hi, 100.0, 0.0) - return clamp(score, 0.0, 100.0), (StatusModulo.ALERTA if age_s >= (0.7 * hi) else StatusModulo.OPERANTE) - # ---------- principal ---------- + score = linmap(age_s, lo, hi, 100.0, 0.0) + status = StatusModulo.ALERTA if age_s >= 0.7 * hi else StatusModulo.OPERANTE + return clamp(score, 0.0, 100.0), status + + # ========================================================== + # Utilitários de relatório + # ========================================================== + + def _motivo(self, nome: str, status: StatusModulo, val=None, unidade=""): + if status == StatusModulo.OPERANTE: + return [] + + if val is None: + vtxt = "s/ dado" + else: + try: + vtxt = f"{float(val):.1f}{unidade}" + except Exception: + vtxt = f"{val}{unidade}" + + return [f"{nome}:{status.value}({vtxt})"] + + def _cond(self, label, valor, severidade, descricao, acoes=None): + return { + "label": label, + "valor": valor, + "severidade": severidade, + "descricao": descricao, + "acoes": acoes or [], + } + + def _item(self, id_, label, status, score, motivos, condicoes, em_uso=True, valor=None, unidade=""): + return { + "id": id_, + "label": label, + "status": status.value if hasattr(status, "value") else status, + "saude": round(clamp(score, 0.0, 100.0), 1), + "valor": valor, + "unidade": unidade, + "motivos": motivos, + "condicoes_operacionais": condicoes, + "em_uso": em_uso, + } + + # ========================================================== + # Principal + # ========================================================== def atualizar_saude(self): try: now = time.time() m = ContextoGlobalRedis.get_modulo(self.t_code) or {} - # Produtor C# - período alvo (freq_base em Hz => período ~ 1/f) - freq_base = float(m.get("freq_base", 1.0)) or 1.0 - period_alvo = 1.0 / freq_base - timeout_stale = max(self.CONFIG["stale_min_timeout"], self.CONFIG["stale_k_periods"] * period_alvo) + freq_base = as_float(m.get("freq_base"), 1.0) or 1.0 + period_alvo = 1.0 / max(freq_base, 1e-6) + timeout_stale = max( + self.CONFIG["stale_min_timeout"], + self.CONFIG["stale_k_periods"] * period_alvo, + ) - # Timestamp do produtor - ts_prod = m.get("timestamp", None) - try: - ts_prod = float(ts_prod) - except Exception: - ts_prod = None + ts_prod = as_float(m.get("timestamp"), None) age_rx = None if ts_prod is None else max(0.0, now - ts_prod) - # Métricas brutas - cpu_load = m.get("cpu_load") - gpu_load = m.get("gpu_load") - hdd_load = m.get("hdd_load") - ram_usage = m.get("ram_usage") - cpu_temp = m.get("cpu_temp") - gpu_temp = m.get("gpu_temp") + raw = { + # CPU + "cpu_load_percent": as_float(m.get("cpu_load_percent"), None), + "cpu_temp_package_c": as_float(m.get("cpu_temp_package_c"), None), + "cpu_power_package_w": as_float(m.get("cpu_power_package_w"), None), + "cpu_clock_avg_mhz": as_float(m.get("cpu_clock_avg_mhz"), None), + "cpu_clock_max_mhz": as_float(m.get("cpu_clock_max_mhz"), None), - # EMA - ema_state = m.get("saude", {}).get("detalhes", {}).get("ema", {}) - cpu_load_e = self._ema("cpu_load", cpu_load, now, ema_state) - gpu_load_e = self._ema("gpu_load", gpu_load, now, ema_state) - hdd_load_e = self._ema("hdd_load", hdd_load, now, ema_state) - ram_usage_e = self._ema("ram_usage", ram_usage, now, ema_state) - cpu_temp_e = self._ema("cpu_temp", cpu_temp, now, ema_state) - gpu_temp_e = self._ema("gpu_temp", gpu_temp, now, ema_state) + # RAM + "ram_usage_percent": as_float(m.get("ram_usage_percent"), None), + "ram_used_gb": as_float(m.get("ram_used_gb"), None), + "ram_available_gb": as_float(m.get("ram_available_gb"), None), + "ram_total_gb": as_float(m.get("ram_total_gb"), None), + + # GPU + "gpu_load_percent": as_float(m.get("gpu_load_percent"), None), + "gpu_core_load_percent": as_float(m.get("gpu_core_load_percent"), None), + "gpu_frame_buffer_load_percent": as_float(m.get("gpu_frame_buffer_load_percent"), None), + "gpu_memory_load_percent": as_float(m.get("gpu_memory_load_percent"), None), + "gpu_video_engine_load_percent": as_float(m.get("gpu_video_engine_load_percent"), None), + "gpu_bus_interface_load_percent": as_float(m.get("gpu_bus_interface_load_percent"), None), + + "gpu_temp_core_c": as_float(m.get("gpu_temp_core_c"), None), + "gpu_power_w": as_float(m.get("gpu_power_w"), None), + "gpu_core_clock_mhz": as_float(m.get("gpu_core_clock_mhz"), None), + "gpu_memory_clock_mhz": as_float(m.get("gpu_memory_clock_mhz"), None), + "gpu_fan_percent": as_float(m.get("gpu_fan_percent"), None), + "gpu_fan_rpm": as_float(m.get("gpu_fan_rpm"), None), + + # VRAM + "gpu_vram_total_mb": as_float(m.get("gpu_vram_total_mb"), None), + "gpu_vram_used_mb": as_float(m.get("gpu_vram_used_mb"), None), + "gpu_vram_free_mb": as_float(m.get("gpu_vram_free_mb"), None), + "gpu_vram_used_percent": as_float(m.get("gpu_vram_used_percent"), None), + + # Disco + "disk_used_space_percent": as_float(m.get("disk_used_space_percent"), None), + + # Processo + "process_thread_count": as_float(m.get("process_thread_count"), None), + "process_working_set_mb": as_float(m.get("process_working_set_mb"), None), + "process_private_memory_mb": as_float(m.get("process_private_memory_mb"), None), + "process_virtual_memory_mb": as_float(m.get("process_virtual_memory_mb"), None), + + # ThreadPool + "threadpool_used_worker_threads": as_float(m.get("threadpool_used_worker_threads"), None), + "threadpool_used_completion_port_threads": as_float(m.get("threadpool_used_completion_port_threads"), None), + "threadpool_used_worker_percent": as_float(m.get("threadpool_used_worker_percent"), None), + "threadpool_used_completion_port_percent": as_float(m.get("threadpool_used_completion_port_percent"), None), + } + + # Fallback: se percent VRAM não vier, calcula. + if raw["gpu_vram_used_percent"] is None: + used = raw["gpu_vram_used_mb"] + total = raw["gpu_vram_total_mb"] + if used is not None and total is not None and total > 0: + raw["gpu_vram_used_percent"] = 100.0 * used / total + + # EMA persistida dentro da própria saúde + ema_state = ( + m.get("saude", {}) + .get("detalhes", {}) + .get("ema", {}) + ) + + ema = {} + for key, value in raw.items(): + if isinstance(value, (int, float)): + ema[key] = self._ema(key, value, now, ema_state) + else: + ema[key] = value C = self.CONFIG motivos = [] condicoes = [] saude_individual = [] - def add_motivo(nome, status, val=None, unidade=""): - if status != StatusModulo.OPERANTE: - vtxt = f"{val:.0f}{unidade}" if val is not None else "s/ dado" - motivos.append(f"{nome}:{status}({vtxt})") - return [vtxt] - return [] + # ====================================================== + # Freshness + # ====================================================== + sc_fresh, st_fresh = self._score_freshness(age_rx, timeout_stale) + fresh_cond = [] + if st_fresh != StatusModulo.OPERANTE: + fresh_cond.append(self._cond( + "Dados do PC desatualizados", + age_rx, + 100 if st_fresh == StatusModulo.FALHA else 70, + "O C# parou de atualizar a telemetria do PC no Redis.", + [ + "Verificar timer de leituras no C#", + "Verificar comunicação com Redis", + "Verificar travamento do processo principal", + ], + )) + motivos += self._motivo("fresh", st_fresh, age_rx, "s") + condicoes += fresh_cond + saude_individual.append(self._item( + "fresh", + "Atualização da Telemetria", + st_fresh, + sc_fresh, + self._motivo("fresh", st_fresh, age_rx, "s"), + fresh_cond, + valor=age_rx, + unidade="s", + )) - # Scores - sc_cpuT, st_cpuT = self._score_temp(cpu_temp_e, C["cpu_temp_ok"], C["cpu_temp_warn"], C["cpu_temp_fail"]) - sc_gpuT, st_gpuT = self._score_temp(gpu_temp_e, C["gpu_temp_ok"], C["gpu_temp_warn"], C["gpu_temp_fail"]) + # ====================================================== + # CPU + # ====================================================== + cpu_temp = ema["cpu_temp_package_c"] + cpu_load = ema["cpu_load_percent"] + cpu_power = ema["cpu_power_package_w"] - sc_cpuL, st_cpuL = self._score_load(cpu_load_e, C["cpu_load_ok"], C["cpu_load_warn"], C["cpu_load_fail"]) - # GPU load: tratar alto como "normal" se temp ok (penaliza pouco) - treat_gpu_high_as_normal = (gpu_temp_e is not None and gpu_temp_e <= C["gpu_temp_warn"]) - sc_gpuL, st_gpuL = self._score_load(gpu_load_e, C["gpu_load_ok"], C["gpu_load_warn"], C["gpu_load_fail"], treat_high_as_normal=treat_gpu_high_as_normal) + sc_cpu_temp, st_cpu_temp = self._score_high_bad( + cpu_temp, + C["cpu_temp_ok"], + C["cpu_temp_warn"], + C["cpu_temp_fail"], + ) + sc_cpu_load, st_cpu_load = self._score_high_bad( + cpu_load, + C["cpu_load_ok"], + C["cpu_load_warn"], + C["cpu_load_fail"], + ) - sc_RAM, st_RAM = self._score_usage(ram_usage_e, C["ram_ok"], C["ram_warn"], C["ram_fail"]) - sc_HDD, st_HDD = self._score_usage(hdd_load_e, C["hdd_ok"], C["hdd_warn"], C["hdd_fail"]) - sc_fresh, st_fr = self._score_freshness(age_rx, timeout_stale) + cpu_cond = [] + if st_cpu_temp != StatusModulo.OPERANTE: + cpu_cond.append(self._cond( + "Temperatura do CPU", + raw["cpu_temp_package_c"], + 100 if st_cpu_temp == StatusModulo.FALHA else 75, + "Temperatura do processador acima da faixa desejada.", + [ + "Reduzir frequência de loops pesados", + "Reduzir paralelismo em processamento visual", + "Verificar ventilação e temperatura ambiente", + ], + )) - gpu_load_m = add_motivo("gpu_load", st_gpuL, gpu_load, "%") - gpu_load_c = [] - if gpu_load_e and gpu_load_e >= C["gpu_load_warn"]: - gpu_load_c.append({ - "label": "Carga do GPU", - "valor": gpu_load, - "severidade": 100, - "descricao": "GPU em carga elevada", - "acoes": [] - }) - cpu_load_m = add_motivo("cpu_load", st_cpuL, cpu_load, "%") - cpu_temp_m = add_motivo("cpu_temp", st_cpuT, cpu_temp, "°C") - cpu_temp_c = [] - if cpu_temp_e and cpu_temp_e >= C["cpu_temp_warn"]: - gpu_load_c.append({ - "label": "Temperatura do CPU", - "valor": cpu_temp, - "severidade": 100, - "descricao": "Temperatura do CPU alta", - "acoes": [ - "Reduzir threads no CPU (manager/IA)", - "Diminuir frequencia de loop (freq_base)", - "Verificar fluxo de ar (limpar filtros)" - ] - }) - gpu_temp_m = add_motivo("gpu_temp", st_gpuT, gpu_temp, "°C") - gpu_temp_c = [] - if gpu_temp_e and gpu_temp_e >= C["gpu_temp_warn"]: - gpu_temp_c.append({ - "label": "Temperatura do GPU", - "valor": gpu_temp, - "severidade": 100, - "descricao": "Temperatura do GPU alta", - "acoes": [ - "Reduzir FPS modelos IA", - "Baixar resolução da camera IA" - ] - }) - ram_usage_m = add_motivo("ram", st_RAM, ram_usage, "%") - ram_usage_c = [] - if ram_usage_e and ram_usage_e >= C["ram_warn"]: - ram_usage_c.append({ - "label": "Uso de memória RAM", - "valor": ram_usage, - "severidade": 100, - "descricao": "Alto consumo de memória RAM", - "acoes": [ - "Limpar caches/desalocar buffers", - "Reduzir parallelismo em tarefas grandes" - ] - }) - hdd_load_m = add_motivo("hdd", st_HDD, hdd_load, "%") - hdd_load_c = [] - if hdd_load_e and hdd_load_e >= C["hdd_warn"]: - hdd_load_c.append({ - "label": "Uso de HD", - "valor": hdd_load, - "severidade": 100, - "descricao": "Alto consumo de disco", - "acoes": [ - "Liberar espaço em disco/log_rotation" - ] - }) - fresh_m = add_motivo("fresh", st_fr, age_rx, "s") - fresh_c = [] - if age_rx is not None and age_rx >= timeout_stale: - fresh_c.append({ - "label": "Taxa de Atualização", - "valor": age_rx, - "severidade": 100, - "descricao": "Dados desatualizados", - "acoes": [ - "Verificar pipeline C#. Redis (tmrLeiturasInterval)" - ] - }) + if st_cpu_load != StatusModulo.OPERANTE: + cpu_cond.append(self._cond( + "Carga do CPU", + raw["cpu_load_percent"], + 90 if st_cpu_load == StatusModulo.FALHA else 65, + "Processador em carga elevada.", + [ + "Reduzir FPS de tarefas não críticas", + "Revisar loops com polling excessivo", + "Verificar consumo do C# e workers Python", + ], + )) - condicoes.extend(gpu_load_c) - condicoes.extend(cpu_temp_c) - condicoes.extend(gpu_temp_c) - condicoes.extend(ram_usage_c) - condicoes.extend(hdd_load_c) - condicoes.extend(fresh_c) + # Power/clock é heurístico, não falha forte sozinho. + sc_cpu_power_clock = 100.0 + st_cpu_power_clock = StatusModulo.OPERANTE + if cpu_power is not None and cpu_power > 0 and cpu_temp is not None: + if cpu_temp >= C["cpu_temp_warn"] and cpu_load is not None and cpu_load >= C["cpu_load_warn"]: + sc_cpu_power_clock = 70.0 + st_cpu_power_clock = StatusModulo.ALERTA - # Monta saúde global (média ponderada) + motivos += self._motivo("cpu_temp", st_cpu_temp, cpu_temp, "°C") + motivos += self._motivo("cpu_load", st_cpu_load, cpu_load, "%") + condicoes += cpu_cond + + saude_individual.append(self._item( + "cpu_temp", + "Temperatura do CPU", + st_cpu_temp, + sc_cpu_temp, + self._motivo("cpu_temp", st_cpu_temp, cpu_temp, "°C"), + [c for c in cpu_cond if c["label"] == "Temperatura do CPU"], + valor=raw["cpu_temp_package_c"], + unidade="°C", + )) + + saude_individual.append(self._item( + "cpu_load", + "Carga do CPU", + st_cpu_load, + sc_cpu_load, + self._motivo("cpu_load", st_cpu_load, cpu_load, "%"), + [c for c in cpu_cond if c["label"] == "Carga do CPU"], + valor=raw["cpu_load_percent"], + unidade="%", + )) + + saude_individual.append(self._item( + "cpu_power_clock", + "Energia/Clock do CPU", + st_cpu_power_clock, + sc_cpu_power_clock, + self._motivo("cpu_power_clock", st_cpu_power_clock, cpu_power, "W"), + [], + valor=raw["cpu_power_package_w"], + unidade="W", + )) + + # ====================================================== + # GPU + # ====================================================== + gpu_temp = ema["gpu_temp_core_c"] + gpu_load = ema["gpu_load_percent"] + gpu_core_load = ema["gpu_core_load_percent"] + gpu_mem_load = ema["gpu_memory_load_percent"] + gpu_fb_load = ema["gpu_frame_buffer_load_percent"] + gpu_power = ema["gpu_power_w"] + + sc_gpu_temp, st_gpu_temp = self._score_high_bad( + gpu_temp, + C["gpu_temp_ok"], + C["gpu_temp_warn"], + C["gpu_temp_fail"], + ) + sc_gpu_load, st_gpu_load = self._score_gpu_load(gpu_load, gpu_temp) + + sc_gpu_core_load, st_gpu_core_load = self._score_gpu_load(gpu_core_load, gpu_temp) + + sc_gpu_mem_load, st_gpu_mem_load = self._score_high_bad( + gpu_mem_load, + C["gpu_memory_load_ok"], + C["gpu_memory_load_warn"], + C["gpu_memory_load_fail"], + ) + + sc_gpu_fb_load, st_gpu_fb_load = self._score_high_bad( + gpu_fb_load, + C["gpu_frame_buffer_ok"], + C["gpu_frame_buffer_warn"], + C["gpu_frame_buffer_fail"], + ) + + gpu_cond = [] + if st_gpu_temp != StatusModulo.OPERANTE: + gpu_cond.append(self._cond( + "Temperatura do GPU", + raw["gpu_temp_core_c"], + 100 if st_gpu_temp == StatusModulo.FALHA else 75, + "GPU acima da temperatura desejada.", + [ + "Reduzir FPS do visual_worker", + "Reduzir resolução ou frequência da segmentação", + "Verificar ventilação", + ], + )) + + if st_gpu_load != StatusModulo.OPERANTE: + gpu_cond.append(self._cond( + "Carga do GPU", + raw["gpu_load_percent"], + 70 if st_gpu_load == StatusModulo.ALERTA else 90, + "GPU em carga elevada. Pode ser normal durante inferência, mas deve ser observado com temperatura e FPS dos workers.", + [ + "Verificar FPS real do weed_worker", + "Verificar modo do GpuPriorityController", + "Reduzir visual_worker se o weed_worker cair", + ], + )) + + if st_gpu_mem_load != StatusModulo.OPERANTE: + gpu_cond.append(self._cond( + "Carga da Memória do GPU", + raw["gpu_memory_load_percent"], + 80, + "Controlador/memória da GPU sob carga elevada.", + [ + "Verificar tamanho dos tensores", + "Reduzir cópias CPU/GPU", + "Avaliar TensorRT/ONNX", + ], + )) + + if gpu_power is not None and gpu_power >= C["gpu_power_warn_w"]: + gpu_cond.append(self._cond( + "Potência do GPU", + raw["gpu_power_w"], + 80 if gpu_power < C["gpu_power_fail_w"] else 100, + "GPU consumindo potência elevada.", + [ + "Monitorar temperatura", + "Verificar throttling", + ], + )) + + motivos += self._motivo("gpu_temp", st_gpu_temp, gpu_temp, "°C") + motivos += self._motivo("gpu_load", st_gpu_load, gpu_load, "%") + condicoes += gpu_cond + + # Score de GPU memory usa o pior entre memory load e frame buffer. + sc_gpu_memory_block = min(sc_gpu_mem_load, sc_gpu_fb_load) + st_gpu_memory_block = ( + StatusModulo.FALHA + if StatusModulo.FALHA in [st_gpu_mem_load, st_gpu_fb_load] + else StatusModulo.ALERTA + if StatusModulo.ALERTA in [st_gpu_mem_load, st_gpu_fb_load] + else StatusModulo.OPERANTE + ) + + saude_individual.append(self._item( + "gpu_temp", + "Temperatura do GPU", + st_gpu_temp, + sc_gpu_temp, + self._motivo("gpu_temp", st_gpu_temp, gpu_temp, "°C"), + [c for c in gpu_cond if c["label"] == "Temperatura do GPU"], + valor=raw["gpu_temp_core_c"], + unidade="°C", + )) + + saude_individual.append(self._item( + "gpu_load", + "Carga Geral do GPU", + st_gpu_load, + sc_gpu_load, + self._motivo("gpu_load", st_gpu_load, gpu_load, "%"), + [c for c in gpu_cond if c["label"] == "Carga do GPU"], + valor=raw["gpu_load_percent"], + unidade="%", + )) + + saude_individual.append(self._item( + "gpu_core_load", + "Carga do Núcleo GPU", + st_gpu_core_load, + sc_gpu_core_load, + self._motivo("gpu_core_load", st_gpu_core_load, gpu_core_load, "%"), + [], + valor=raw["gpu_core_load_percent"], + unidade="%", + )) + + saude_individual.append(self._item( + "gpu_memory_load", + "Carga de Memória/Framebuffer GPU", + st_gpu_memory_block, + sc_gpu_memory_block, + self._motivo("gpu_memory_load", st_gpu_memory_block, min( + x for x in [gpu_mem_load, gpu_fb_load] if x is not None + ) if gpu_mem_load is not None or gpu_fb_load is not None else None, "%"), + [c for c in gpu_cond if c["label"] == "Carga da Memória do GPU"], + valor=raw["gpu_memory_load_percent"], + unidade="%", + )) + + # ====================================================== + # RAM / VRAM + # ====================================================== + ram_usage = ema["ram_usage_percent"] + ram_avail = ema["ram_available_gb"] + + sc_ram_usage, st_ram_usage = self._score_high_bad( + ram_usage, + C["ram_ok"], + C["ram_warn"], + C["ram_fail"], + ) + + sc_ram_avail, st_ram_avail = self._score_low_bad( + ram_avail, + ok_min=C["ram_available_warn_gb"] * 2.0, + warn_min=C["ram_available_warn_gb"], + fail_min=C["ram_available_fail_gb"], + ) + + sc_ram = min(sc_ram_usage, sc_ram_avail) + st_ram = ( + StatusModulo.FALHA + if StatusModulo.FALHA in [st_ram_usage, st_ram_avail] + else StatusModulo.ALERTA + if StatusModulo.ALERTA in [st_ram_usage, st_ram_avail] + else StatusModulo.OPERANTE + ) + + ram_cond = [] + if st_ram != StatusModulo.OPERANTE: + ram_cond.append(self._cond( + "Memória RAM", + raw["ram_usage_percent"], + 100 if st_ram == StatusModulo.FALHA else 75, + "Uso de RAM alto ou memória disponível baixa.", + [ + "Reduzir cache de imagens", + "Verificar vazamento de memória", + "Reduzir buffers de replay/log visual", + ], + )) + + vram_usage = ema["gpu_vram_used_percent"] + vram_free = ema["gpu_vram_free_mb"] + + sc_vram_usage, st_vram_usage = self._score_high_bad( + vram_usage, + C["vram_ok"], + C["vram_warn"], + C["vram_fail"], + ) + + sc_vram_free, st_vram_free = self._score_low_bad( + vram_free, + ok_min=C["vram_free_warn_mb"] * 2.0, + warn_min=C["vram_free_warn_mb"], + fail_min=C["vram_free_fail_mb"], + ) + + sc_vram = min(sc_vram_usage, sc_vram_free) + st_vram = ( + StatusModulo.FALHA + if StatusModulo.FALHA in [st_vram_usage, st_vram_free] + else StatusModulo.ALERTA + if StatusModulo.ALERTA in [st_vram_usage, st_vram_free] + else StatusModulo.OPERANTE + ) + + vram_cond = [] + if st_vram != StatusModulo.OPERANTE: + vram_cond.append(self._cond( + "Memória VRAM", + raw["gpu_vram_used_percent"], + 100 if st_vram == StatusModulo.FALHA else 75, + "VRAM alta ou memória livre da GPU baixa.", + [ + "Reduzir batch/tamanho dos tensores", + "Evitar manter modelos duplicados se possível", + "Avaliar TensorRT para reduzir footprint", + ], + )) + + motivos += self._motivo("ram", st_ram, ram_usage, "%") + motivos += self._motivo("vram", st_vram, vram_usage, "%") + condicoes += ram_cond + vram_cond + + saude_individual.append(self._item( + "ram", + "Memória RAM", + st_ram, + sc_ram, + self._motivo("ram", st_ram, ram_usage, "%"), + ram_cond, + valor=raw["ram_usage_percent"], + unidade="%", + )) + + saude_individual.append(self._item( + "vram", + "Memória GPU/VRAM", + st_vram, + sc_vram, + self._motivo("vram", st_vram, vram_usage, "%"), + vram_cond, + valor=raw["gpu_vram_used_percent"], + unidade="%", + )) + + # ====================================================== + # Disco + # ====================================================== + disk_used = ema["disk_used_space_percent"] + sc_disk, st_disk = self._score_high_bad( + disk_used, + C["disk_used_ok"], + C["disk_used_warn"], + C["disk_used_fail"], + ) + + disk_cond = [] + if st_disk != StatusModulo.OPERANTE: + disk_cond.append(self._cond( + "Espaço em Disco", + raw["disk_used_space_percent"], + 100 if st_disk == StatusModulo.FALHA else 75, + "Disco com pouco espaço livre. Isso pode afetar logs, replay e salvamento de imagens.", + [ + "Executar limpeza de logs", + "Mover operações antigas", + "Verificar rotação automática de arquivos", + ], + )) + + motivos += self._motivo("disk", st_disk, disk_used, "%") + condicoes += disk_cond + + saude_individual.append(self._item( + "disk_used", + "Espaço Usado em Disco", + st_disk, + sc_disk, + self._motivo("disk", st_disk, disk_used, "%"), + disk_cond, + valor=raw["disk_used_space_percent"], + unidade="%", + )) + + # ====================================================== + # Processo / ThreadPool + # ====================================================== + proc_private = ema["process_private_memory_mb"] + proc_threads = ema["process_thread_count"] + tp_worker = ema["threadpool_used_worker_percent"] + + sc_proc_mem, st_proc_mem = self._score_high_bad( + proc_private, + C["process_private_mem_warn_mb"] * 0.75, + C["process_private_mem_warn_mb"], + C["process_private_mem_fail_mb"], + ) + + sc_proc_threads, st_proc_threads = self._score_high_bad( + proc_threads, + C["process_threads_warn"] * 0.7, + C["process_threads_warn"], + C["process_threads_fail"], + ) + + sc_tp, st_tp = self._score_high_bad( + tp_worker, + C["threadpool_used_worker_warn"] * 0.5, + C["threadpool_used_worker_warn"], + C["threadpool_used_worker_fail"], + ) + + sc_process = min(sc_proc_mem, sc_proc_threads, sc_tp) + st_process = ( + StatusModulo.FALHA + if StatusModulo.FALHA in [st_proc_mem, st_proc_threads, st_tp] + else StatusModulo.ALERTA + if StatusModulo.ALERTA in [st_proc_mem, st_proc_threads, st_tp] + else StatusModulo.OPERANTE + ) + + process_cond = [] + if st_process != StatusModulo.OPERANTE: + process_cond.append(self._cond( + "Processo C#", + raw["process_private_memory_mb"], + 100 if st_process == StatusModulo.FALHA else 70, + "Processo principal com uso elevado de memória, threads ou ThreadPool.", + [ + "Verificar vazamento de memória", + "Revisar criação de threads", + "Verificar tasks longas no C#", + ], + )) + + motivos += self._motivo("process", st_process, proc_private, "MB") + condicoes += process_cond + + saude_individual.append(self._item( + "process", + "Processo Principal", + st_process, + sc_process, + self._motivo("process", st_process, proc_private, "MB"), + process_cond, + valor=raw["process_private_memory_mb"], + unidade="MB", + )) + + # ====================================================== + # Saúde global + # ====================================================== pesos = { - "cpuT": C["w_cpu_temp"], - "gpuT": C["w_gpu_temp"], - "cpuL": C["w_cpu_load"], - "gpuL": C["w_gpu_load"], - "ram": C["w_ram"], - "hdd": C["w_hdd"], - "fr": C["w_fresh"], + "fresh": C["w_fresh"], + + "cpu_temp": C["w_cpu_temp"], + "cpu_load": C["w_cpu_load"], + "cpu_power_clock": C["w_cpu_power_clock"], + + "gpu_temp": C["w_gpu_temp"], + "gpu_load": C["w_gpu_load"], + "gpu_memory": C["w_gpu_memory"], + "gpu_power": C["w_gpu_power"], + + "ram": C["w_ram"], + "vram": C["w_vram"], + "disk": C["w_disk"], + + "process": C["w_process"], } + + # GPU power score leve + if gpu_power is None: + sc_gpu_power = 100.0 + st_gpu_power = StatusModulo.OPERANTE + elif gpu_power >= C["gpu_power_fail_w"]: + sc_gpu_power = 40.0 + st_gpu_power = StatusModulo.ALERTA + elif gpu_power >= C["gpu_power_warn_w"]: + sc_gpu_power = 75.0 + st_gpu_power = StatusModulo.ALERTA + else: + sc_gpu_power = 100.0 + st_gpu_power = StatusModulo.OPERANTE + soma_pesos = sum(pesos.values()) score_total = ( - sc_cpuT * pesos["cpuT"] + - sc_gpuT * pesos["gpuT"] + - sc_cpuL * pesos["cpuL"] + - sc_gpuL * pesos["gpuL"] + - sc_RAM * pesos["ram"] + - sc_HDD * pesos["hdd"] + - sc_fresh* pesos["fr"] - ) / soma_pesos + sc_fresh * pesos["fresh"] + - saude_individual.extend([ - { "id": "cpu_temp", "label": "Temperatura do CPU", "status": st_cpuT.value, "saude": sc_cpuT, "motivos": cpu_temp_m, "condicoes_operacionais": cpu_temp_c, "em_uso": True }, - { "id": "gpu_temp", "label": "Temperatura do GPU", "status": st_gpuT.value, "saude": sc_gpuT, "motivos": gpu_temp_m, "condicoes_operacionais": gpu_temp_c, "em_uso": True }, - { "id": "cpu_load", "label": "Carga do CPU", "status": st_cpuL.value, "saude": sc_cpuL, "motivos": cpu_load_m, "condicoes_operacionais": [], "em_uso": True }, - { "id": "gpu_load", "label": "Carga do GPU", "status": st_gpuL.value, "saude": sc_gpuL, "motivos": gpu_load_m, "condicoes_operacionais": gpu_load_c, "em_uso": True }, - { "id": "ram_usage", "label": "Uso de RAM", "status": st_RAM.value, "saude": sc_RAM, "motivos": ram_usage_m, "condicoes_operacionais": ram_usage_c, "em_uso": True }, - { "id": "hdd_load", "label": "Carga de HD", "status": st_HDD.value, "saude": sc_HDD, "motivos": hdd_load_m, "condicoes_operacionais": hdd_load_c, "em_uso": True }, - { "id": "fresh", "label": "Taxa de Atualização", "status": st_fr.value, "saude": sc_fresh, "motivos": fresh_m, "condicoes_operacionais": fresh_c, "em_uso": True }, + sc_cpu_temp * pesos["cpu_temp"] + + sc_cpu_load * pesos["cpu_load"] + + sc_cpu_power_clock * pesos["cpu_power_clock"] + + + sc_gpu_temp * pesos["gpu_temp"] + + sc_gpu_load * pesos["gpu_load"] + + sc_gpu_memory_block * pesos["gpu_memory"] + + sc_gpu_power * pesos["gpu_power"] + + + sc_ram * pesos["ram"] + + sc_vram * pesos["vram"] + + sc_disk * pesos["disk"] + + + sc_process * pesos["process"] + ) / max(soma_pesos, 1e-6) + + score_total = clamp(score_total, 0.0, 100.0) + + # Regras duras + hard_fail = any([ + st_fresh == StatusModulo.FALHA, + st_cpu_temp == StatusModulo.FALHA, + st_gpu_temp == StatusModulo.FALHA, + st_ram == StatusModulo.FALHA, + st_vram == StatusModulo.FALHA, + st_disk == StatusModulo.FALHA, ]) - if age_rx is not None and age_rx >= timeout_stale: + any_alert = any( + item.get("status") == StatusModulo.ALERTA.value + for item in saude_individual + ) + + if hard_fail: status = StatusModulo.FALHA - motivos.append("dados_sem_atualizacao") - score_total = min(score_total, 20.0) - elif score_total < 80.0: + score_total = min(score_total, 35.0) + elif score_total < 80.0 or any_alert: status = StatusModulo.ALERTA else: status = StatusModulo.OPERANTE payload = { "status": status.value, - "saude": round(clamp(score_total, 0.0, 100.0), 1), + "saude": round(score_total, 1), "motivos": motivos, "saude_individual": saude_individual, "condicoes_operacionais": condicoes, @@ -348,32 +1005,54 @@ class ModuloPC(ModuloDiagnosticoBase): "periodo_alvo_s": period_alvo, "timeout_stale_s": timeout_stale, "age_rx_s": age_rx, - "raw": { - "cpu_load": cpu_load, - "gpu_load": gpu_load, - "hdd_load": hdd_load, - "ram_usage": ram_usage, - "cpu_temp": cpu_temp, - "gpu_temp": gpu_temp, + + "raw": raw, + "ema": ema_state, + + "scores": { + "fresh": sc_fresh, + + "cpu_temp": sc_cpu_temp, + "cpu_load": sc_cpu_load, + "cpu_power_clock": sc_cpu_power_clock, + + "gpu_temp": sc_gpu_temp, + "gpu_load": sc_gpu_load, + "gpu_core_load": sc_gpu_core_load, + "gpu_memory_block": sc_gpu_memory_block, + "gpu_power": sc_gpu_power, + + "ram": sc_ram, + "vram": sc_vram, + "disk": sc_disk, + "process": sc_process, }, - "ema": { - "cpu_load": cpu_load_e, - "gpu_load": gpu_load_e, - "hdd_load": hdd_load_e, - "ram_usage": ram_usage_e, - "cpu_temp": cpu_temp_e, - "gpu_temp": gpu_temp_e, + + "status_componentes": { + "fresh": st_fresh.value, + + "cpu_temp": st_cpu_temp.value, + "cpu_load": st_cpu_load.value, + "cpu_power_clock": st_cpu_power_clock.value, + + "gpu_temp": st_gpu_temp.value, + "gpu_load": st_gpu_load.value, + "gpu_core_load": st_gpu_core_load.value, + "gpu_memory": st_gpu_memory_block.value, + "gpu_power": st_gpu_power.value, + + "ram": st_ram.value, + "vram": st_vram.value, + "disk": st_disk.value, + "process": st_process.value, }, }, } - # publica no Redis ContextoGlobalRedis.atualizar_ctx_dict( ContextoGlobalRedis.ModKey(self.t_code), saude=payload ) - except Exception as e: print(f"[ModuloPC] Erro ao atualizar saude: {e}") - diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/shared/contexto_global_redis.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/shared/contexto_global_redis.py index cb03cc862..428f37dc9 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/shared/contexto_global_redis.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/shared/contexto_global_redis.py @@ -158,6 +158,11 @@ class ContextoGlobalRedis: def CamKey(cls, mx_id: str): """Lê a estrutura e decodifica de volta para objeto""" return CtxKey.DadosCameras.value + mx_id + + @classmethod + def CamImuKey(cls, mx_id: str): + """Lê a estrutura e decodifica de volta para objeto""" + return CtxKey.DadosCameras.value + mx_id + "_Imu" @classmethod def get_cameras(cls): @@ -166,6 +171,10 @@ class ContextoGlobalRedis: @classmethod def get_camera(cls, mx_id: str): return cls.get(cls.CamKey(mx_id), None) + + @classmethod + def get_camera_imu(cls, mx_id: str): + return cls.get(cls.CamImuKey(mx_id), None) @classmethod def get_equipamento(cls): diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/shared/gpu_priority_controller.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/shared/gpu_priority_controller.py new file mode 100644 index 000000000..154dd1cc1 --- /dev/null +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/shared/gpu_priority_controller.py @@ -0,0 +1,544 @@ +import time +from dataclasses import dataclass, field +from typing import Any, Dict, Optional + + +@dataclass +class GpuModeConfig: + mode: str + visual_fps: Dict[str, float] + + +class GpuPriorityController: + """ + Controlador adaptativo de prioridade de GPU. + + Primeira versão: + - Não usa lock real de GPU. + - Não centraliza inferência. + - Apenas reduz/aumenta FPS do visual_worker conforme saúde do weed_worker. + + O objetivo é proteger o weed_worker, mantendo o visual_worker vivo, + mas descartável quando o sistema estiver pesado. + """ + + MODES = ["critical", "safe", "eco", "normal"] + + def __init__( + self, + mostrar_log=None, + enabled: bool = True, + update_interval_s: float = 1.0, + log_interval_s: float = 3.0, + weed_targets: Optional[Dict[str, float]] = None, + visual_targets_normal: Optional[Dict[str, float]] = None, + mode_configs: Optional[Dict[str, Dict[str, float]]] = None, + bad_cycles_to_degrade: int = 3, + good_cycles_to_recover: int = 5, + min_data_age_s: float = 0.0, + max_data_age_s: float = 3.0, + log_periodic: bool = False, + log_warnings: bool = True, + ): + self.mostrar_log = mostrar_log + self.enabled = bool(enabled) + + self.update_interval_s = float(update_interval_s) + self.log_interval_s = float(log_interval_s) + + self.weed_targets = weed_targets or { + "tensor": 18.0, + "inferencia": 15.0, + "deteccao": 15.0, + } + + self.visual_targets_normal = visual_targets_normal or { + "segmentacao": 8.0, + "grid": 6.0, + "deteccao": 5.0, + "publicacao": 15.0, + "analise": 8.0, + } + + # Modos do visual. Ajustamos principalmente segmentação e grid. + self.mode_configs = mode_configs or { + "normal": { + "segmentacao": self.visual_targets_normal.get("segmentacao", 8.0), + "grid": self.visual_targets_normal.get("grid", 6.0), + "deteccao": self.visual_targets_normal.get("deteccao", 5.0), + "publicacao": self.visual_targets_normal.get("publicacao", 15.0), + "analise": self.visual_targets_normal.get("analise", 8.0), + }, + "eco": { + "segmentacao": 4.0, + "grid": 4.0, + "deteccao": 5.0, + "publicacao": 10.0, + "analise": 6.0, + }, + "safe": { + "segmentacao": 2.0, + "grid": 2.0, + "deteccao": 3.0, + "publicacao": 5.0, + "analise": 4.0, + }, + "critical": { + "segmentacao": 0.5, + "grid": 1.0, + "deteccao": 2.0, + "publicacao": 5.0, + "analise": 2.0, + }, + } + + self.bad_cycles_to_degrade = int(bad_cycles_to_degrade) + self.good_cycles_to_recover = int(good_cycles_to_recover) + + self.min_data_age_s = float(min_data_age_s) + self.max_data_age_s = float(max_data_age_s) + + self._mode = "normal" + self._desired_mode = "normal" + + self._bad_cycles = 0 + self._good_cycles = 0 + + self._last_update = 0.0 + self._last_log = 0.0 + + self._last_weed_perf = {} + self._last_health = { + "health": 1.0, + "reason": "init", + "fps": {}, + "ratios": {}, + "gpu_ms": 0.0, + "data_age_s": None, + "data_ok": False, + } + + self.log_periodic = bool(log_periodic) + self.log_warnings = bool(log_warnings) + self._last_warning_reason = None + + # ========================================================== + # API pública + # ========================================================== + + def update_from_redis(self): + """ + Lê Redis e atualiza modo. + Seguro para chamar várias vezes; respeita update_interval_s. + """ + now = time.time() + + if not self.enabled: + self._mode = "normal" + return self._last_health + + if now - self._last_update < self.update_interval_s: + return self._last_health + + self._last_update = now + + weed_ctx = self._read_weed_context() + health = self._evaluate_weed_health(weed_ctx) + + self._last_weed_perf = weed_ctx or {} + self._last_health = health + + desired = self._choose_desired_mode(health) + self._apply_hysteresis(desired) + + if self.log_periodic and (now - self._last_log >= self.log_interval_s): + self._last_log = now + self._log_status() + + if self.log_warnings: + self._log_warning_if_needed() + + return self._last_health + + def get_mode(self) -> str: + return self._mode + + def get_health(self) -> Dict[str, Any]: + return dict(self._last_health) + + def get_fps(self, task_name: str, fallback: Optional[float] = None) -> float: + """ + Retorna FPS atual para uma tarefa do visual_worker. + + task_name esperado: + - segmentacao + - grid + - deteccao + - publicacao + - analise + """ + if not self.enabled: + if fallback is not None: + return float(fallback) + return float(self.visual_targets_normal.get(task_name, 1.0)) + + cfg = self.mode_configs.get(self._mode, self.mode_configs["normal"]) + fps = cfg.get(task_name) + + if fps is None: + fps = fallback if fallback is not None else self.visual_targets_normal.get(task_name, 1.0) + + try: + return max(0.0, float(fps)) + except Exception: + return 1.0 + + def allow(self, task_name: str) -> bool: + """ + Por enquanto é simples: + - se FPS da tarefa <= 0, bloqueia. + - caso contrário, permite. + """ + return self.get_fps(task_name, fallback=1.0) > 0.01 + + def sleep_for_task(self, task_name: str, t0_wall: float, fallback_fps: float): + """ + Sleep adaptativo para usar no finally dos loops. + """ + fps = self.get_fps(task_name, fallback=fallback_fps) + + if fps <= 0.01: + time.sleep(0.25) + return + + elapsed = time.time() - t0_wall + period = 1.0 / max(fps, 0.01) + time.sleep(max(0.0, period - elapsed)) + + # ========================================================== + # Leitura Redis + # ========================================================== + + def _read_weed_context(self) -> Dict[str, Any]: + """ + Leitura robusta do contexto do weed_worker. + + Ajuste o nome da chave se no seu CtxKey estiver diferente. + """ + try: + from shared.contexto_global_redis import ContextoGlobalRedis, CtxKey + + # Tenta nomes prováveis sem quebrar se algum não existir. + possible_attrs = [ + "DadosWeedWorker", + "DadosErvasWorker", + "DadosWeed", + "WeedWorker", + ] + + for attr in possible_attrs: + if hasattr(CtxKey, attr): + key = getattr(CtxKey, attr) + ctx = ContextoGlobalRedis.get(key, {}) + if isinstance(ctx, dict) and ctx: + return ctx + + # Fallback opcional caso seu Redis aceite string. + try: + ctx = ContextoGlobalRedis.get("DadosWeedWorker", {}) + if isinstance(ctx, dict): + return ctx + except Exception: + pass + + except Exception as e: + self._log(f"[GPU_CTRL] erro lendo contexto weed: {e}") + + return {} + + # ========================================================== + # Avaliação de saúde + # ========================================================== + + def _evaluate_weed_health(self, ctx: Dict[str, Any]) -> Dict[str, Any]: + now = time.time() + + if not isinstance(ctx, dict) or not ctx: + return { + "health": 0.0, + "reason": "sem_ctx_weed", + "fps": {}, + "ratios": {}, + "gpu_ms": 0.0, + "data_age_s": None, + "data_ok": False, + } + + perf = self._extract_perf_root(ctx) + + ts = self._extract_timestamp(ctx, perf) + data_age_s = None + + if ts: + data_age_s = max(0.0, now - float(ts)) + + data_ok = True + if data_age_s is not None: + data_ok = self.min_data_age_s <= data_age_s <= self.max_data_age_s + + fps_tensor = self._extract_loop_fps(perf, ["tensor", "tensores"]) + fps_inf = self._extract_loop_fps(perf, ["inferencia", "inf", "infer"]) + fps_det = self._extract_loop_fps(perf, ["deteccao", "det", "detector"]) + + gpu_ms = self._extract_gpu_ms(perf) + + fps = { + "tensor": fps_tensor, + "inferencia": fps_inf, + "deteccao": fps_det, + } + + ratios = {} + for name, target in self.weed_targets.items(): + real = fps.get(name, 0.0) + target = max(float(target), 0.01) + ratios[name] = max(0.0, min(1.5, float(real) / target)) + + # O gargalo manda. Se um deles despencou, saúde despenca. + valid_ratios = [v for v in ratios.values() if v is not None] + health = min(valid_ratios) if valid_ratios else 0.0 + + reason = "ok" + + if not data_ok: + health = min(health, 0.40) + reason = "ctx_weed_desatualizado" + + if fps_inf <= 0.1: + health = 0.0 + reason = "weed_sem_inferencia" + + # Latência GPU como alarme extra. + # Não derruba direto para zero, mas limita a saúde. + if gpu_ms >= 120: + health = min(health, 0.35) + reason = "gpu_ms_muito_alto" + elif gpu_ms >= 95: + health = min(health, 0.60) + reason = "gpu_ms_alto" + elif gpu_ms >= 80: + health = min(health, 0.80) + reason = "gpu_ms_moderado" + + return { + "health": float(health), + "reason": reason, + "fps": fps, + "ratios": ratios, + "gpu_ms": float(gpu_ms or 0.0), + "data_age_s": data_age_s, + "data_ok": bool(data_ok), + } + + def _extract_perf_root(self, ctx: Dict[str, Any]) -> Dict[str, Any]: + """ + Aceita formatos variados: + - ctx["performance_weed"] + - ctx["performance"] + - ctx direto com loops + """ + for key in ["performance_weed", "performance", "perf", "performance_visual"]: + val = ctx.get(key) + if isinstance(val, dict): + return val + + return ctx + + def _extract_timestamp(self, ctx: Dict[str, Any], perf: Dict[str, Any]) -> Optional[float]: + for root in [perf, ctx]: + for key in ["ts", "timestamp", "ts_analise", "ultima_chamada"]: + val = root.get(key) + if isinstance(val, (int, float)) and val > 0: + return float(val) + return None + + def _extract_loop_fps(self, perf: Dict[str, Any], aliases) -> float: + loops = perf.get("loops", {}) + + if isinstance(loops, dict): + for alias in aliases: + loop = loops.get(alias) + if isinstance(loop, dict): + fps = loop.get("fps_real", loop.get("fps", None)) + if fps is not None: + try: + return float(fps) + except Exception: + pass + + # Fallbacks planos + for alias in aliases: + for key in [ + f"fps_{alias}", + f"{alias}_fps", + alias, + ]: + val = perf.get(key) + if isinstance(val, (int, float)): + return float(val) + + return 0.0 + + def _extract_gpu_ms(self, perf: Dict[str, Any]) -> float: + loops = perf.get("loops", {}) + + candidates = [] + + if isinstance(loops, dict): + for name in ["inferencia", "inf", "infer"]: + loop = loops.get(name) + if not isinstance(loop, dict): + continue + + metrics = loop.get("metrics_ms", {}) + if isinstance(metrics, dict): + for metric_name in ["gpu_ms", "infer_gpu_ms", "infer_ms"]: + metric = metrics.get(metric_name) + if isinstance(metric, dict): + val = metric.get("med", metric.get("avg", metric.get("last"))) + if isinstance(val, (int, float)): + candidates.append(float(val)) + elif isinstance(metric, (int, float)): + candidates.append(float(metric)) + + for key in ["gpu_ms", "infer_gpu_ms", "weed_gpu_ms"]: + val = perf.get(key) + if isinstance(val, (int, float)): + candidates.append(float(val)) + + return candidates[0] if candidates else 0.0 + + # ========================================================== + # Decisão de modo + # ========================================================== + + def _choose_desired_mode(self, health: Dict[str, Any]) -> str: + h = float(health.get("health", 0.0)) + + if h >= 0.90: + return "normal" + if h >= 0.75: + return "eco" + if h >= 0.45: + return "safe" + return "critical" + + def _mode_index(self, mode: str) -> int: + try: + return self.MODES.index(mode) + except ValueError: + return self.MODES.index("normal") + + def _apply_hysteresis(self, desired: str): + current_i = self._mode_index(self._mode) + desired_i = self._mode_index(desired) + + self._desired_mode = desired + + # Quanto maior o índice, mais leve/restritivo? Aqui: + # critical=0, safe=1, eco=2, normal=3. + if desired_i < current_i: + # Piorar modo. + self._bad_cycles += 1 + self._good_cycles = 0 + + if self._bad_cycles >= self.bad_cycles_to_degrade: + old = self._mode + self._mode = desired + self._bad_cycles = 0 + self._log(f"[GPU_CTRL] modo visual: {old} -> {self._mode}") + + elif desired_i > current_i: + # Melhorar modo. + self._good_cycles += 1 + self._bad_cycles = 0 + + if self._good_cycles >= self.good_cycles_to_recover: + old = self._mode + self._mode = desired + self._good_cycles = 0 + self._log(f"[GPU_CTRL] modo visual: {old} -> {self._mode}") + + else: + self._bad_cycles = 0 + self._good_cycles = 0 + + # ========================================================== + # Logs + # ========================================================== + + def _log_status(self): + h = self._last_health + fps = h.get("fps", {}) + ratios = h.get("ratios", {}) + + self._log( + "[GPU_CTRL] " + f"mode={self._mode} desired={self._desired_mode} " + f"health={h.get('health', 0.0):.2f} reason={h.get('reason')} " + f"weed_fps tensor={fps.get('tensor', 0.0):.1f} " + f"inf={fps.get('inferencia', 0.0):.1f} " + f"det={fps.get('deteccao', 0.0):.1f} " + f"ratios tensor={ratios.get('tensor', 0.0):.2f} " + f"inf={ratios.get('inferencia', 0.0):.2f} " + f"det={ratios.get('deteccao', 0.0):.2f} " + f"gpu={h.get('gpu_ms', 0.0):.1f}ms " + f"visual_fps seg={self.get_fps('segmentacao'):.1f} " + f"grid={self.get_fps('grid'):.1f} " + f"det={self.get_fps('deteccao'):.1f}" + ) + + def _log(self, msg: str): + if self.mostrar_log: + try: + self.mostrar_log(msg) + except Exception: + pass + + def _log_warning_if_needed(self): + h = self._last_health + reason = h.get("reason", "ok") + health = float(h.get("health", 1.0) or 0.0) + + # Só avisa quando é algo realmente relevante. + warning_reason = None + + if reason not in ["ok", "init"]: + warning_reason = reason + elif health < 0.45: + warning_reason = "weed_health_critical" + elif health < 0.75: + warning_reason = "weed_health_low" + + if warning_reason is None: + self._last_warning_reason = None + return + + # Evita repetir o mesmo aviso em todo ciclo. + if warning_reason == self._last_warning_reason: + return + + self._last_warning_reason = warning_reason + + fps = h.get("fps", {}) + self._log( + "[GPU_CTRL][WARN] " + f"{warning_reason} | " + f"mode={self._mode} desired={self._desired_mode} " + f"health={health:.2f} reason={reason} " + f"weed_fps tensor={fps.get('tensor', 0.0):.1f} " + f"inf={fps.get('inferencia', 0.0):.1f} " + f"det={fps.get('deteccao', 0.0):.1f} " + f"gpu={h.get('gpu_ms', 0.0):.1f}ms" + ) + \ No newline at end of file diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/shared/perf_monitor.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/shared/perf_monitor.py new file mode 100644 index 000000000..4299fc2ed --- /dev/null +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/shared/perf_monitor.py @@ -0,0 +1,159 @@ +import time +import threading +from collections import defaultdict, deque +import numpy as np + + +class VisualPerfMonitor: + def __init__(self, janela=120): + self.janela = janela + self.lock = threading.RLock() + self.eventos = defaultdict(lambda: deque(maxlen=janela)) + self.contadores = defaultdict(int) + self.ultimo_log = 0.0 + + def tick(self, nome, **dados): + agora = time.time() + item = { + "ts": agora, + **dados + } + + with self.lock: + self.eventos[nome].append(item) + self.contadores[f"{nome}_count"] += 1 + + def inc(self, nome, n=1): + with self.lock: + self.contadores[nome] += n + + def _stats_lista(self, valores): + valores = [v for v in valores if v is not None and np.isfinite(v)] + if not valores: + return { + "min": 0.0, + "med": 0.0, + "p95": 0.0, + "max": 0.0 + } + + arr = np.asarray(valores, dtype=np.float32) + return { + "min": float(np.min(arr)), + "med": float(np.mean(arr)), + "p95": float(np.percentile(arr, 95)), + "max": float(np.max(arr)) + } + + def resumo_loop(self, nome): + with self.lock: + evs = list(self.eventos.get(nome, [])) + + if len(evs) < 2: + return { + "fps_real": 0.0, + "periodo_ms": {}, + "latencia_ms": {}, + "idade_ms": {}, + "metrics_ms": {}, + "n": len(evs), + } + + tss = [e["ts"] for e in evs] + dts = np.diff(tss) + + fps_real = 1.0 / max(float(np.mean(dts)), 1e-6) + + latencias = [e.get("latencia_ms") for e in evs] + idades = [e.get("idade_frame_ms") for e in evs] + + metrics_ms = {} + + ignorar = { + "ts", + "frame_ts", + "depth_ts", + "seg_ts", + "det_ts", + "seq", + "seq_delta", + "frames_descartados", + "n_pkts_getall", + "valido", + "n_dets", + } + + for k in evs[-1].keys(): + if k in ignorar: + continue + + if not ( + k.endswith("_ms") + or k in ["latencia_ms", "idade_frame_ms"] + ): + continue + + vals = [] + for e in evs: + v = e.get(k) + if isinstance(v, (int, float)) and np.isfinite(v): + vals.append(float(v)) + + if vals: + metrics_ms[k] = self._stats_lista(vals) + + ultimo = evs[-1] if evs else {} + + return { + "fps_real": float(fps_real), + "periodo_ms": self._stats_lista([dt * 1000.0 for dt in dts]), + "latencia_ms": self._stats_lista(latencias), + "idade_ms": self._stats_lista(idades), + "metrics_ms": metrics_ms, + "last": ultimo, + "n": len(evs), + } + + def resumo(self): + nomes = [ + "camera_rgb", + "camera_depth", + "tensor", + "inferencia", + "segmentacao", + "deteccao", + "grid", + "stream", + "publicacao", + ] + + saida = { + "timestamp": time.time(), + "loops": {}, + "contadores": dict(self.contadores) + } + + for nome in nomes: + saida["loops"][nome] = self.resumo_loop(nome) + + # Sincronismo RGB/depth + with self.lock: + rgb = list(self.eventos.get("camera_rgb", [])) + depth = list(self.eventos.get("camera_depth", [])) + + if rgb and depth: + rgb_ts = rgb[-1].get("frame_ts_host", rgb[-1].get("ts")) + depth_ts = depth[-1].get("frame_ts_host", depth[-1].get("ts")) + saida["sync"] = { + "rgb_depth_dt_ms": float(abs(rgb_ts - depth_ts) * 1000.0), + "rgb_age_ms": float((time.time() - rgb_ts) * 1000.0), + "depth_age_ms": float((time.time() - depth_ts) * 1000.0), + } + else: + saida["sync"] = { + "rgb_depth_dt_ms": None, + "rgb_age_ms": None, + "depth_age_ms": None, + } + + return saida \ No newline at end of file diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/camera_manager.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/camera_manager.py index c72c5963b..54a26b2d6 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/camera_manager.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/camera_manager.py @@ -14,14 +14,30 @@ from shared.enums import StatusModulo, T_Code, TipoFrameCamera from shared.utils import converter_mask_ids_para_bgr, get_velocidade_atual_ms from camera_worker.camera_oak import CameraOak from shared.contexto_global_redis import ContextoGlobalRedis, CtxKey +from shared.perf_monitor import VisualPerfMonitor +from shared.gpu_priority_controller import GpuPriorityController class CameraManager: def __init__(self, mostrar_log): self.mostrar_log = mostrar_log self.mx_id = None + self.camera = None + self._loop_analise_iniciado = False + self._loop_stream_iniciado = False + self._loop_segmentacao_iniciado = False + self._loop_deteccao_iniciado = False + self._loop_grid_iniciado = False + self._loop_publicacao_iniciado = False + self._vida_lock = threading.RLock() + self._fechando_camera = False self.reiniciar_status() def reiniciar_status(self): + if self.camera is not None: + try: + self.camera.parar() + except Exception as e: + self.mostrar_log(f"Erro ao parar camera: {e}") self.camera = None self.operante = False self.iniciando = False @@ -45,6 +61,54 @@ class CameraManager: self._ts_deteccao_anterior = 0 self._pool = ThreadPoolExecutor(max_workers=6) + self._startup_grace_until = 0.0 + + self._em_warmup = False + self._falhas_utilizavel = 0 + self._fechando_camera = False + self._cache_lock = threading.RLock() + self._seg_cache = { + "ts": 0.0, + "frame_ts": 0.0, + "predictions": None, + "aux_result": None, + "res": None, + } + + self._det_cache = { + "ts": 0.0, + "detections": [], + "res": None, + } + + self._grid_cache = { + "ts": 0.0, + "snapshot": None, + "grid_conf": None, + } + + self._pub_lock = threading.RLock() + self._pub_cache = { + "ts_analise": 0.0, + "segmentacao": None, + "deteccao": None, + "matriz_confianca": None, + "performance_visual": None, + + "ts_segmentacao": 0.0, + "ts_deteccao": 0.0, + "ts_matriz_confianca": 0.0, + "ts_performance_visual": 0.0, + + "dirty_segmentacao": False, + "dirty_deteccao": False, + "dirty_matriz_confianca": False, + "dirty_performance_visual": False, + } + + self.perf = VisualPerfMonitor(janela=180) + self.gpu_controller = None + def inicializar(self, mx_id): if self.iniciando: return @@ -65,7 +129,7 @@ class CameraManager: det_config = load_det_config() try: - nova = CameraOak(self.mostrar_log, mx_id, modelo_ia_seg=None, modelo_ia_det=det_config, iniciar_imu=True) + nova = CameraOak(self.mostrar_log, mx_id, modelo_ia_seg=None, modelo_ia_det=det_config, iniciar_imu=True, perf_monitor=self.perf) if nova.iniciado: self.camera = nova except Exception as e: @@ -78,7 +142,6 @@ class CameraManager: else: self.mostrar_log(f"📷 Camera selecionada: {self.camera.modelo} - {self.camera.mx_id}") self.largura_robo_m = ContextoGlobalRedis.get(CtxKey.DadosEquipamento, {}).get("largura", 0.85) - self.operante = True self._timestamp_analise = None self.grid_ref_shape = (15, 10) @@ -86,8 +149,14 @@ class CameraManager: self.grid_ref = self.grid_ref_base.copy() self.data_fuser = CostmapFuser(grid_shape=self.grid_ref_shape, K=3, M=2, central_cols=None, y_range_m=(0.5,5.0), near_is_bottom=True, fov_h_rad=np.radians(self.camera.parametros["fov_h"]), robot_width=self.largura_robo_m) - self.seg_runner = SegformerNavRunner(seg_config) - self.segmentacao_manager = SegmentacaoManager(color_map=self.seg_runner.colormap_rgb, classes=self.seg_runner.classes) + if not hasattr(self, "seg_runner") or self.seg_runner is None: + self.seg_runner = SegformerNavRunner(seg_config) + + if not hasattr(self, "segmentacao_manager") or self.segmentacao_manager is None: + self.segmentacao_manager = SegmentacaoManager( + color_map=self.seg_runner.colormap_rgb, + classes=self.seg_runner.classes + ) self._ultima_analise_segmentacao = {} self._ultima_analise_deteccao = {} @@ -115,11 +184,484 @@ class CameraManager: self._analisando_segmentacao = False self._analisando_deteccao = False - self._iniciar_loop_analise_continua(15.0) - self._iniciar_loop_frame_stream(self.camera.stream._op_fps) + # Mantém os loops bloqueados durante o warmup. + self.operante = False + self._camera_pronta_em = time.time() + 999.0 + + warmup_ok = self._executar_warmup_camera_manager( + n_frames_camera=8, + n_seg=4, + n_det=4, + n_grid=3, + timeout_s=6.0 + ) + + if not warmup_ok: + self.mostrar_log("[WARMUP] finalizou com alerta, liberando operação mesmo assim") + + # Dá uma janela para saúde/cache estabilizarem. + self._startup_grace_until = time.time() + 5.0 + + # Libera operação. + self._camera_pronta_em = time.time() + 0.2 + self.operante = True + + cam_atual = self.camera + if cam_atual is None: + self.mostrar_log("[INIT] camera ficou None antes de iniciar loops; abortando init") + self.iniciando = False + return + + self.debug_perf = bool(seg_config.get("debug_perf", False)) + freq_analise = float(seg_config.get("analise_fps", 20.0)) + freq_grid = float(seg_config.get("grid_fps", 20.0)) + freq_publicacao = float(seg_config.get("publicacao_fps", 15.0)) + freq_inferencia = float(seg_config.get("inferencia_fps", freq_analise)) + freq_deteccao = float(seg_config.get("deteccao_fps", freq_analise)) + + try: + from weed_worker.config import load_seg_config as load_weed_config + weed_config = load_weed_config() or {} + except Exception as e: + self.mostrar_log(f"[GPU_CTRL] erro ao carregar config do weed_worker: {e}") + weed_config = {} + gpu_ctrl_cfg = seg_config.get("gpu_priority", {}) or {} + self.gpu_controller = GpuPriorityController( + mostrar_log=self.mostrar_log, + enabled=bool(gpu_ctrl_cfg.get("enabled", True)), + update_interval_s=float(gpu_ctrl_cfg.get("update_interval_s", 1.0)), + log_interval_s=float(gpu_ctrl_cfg.get("log_interval_s", 3.0)), + log_periodic=bool(gpu_ctrl_cfg.get("log_periodic", False)), + log_warnings=bool(gpu_ctrl_cfg.get("log_warnings", True)), + weed_targets = { + "tensor": float(weed_config.get("tensor_fps", 18.0)), + "inferencia": float(weed_config.get("inferencia_fps", 15.0)), + "deteccao": float(weed_config.get("deteccao_fps", 15.0)), + }, + visual_targets_normal={ + "segmentacao": freq_inferencia, + "grid": freq_grid, + "deteccao": freq_deteccao, + "publicacao": freq_publicacao, + "analise": freq_analise, + }, + mode_configs=gpu_ctrl_cfg.get("mode_configs", None), + bad_cycles_to_degrade=int(gpu_ctrl_cfg.get("bad_cycles_to_degrade", 3)), + good_cycles_to_recover=int(gpu_ctrl_cfg.get("good_cycles_to_recover", 5)), + max_data_age_s=float(gpu_ctrl_cfg.get("max_data_age_s", 3.0)), + ) + + if not self._loop_stream_iniciado: + cam_atual = self.camera + if cam_atual is None: + self.mostrar_log("[INIT] não iniciou stream: camera None") + else: + stream = getattr(cam_atual, "stream", None) + stream_fps = getattr(stream, "_op_fps", 2.0) + self._iniciar_loop_frame_stream(stream_fps) + self._loop_stream_iniciado = True + + if not self._loop_publicacao_iniciado: + self._iniciar_loop_publicacao_visual(freq=freq_publicacao) + self._loop_publicacao_iniciado = True + + if not self._loop_segmentacao_iniciado: + self._iniciar_loop_segmentacao(freq=freq_inferencia) + self._loop_segmentacao_iniciado = True + + if not self._loop_deteccao_iniciado: + self._iniciar_loop_deteccao(freq=freq_deteccao) + self._loop_deteccao_iniciado = True + + if not self._loop_grid_iniciado: + self._iniciar_loop_matriz_confianca(freq=freq_grid) + self._loop_grid_iniciado = True + + if not self._loop_analise_iniciado: + self._iniciar_loop_analise_continua(15.0) + self._loop_analise_iniciado = True + self.iniciando = False self.atualizar_saude_camera() + with self._vida_lock: + self._fechando_camera = False + + def fechar_camera_manager(self, motivo=""): + with self._vida_lock: + if self._fechando_camera: + return + + self._fechando_camera = True + cam = self.camera + self.camera = None + self.operante = False + self.iniciando = False + + try: + if cam is not None: + cam.parar() + except Exception as e: + self.mostrar_log(f"Erro ao fechar camera: {e}") + + with self._cache_lock: + self._ultimo_rgb_frame = None + self._ultimo_depth_frame = None + self._ultimo_heatmap_frame = None + self._ultimo_predictions = None + self._ultimo_detections = None + self._ultimo_snapshot = None + + self._seg_cache = { + "ts": 0.0, + "frame_ts": 0.0, + "predictions": None, + "aux_result": None, + "res": None, + } + + self._det_cache = { + "ts": 0.0, + "detections": [], + "res": None, + } + + self._grid_cache = { + "ts": 0.0, + "snapshot": None, + "grid_conf": None, + } + + self.mostrar_log(f"Camera Manager fechado: {motivo}") + + def _executar_warmup_camera_manager( + self, + n_frames_camera=8, + n_seg=4, + n_det=4, + n_grid=3, + timeout_s=6.0 + ): + """ + Aquece CameraOak, detector, SegFormer, pós-segmentação, grid e fuser + antes de liberar os loops principais. + + Objetivo: + - evitar primeiro infer_ms gigante; + - garantir cache RGB/depth preenchido; + - garantir detector respondendo; + - preencher cache inicial de segmentação e grid; + - deixar o VisualWorker entrar operante já com dados úteis. + """ + t_all0 = time.perf_counter() + + if self.camera is None: + return False + + self.mostrar_log("[WARMUP] iniciando warmup do CameraManager...") + + # Durante o warmup, não queremos os loops principais rodando. + operante_anterior = self.operante + warmup_anterior = getattr(self, "_em_warmup", False) + + self.operante = False + self._em_warmup = True + + rgb_frame = None + rgb_ts = None + depth_frame = None + depth_ts = None + dets = [] + + try: + # ===================================================== + # 1) Esperar cache RGB/depth ficar vivo + # ===================================================== + t0 = time.time() + rgb_ok = False + depth_ok = False + + while time.time() - t0 < timeout_s: + try: + rgb_frame, rgb_ts, _ = self.get_rgb_frame() + depth_frame, depth_ts, _ = self.get_depth_frame() + + rgb_ok = rgb_frame is not None and rgb_ts is not None + depth_ok = depth_frame is not None and depth_ts is not None + + if rgb_ok and depth_ok: + break + + except Exception: + pass + + time.sleep(0.03) + + self.mostrar_log( + f"[WARMUP] cache camera rgb_ok={rgb_ok} depth_ok={depth_ok} " + f"rgb_ts={rgb_ts} depth_ts={depth_ts}" + ) + + if not rgb_ok: + self.mostrar_log("[WARMUP] abortado: sem RGB válido") + self.operante = operante_anterior + return False + + # Faz algumas leituras extras para estabilizar o cache. + for _ in range(max(0, n_frames_camera)): + rgb_frame, rgb_ts, _ = self.get_rgb_frame() + depth_frame, depth_ts, _ = self.get_depth_frame() + time.sleep(0.02) + + # ===================================================== + # 2) Warmup detector onboard + # ===================================================== + det_ok_count = 0 + for _ in range(max(0, n_det)): + try: + dets_tmp, det_ts, det_res = self.get_detections() + + if dets_tmp is not None and det_ts is not None: + dets = dets_tmp + det_ok_count += 1 + + with self._cache_lock: + self._ultimo_detections = dets + self._ultima_analise_deteccao = { + "bboxes": dets, + "ultima_chamada": time.time() + } + self._det_cache = { + "ts": time.time(), + "detections": dets, + "res": det_res, + } + self._nova_deteccao_disponivel = True + + except Exception as e: + self.mostrar_log(f"[WARMUP] detector falhou: {e}") + + time.sleep(0.03) + + self.mostrar_log(f"[WARMUP] detector ok_count={det_ok_count}/{n_det}") + + # ===================================================== + # 3) Warmup SegFormer + SegmentacaoManager + # ===================================================== + seg_ok_count = 0 + analise_segmentacao = None + predictions = None + aux_result = None + seg_res = None + frame_ts = rgb_ts + + for i in range(max(0, n_seg)): + try: + # Pega frame mais recente antes de cada inferência. + rgb_frame, frame_ts, seg_res = self.get_rgb_frame() + + if rgb_frame is None: + continue + + t_inf0 = time.perf_counter() + predictions, ts, roi_resized, roi_info, aux_result = self.seg_runner.infer_ids(rgb_frame) + t_inf1 = time.perf_counter() + + if predictions is None or ts is None: + continue + + t_post0 = time.perf_counter() + analise_segmentacao, log = self.segmentacao_manager.segmentar( + predictions, + aux_result=aux_result + ) + t_post1 = time.perf_counter() + + if analise_segmentacao is None: + if log: + self.mostrar_log(f"[WARMUP] segmentacao log: {log}") + continue + + if "classes" not in analise_segmentacao: + analise_segmentacao["classes"] = predictions + + seg_ok_count += 1 + + self.mostrar_log( + f"[WARMUP] seg {i+1}/{n_seg} " + f"infer={(t_inf1-t_inf0)*1000:.1f}ms " + f"post={(t_post1-t_post0)*1000:.1f}ms" + ) + + except Exception as e: + self.mostrar_log(f"[WARMUP] segmentacao falhou: {e}") + + time.sleep(0.03) + + if analise_segmentacao is not None and predictions is not None: + with self._cache_lock: + self._ultimo_predictions = predictions + self._ultimo_seg_aux_result = aux_result + self._ultima_analise_segmentacao = analise_segmentacao + self._seg_cache = { + "ts": time.time(), + "frame_ts": frame_ts or 0.0, + "predictions": predictions, + "aux_result": aux_result, + "res": seg_res, + } + self._nova_segmentacao_disponivel = True + + self.mostrar_log(f"[WARMUP] segmentacao ok_count={seg_ok_count}/{n_seg}") + + # Sem segmentação, não tem como aquecer grid corretamente. + if analise_segmentacao is None: + self.mostrar_log("[WARMUP] sem segmentação válida para aquecer grid") + self.operante = operante_anterior + return False + + # ===================================================== + # 4) Warmup grid + fuser + # ===================================================== + grid_ok_count = 0 + + det_params = { + "w4": 0.18, + "thr_det_soft": 0.45, + "min_cell_coverage": 0.10, + "min_det_conf": 0.45, + "class_weights": { + "person": 1.0, + "dog": 0.7, + "cat": 0.5, + }, + "veto_labels": {"person"}, + "combine": "max", + "conf_drop_alpha": 0.0, + "only_veto_blocks_nav": True, + "non_veto_cost_scale": 0.50, + } + + for i in range(max(0, n_grid)): + try: + depth_frame, depth_ts, _ = self.get_depth_frame() + + if depth_frame is None: + continue + + try: + imu_ctx = ContextoGlobalRedis.get_modulo(T_Code.Imu) or {} + imu_roll = imu_ctx.get("roll_seg", 0) + except Exception: + imu_roll = 0 + + self.grid_ref = self._ajustar_grid_ref_por_pitch( + pitch_graus=imu_roll + ) + + t_grid0 = time.perf_counter() + grid_conf = self._construir_grid_confianca( + depth_frame, + analise_segmentacao["classes"], + self.grid_ref, + self.grid_ref_shape, + deteccoes=dets, + det_params=det_params + ) + t_grid1 = time.perf_counter() + + if grid_conf is None: + continue + + t_fuser0 = time.perf_counter() + snapshot = self.data_fuser.update( + grid_conf, + velocidade_ms=0.0, + status_seg=analise_segmentacao.get("dados_visuais", {}).get("status_corredor") + ) + t_fuser1 = time.perf_counter() + + with self._cache_lock: + self._ultimo_snapshot = snapshot + self._ultima_analise_matriz_confianca = grid_conf + self._grid_cache = { + "ts": time.time(), + "snapshot": snapshot, + "grid_conf": grid_conf, + } + self._nova_grid_conf_disponivel = True + + grid_ok_count += 1 + + self.mostrar_log( + f"[WARMUP] grid {i+1}/{n_grid} " + f"build={(t_grid1-t_grid0)*1000:.1f}ms " + f"fuser={(t_fuser1-t_fuser0)*1000:.1f}ms" + ) + + except Exception as e: + self.mostrar_log(f"[WARMUP] grid falhou: {e}") + + time.sleep(0.03) + + self.mostrar_log(f"[WARMUP] grid ok_count={grid_ok_count}/{n_grid}") + + # ===================================================== + # 5) Publicação inicial opcional no Redis + # ===================================================== + try: + if analise_segmentacao is not None: + self._set_pub_cache("segmentacao", analise_segmentacao["dados_visuais"]) + + if self._ultimo_snapshot is not None: + self._set_pub_cache("matriz_confianca", self._ultimo_snapshot) + + if dets is not None: + self._set_pub_cache("deteccao", dets) + + except Exception as e: + self.mostrar_log(f"[WARMUP] publicação inicial Redis falhou: {e}") + + dt_all = (time.perf_counter() - t_all0) * 1000.0 + self.mostrar_log( + f"[WARMUP] finalizado em {dt_all:.1f}ms | " + f"seg_ok={seg_ok_count}/{n_seg} " + f"grid_ok={grid_ok_count}/{n_grid} " + f"det_ok={det_ok_count}/{n_det}" + ) + + self.operante = operante_anterior + return True + + except Exception as e: + self.mostrar_log(f"[WARMUP] erro geral: {e}") + self.operante = operante_anterior + return False + + finally: + self.operante = operante_anterior + self._em_warmup = warmup_anterior + + self.perf = VisualPerfMonitor(janela=180) + if self.camera is not None: + self.camera.perf = self.perf + + def _is_erro_fatal_camera(self, erro): + if erro is None: + return False + + txt = str(erro) + + sinais = [ + "X_LINK_ERROR", + "Communication exception", + "Couldn't read data from stream", + "Device already closed", + "device has been closed", + ] + + return any(s in txt for s in sinais) + def gerar_grid_ref(self, n_frames=50): try: grid = self._gerar_grid_referencia_geometrico() @@ -227,87 +769,180 @@ class CameraManager: except Exception as e: self.mostrar_log(f"[saude] erro: {e}") + + def _camera_indisponivel_temporaria(self, cam): + if cam is None: + return True + + if not hasattr(cam, "esta_utilizavel"): + return False + + if cam.esta_utilizavel(): + self._falhas_utilizavel = 0 + return False + + if time.time() < getattr(self, "_camera_pronta_em", 0): + return True + + self._falhas_utilizavel = getattr(self, "_falhas_utilizavel", 0) + 1 + + if self._falhas_utilizavel >= 10: + self.fechar_camera_manager("camera não utilizável") + + return True + + def _camera_indisponivel_rapida(self, cam): + if cam is None: + return True + + if not self.operante and not getattr(self, "_em_warmup", False): + return True + + if getattr(cam, "device", None) is None: + return True + + return False + + def get_rgb_frame(self): - if self.camera is None: + cam = self.camera + + if self._camera_indisponivel_rapida(cam): return None, None, None try: - frame, res = self.camera.requisitar_frame_rgb() - if frame is not None: - self._ultimo_rgb_frame = frame - return frame, self.camera.timestamp_ultimo_frame_rgb, res - elif "X_LINK_ERROR" in res["erro"]: - self.reiniciar_status() + frame, res = cam.requisitar_frame_rgb() + erro = (res or {}).get("erro") + + if self._is_erro_fatal_camera(erro): + self.fechar_camera_manager(f"falha fatal DepthAI: {erro}") + return None, None, res + + ts = getattr(cam, "timestamp_ultimo_frame_rgb", None) + + if frame is not None and ts is not None: + with self._cache_lock: + self._ultimo_rgb_frame = frame + return frame, ts, res + + return None, ts, res + except Exception as e: self.mostrar_log(f"Erro ao requisitar rgb frame: {e}") - if "X_LINK_ERROR" in str(e): - self.reiniciar_status() + + if self._is_erro_fatal_camera(e): + self.fechar_camera_manager(f"falha fatal DepthAI: {e}") return None, None, None def get_depth_frame(self): - if self.camera is None or not self.camera.tem_depth: + cam = self.camera + + if cam is None or not getattr(cam, "tem_depth", False): + return None, None, None + + if self._camera_indisponivel_rapida(cam): return None, None, None try: - frame, res = self.camera.requisitar_frame_depth() - if frame is not None: - self._ultimo_depth_frame = frame - return frame, self.camera.timestamp_ultimo_frame_depth, res - elif "X_LINK_ERROR" in res["erro"]: - self.reiniciar_status() + frame, res = cam.requisitar_frame_depth() + erro = (res or {}).get("erro") + + if self._is_erro_fatal_camera(erro): + self.fechar_camera_manager(f"falha fatal DepthAI: {erro}") + return None, None, res + + ts = getattr(cam, "timestamp_ultimo_frame_depth", None) + + if frame is not None and ts is not None: + with self._cache_lock: + self._ultimo_depth_frame = frame + return frame, ts, res + + return None, ts, res + except Exception as e: self.mostrar_log(f"Erro ao requisitar depth frame: {e}") - if "X_LINK_ERROR" in str(e): - self.reiniciar_status() + + if self._is_erro_fatal_camera(e): + self.fechar_camera_manager(f"falha fatal DepthAI: {e}") return None, None, None def get_heatmap_frame(self): + cam = self.camera frame, timestamp, res = self.get_depth_frame() - if frame is not None: - heatmap = gerar_heatmap(frame, self.camera.parametros["distancia_maxima"]) + + if frame is not None and cam is not None: + heatmap = gerar_heatmap(frame, cam.parametros["distancia_maxima"]) self._ultimo_heatmap_frame = heatmap return heatmap, timestamp, res + return None, None, None def get_segmentation_predictions(self): if self.camera is None: - return None, None, None + return None, None, None, None try: - #predictions, res = self.camera.requisitar_segmentacao() - #if predictions is not None: - # return predictions, self.camera.timestamp_ultima_segmentacao, res + t_rgb0 = time.perf_counter() rgb_frame, _ts, res = self.get_rgb_frame() - predictions, ts, roi_resized, (y_fim, y_inicio) = self.seg_runner.infer_ids(rgb_frame) + t_rgb1 = time.perf_counter() + + t_inf0 = time.perf_counter() + predictions, ts, roi_resized, roi_info, aux_result = self.seg_runner.infer_ids(rgb_frame) + t_inf1 = time.perf_counter() + + #print( + # f"GET_SEG | rgb={(t_rgb1-t_rgb0)*1000:.1f}ms | " + # f"infer={(t_inf1-t_inf0)*1000:.1f}ms | " + # f"total={(t_inf1-t_rgb0)*1000:.1f}ms" + #) + self._ultimo_predictions = predictions + self._ultimo_seg_aux_result = aux_result + if predictions is not None: - return predictions, ts, res - elif "X_LINK_ERROR" in res["erro"]: + return predictions, ts, res, aux_result + + elif "X_LINK_ERROR" in res.get("erro", ""): self.reiniciar_status() + except Exception as e: self.mostrar_log(f"Erro ao requisitar predictions: {e}") if "X_LINK_ERROR" in str(e): self.reiniciar_status() - return None, None, None + return None, None, None, None def get_detections(self): - if self.camera is None: + cam = self.camera + + if self._camera_indisponivel_rapida(cam): return None, None, None try: - detections, res = self.camera.requisitar_deteccao() - self._ultimo_detections = detections - if detections is not None: - return detections, self.camera.timestamp_ultima_deteccao, res - elif "X_LINK_ERROR" in res["erro"]: - self.reiniciar_status() + detections, res = cam.requisitar_deteccao() + erro = (res or {}).get("erro") + + if self._is_erro_fatal_camera(erro): + self.fechar_camera_manager(f"falha fatal DepthAI: {erro}") + return None, None, res + + ts = getattr(cam, "timestamp_ultima_deteccao", None) + + if detections is not None and ts is not None: + with self._cache_lock: + self._ultimo_detections = detections + return detections, ts, res + + return detections, ts, res + except Exception as e: self.mostrar_log(f"Erro ao requisitar detections: {e}") - if "X_LINK_ERROR" in str(e): - self.reiniciar_status() + + if self._is_erro_fatal_camera(e): + self.fechar_camera_manager(f"falha fatal DepthAI: {e}") return None, None, None @@ -335,49 +970,136 @@ class CameraManager: def _iniciar_loop_analise_continua(self, freq): def loop(): - ultima_atualizacao = 0 while True: - if self.camera is None: - time.sleep(5) - continue - t0 = time.time() + try: - status = StatusModulo((self.camera.ultima_saude or {}).get("status", StatusModulo.DESCONECTADO.value)) - ts_status = (self.camera.ultima_saude or {}).get("timestamp", 0) - if status == StatusModulo.DESCONECTADO and (t0 - ts_status) > 5.0: - if self.camera.imu: - self.camera.imu.parar() - self.reiniciar_status() + if self.camera is None: + time.sleep(0.5) continue - elif self.operante and status == StatusModulo.OPERANTE and self.camera is not None: - self._realizar_analises() + + status = StatusModulo( + (self.camera.ultima_saude or {}).get( + "status", + StatusModulo.DESCONECTADO.value + ) + ) + + ts_status = (self.camera.ultima_saude or {}).get("timestamp", 0) + + if self.iniciando or t0 < getattr(self, "_startup_grace_until", 0.0): + time.sleep(0.1) + continue + + if status == StatusModulo.DESCONECTADO and ts_status and (t0 - ts_status) > 10.0: + self.fechar_camera_manager("status desconectado") + continue + + agora = time.time() + + if not hasattr(self, "_ultimo_perf_publish"): + self._ultimo_perf_publish = 0.0 + + if agora - self._ultimo_perf_publish >= 1.0: + self._ultimo_perf_publish = agora + + ctrl = getattr(self, "gpu_controller", None) + if ctrl is not None: + ctrl.update_from_redis() + + resumo = self.perf.resumo() + + if self.camera is not None and hasattr(self.camera, "get_cache_stats"): + resumo["camera_cache"] = self.camera.get_cache_stats() + + self._set_pub_cache("performance_visual", resumo) + + if not self.debug_perf: + continue + + loops = resumo.get("loops", {}) + sync = resumo.get("sync", {}) + + def _fmt(v, casas=1, default=0.0): + try: + if v is None: + v = default + return f"{float(v):.{casas}f}" + except Exception: + return f"{default:.{casas}f}" + + def _m(loop, nome, stat="med", default=0.0): + try: + return ( + loop.get("metrics_ms", {}) + .get(nome, {}) + .get(stat, default) + ) + except Exception: + return default + + def _lat(loop, stat="med", default=0.0): + try: + return loop.get("latencia_ms", {}).get(stat, default) + except Exception: + return default + + def _per(loop, stat="med", default=0.0): + try: + return loop.get("periodo_ms", {}).get(stat, default) + except Exception: + return default + + def _fps(loop): + try: + return float(loop.get("fps_real", 0.0) or 0.0) + except Exception: + return 0.0 + + cam_rgb = loops.get("camera_rgb", {}) + cam_depth = loops.get("camera_depth", {}) + seg = loops.get("segmentacao", {}) + det = loops.get("deteccao", {}) + grid = loops.get("grid", {}) + pub = loops.get("publicacao", {}) + + self.mostrar_log( + "[PERF_VISUAL] " + f"fps rgb={_fps(cam_rgb):.1f} depth={_fps(cam_depth):.1f} " + f"seg={_fps(seg):.1f} det={_fps(det):.1f} grid={_fps(grid):.1f} | " + f"period seg={_fmt(_per(seg))}ms grid={_fmt(_per(grid))}ms | " + f"sync={_fmt(sync.get('rgb_depth_dt_ms'))}ms" + ) + + self.mostrar_log( + "[PERF_DETAIL] " + f"SEG total={_fmt(_lat(seg))} get={_fmt(_m(seg,'get_rgb_ms'))} " + f"infer={_fmt(_m(seg,'infer_ms'))} post={_fmt(_m(seg,'post_ms'))} " + f"redis={_fmt(_m(seg,'redis_ms'))} | " + f"GRID total={_fmt(_lat(grid))} depth={_fmt(_m(grid,'get_depth_ms'))} " + f"build={_fmt(_m(grid,'build_grid_ms'))} fuser={_fmt(_m(grid,'fuser_ms'))} " + f"redis={_fmt(_m(grid,'redis_ms'))} | " + f"DET total={_fmt(_lat(det))} read={_fmt(_m(det,'oak_read_ms'))} " + f"redis={_fmt(_m(det,'redis_ms'))} | " + f"PUB total={_fmt(_lat(pub))} redis={_fmt(_m(pub,'redis_ms'))}" + ) + except Exception as e: - self.mostrar_log(f"Erro no loop de analise continua: {e}") + self.mostrar_log(f"Erro no loop supervisor visual: {e}") + finally: - latencia, fps, _freq = self._calcular_performance(t0, time.time(), {"ultima_chamada": ultima_atualizacao}) - novo_delay = max(0, (1.0 / freq) - latencia) - #self.mostrar_log( - # self._log_performance("Loop", { "freq": _freq, "fps": fps, "latencia": latencia }) + - # #self._log_performance("Radar", self._ultima_analise_radar) + - # self._log_performance("Segmentacao", self._ultima_analise_segmentacao) + - # self._log_performance("Matriz Confianca", self._ultima_analise_matriz_confianca) + - # #self._log_performance("Anomalias", self._ultima_analise_anomalias) + - # #self._log_performance("Solo", self._ultima_analise_solo) + - # #self._log_performance("Matriz Custo", self._ultima_analise_matriz_custo) + - # "" - #) - ultima_atualizacao = t0 - time.sleep(novo_delay) - + self._sleep_loop_adaptativo("analise", t0, freq) + threading.Thread(target=loop, daemon=True).start() def _iniciar_loop_frame_stream(self, freq): def loop(): while True: - if self.camera is None: - time.sleep(5) + cam = self.camera + if cam is None: + time.sleep(0.5) continue + t0 = time.time() try: _camera = (ContextoGlobalRedis.get_camera(self.mx_id) or {}) @@ -400,10 +1122,483 @@ class CameraManager: self.mostrar_log(f"Erro no loop de stream: {e}") finally: latencia = time.time() - t0 - freq = self.camera.stream._op_fps if self.camera is not None else 1.0 + cam = self.camera + stream = getattr(cam, "stream", None) if cam is not None else None + freq_loop = getattr(stream, "_op_fps", freq if freq else 1.0) + time.sleep(max(0, (1.0 / freq_loop) - latencia)) time.sleep(max(0, (1.0 / freq) - latencia)) threading.Thread(target=loop, daemon=True).start() + def _iniciar_loop_publicacao_visual(self, freq=15.0): + def loop(): + ultimo_payload_forcado = 0.0 + + while True: + t0 = time.time() + t_loop0 = time.perf_counter() + + try: + if self.camera is None: + time.sleep(0.5) + continue + + if not self.operante: + time.sleep(0.2) + continue + + # Publica tudo pelo menos 1 vez por segundo, + # mesmo que alguma chave não tenha mudado. + agora = time.time() + publicar_tudo = (agora - ultimo_payload_forcado) >= 1.0 + + payload = self._montar_payload_publicacao_visual( + publicar_tudo=publicar_tudo + ) + + # Se só tem ts e timestamps auxiliares, não precisa mandar sempre. + tem_dado_real = any( + k in payload + for k in [ + "segmentacao", + "deteccao", + "matriz_confianca", + "performance_visual", + ] + ) + + if tem_dado_real: + t_redis0 = time.perf_counter() + + ContextoGlobalRedis.atualizar_ctx_dict( + CtxKey.DadosVisualWorker, + **converter_valores_numpy(payload) + ) + + t_redis1 = time.perf_counter() + + if publicar_tudo: + ultimo_payload_forcado = agora + + self.perf.tick( + "publicacao", + latencia_ms=(time.perf_counter() - t_loop0) * 1000.0, + redis_ms=(t_redis1 - t_redis0) * 1000.0, + campos=len(payload), + publicou=1, + ) + else: + self.perf.tick( + "publicacao", + latencia_ms=(time.perf_counter() - t_loop0) * 1000.0, + redis_ms=0.0, + campos=0, + publicou=0, + ) + + except Exception as e: + self.mostrar_log(f"❌ Erro no loop_publicacao_visual: {e}") + + finally: + self._sleep_loop_adaptativo("publicacao", t0, freq) + + threading.Thread(target=loop, daemon=True).start() + + def _iniciar_loop_segmentacao(self, freq=10.0): + def loop(): + ultimo_frame_ts = 0.0 + + while True: + t0 = time.time() + if self.iniciando or t0 < getattr(self, "_camera_pronta_em", 0): + time.sleep(0.05) + continue + + try: + if not self.operante or self.camera is None: + time.sleep(0.2) + continue + + ctrl = getattr(self, "gpu_controller", None) + if ctrl is not None and not ctrl.allow("segmentacao"): + self.perf.inc("segmentacao_bloqueada_gpu_ctrl") + time.sleep(0.05) + continue + + t_loop0 = time.perf_counter() + t0_wall = time.time() + + t_get0 = time.perf_counter() + rgb_frame, frame_ts, res = self.get_rgb_frame() + t_get1 = time.perf_counter() + + if rgb_frame is None or frame_ts is None: + self.perf.inc("segmentacao_sem_rgb") + time.sleep(0.02) + continue + + if frame_ts == ultimo_frame_ts: + self.perf.inc("segmentacao_frame_repetido") + time.sleep(0.005) + continue + + ultimo_frame_ts = frame_ts + + t_inf0 = time.perf_counter() + predictions, ts, roi_resized, roi_info, aux_result = self.seg_runner.infer_ids(rgb_frame) + t_inf1 = time.perf_counter() + + if predictions is None or ts is None: + self.perf.inc("segmentacao_pred_none") + continue + + t_post0 = time.perf_counter() + analise_segmentacao, log = self.segmentacao_manager.segmentar( + predictions, + aux_result=aux_result + ) + t_post1 = time.perf_counter() + + if analise_segmentacao is None: + self.perf.inc("segmentacao_analise_none") + if log: + self.mostrar_log(log) + continue + + t_pub0 = time.perf_counter() + self._set_pub_cache("segmentacao", analise_segmentacao["dados_visuais"]) + t_pub1 = time.perf_counter() + + t_loop1 = time.perf_counter() + + self.perf.tick( + "segmentacao", + latencia_ms=(t_loop1 - t_loop0) * 1000.0, + get_rgb_ms=(t_get1 - t_get0) * 1000.0, + infer_ms=(t_inf1 - t_inf0) * 1000.0, + post_ms=(t_post1 - t_post0) * 1000.0, + pub_cache_ms=(t_pub1 - t_pub0) * 1000.0, + redis_ms=0.0, + frame_ts=frame_ts, + idade_frame_ms=(time.time() - frame_ts) * 1000.0, + ) + + with self._cache_lock: + self._ultimo_predictions = predictions + self._ultimo_seg_aux_result = aux_result + self._ultima_analise_segmentacao = analise_segmentacao + self._seg_cache = { + "ts": time.time(), + "frame_ts": frame_ts, + "predictions": predictions, + "aux_result": aux_result, + "res": res, + } + self._nova_segmentacao_disponivel = True + + except Exception as e: + self.mostrar_log(f"❌ Erro no loop_segmentacao: {e}") + + finally: + self._sleep_loop_adaptativo("segmentacao", t0, freq) + + threading.Thread(target=loop, daemon=True).start() + + def _iniciar_loop_deteccao(self, freq=15.0): + def loop(): + ultimo_ts = 0.0 + + while True: + t0 = time.time() + + if self.iniciando or t0 < getattr(self, "_camera_pronta_em", 0): + time.sleep(0.05) + continue + + try: + if not self.operante or self.camera is None: + time.sleep(0.2) + continue + + t_loop0 = time.perf_counter() + + t_det0 = time.perf_counter() + dets, ts, meta = self.get_detections() + t_det1 = time.perf_counter() + + if ts is None: + self.perf.inc("deteccao_sem_ts") + time.sleep(0.02) + continue + + if ts == ultimo_ts: + self.perf.inc("deteccao_repetida") + time.sleep(0.005) + continue + + ultimo_ts = ts + + if dets is None: + self.perf.inc("deteccao_none") + continue + + t_pub0 = time.perf_counter() + + with self._cache_lock: + self._ultimo_detections = dets + self._ultima_analise_deteccao = { + "bboxes": dets, + "ultima_chamada": t0 + } + self._det_cache = { + "ts": time.time(), + "detections": dets, + "res": meta, + } + self._nova_deteccao_disponivel = True + + self._set_pub_cache("deteccao", dets) + + t_pub1 = time.perf_counter() + t_loop1 = time.perf_counter() + + self.perf.tick( + "deteccao", + latencia_ms=(t_loop1 - t_loop0) * 1000.0, + oak_read_ms=(t_det1 - t_det0) * 1000.0, + pub_cache_ms=(t_pub1 - t_pub0) * 1000.0, + redis_ms=0.0, + frame_ts=ts, + idade_frame_ms=(time.time() - ts) * 1000.0, + n_dets=len(dets), + ) + + except Exception as e: + self.mostrar_log(f"❌ Erro no loop_deteccao: {e}") + + finally: + self._sleep_loop_adaptativo("deteccao", t0, freq) + + threading.Thread(target=loop, daemon=True).start() + + def _iniciar_loop_matriz_confianca(self, freq=15.0): + def loop(): + ultimo_depth_ts = 0.0 + ultimo_seg_ts = 0.0 + ultimo_det_ts = 0.0 + + while True: + t_wall0 = time.time() + t_loop0 = time.perf_counter() + + if self.iniciando or t_wall0 < getattr(self, "_camera_pronta_em", 0): + time.sleep(0.05) + continue + + try: + if not self.operante or self.camera is None: + time.sleep(0.2) + continue + + ctrl = getattr(self, "gpu_controller", None) + if ctrl is not None and not ctrl.allow("grid"): + self.perf.inc("grid_bloqueada_gpu_ctrl") + time.sleep(0.05) + continue + + # ===================================================== + # 1) Ler depth do cache da CameraOak + # ===================================================== + t_depth0 = time.perf_counter() + depth_frame_np, depth_ts, depth_res = self.get_depth_frame() + t_depth1 = time.perf_counter() + + if depth_frame_np is None or depth_ts is None: + self.perf.inc("grid_sem_depth") + time.sleep(0.02) + continue + + # ===================================================== + # 2) Ler segmentação/detecção do cache do CameraManager + # ===================================================== + t_cache0 = time.perf_counter() + with self._cache_lock: + analise_seg = self._ultima_analise_segmentacao + segmentacao = analise_seg.get("classes") if analise_seg else None + + dados_visuais_seg = ( + analise_seg.get("dados_visuais", {}) + if analise_seg else {} + ) + + deteccoes = list(self._det_cache.get("detections", [])) + seg_ts = float(self._seg_cache.get("ts", 0.0) or 0.0) + det_ts = float(self._det_cache.get("ts", 0.0) or 0.0) + t_cache1 = time.perf_counter() + + if segmentacao is None: + self.perf.inc("grid_sem_segmentacao") + time.sleep(0.02) + continue + + # ===================================================== + # 3) Evitar recalcular o mesmo pacote + # ===================================================== + mesmo_depth = depth_ts == ultimo_depth_ts + mesma_seg = seg_ts == ultimo_seg_ts + mesma_det = det_ts == ultimo_det_ts + + # A grid depende principalmente de depth + segmentação. + # Detecção sozinha pode mudar custo, então se det mudou, + # vale recalcular também. + if mesmo_depth and mesma_seg and mesma_det: + self.perf.inc("grid_pacote_repetido") + time.sleep(0.005) + continue + + ultimo_depth_ts = depth_ts + ultimo_seg_ts = seg_ts + ultimo_det_ts = det_ts + + # ===================================================== + # 4) Atualizar referência geométrica pelo IMU + # ===================================================== + t_ref0 = time.perf_counter() + try: + imu_ctx = ContextoGlobalRedis.get_modulo(T_Code.Imu) or {} + imu_roll = imu_ctx.get("roll_seg", 0) + except Exception: + imu_roll = 0 + + self.grid_ref = self._ajustar_grid_ref_por_pitch( + pitch_graus=imu_roll + ) + t_ref1 = time.perf_counter() + + # ===================================================== + # 5) Montar parâmetros + # ===================================================== + det_params = { + "w4": 0.18, + "thr_det_soft": 0.45, + "min_cell_coverage": 0.10, + "min_det_conf": 0.45, + "class_weights": { + "person": 1.0, + "dog": 0.7, + "cat": 0.5, + }, + "veto_labels": {"person"}, + "combine": "max", + "conf_drop_alpha": 0.0, + "only_veto_blocks_nav": True, + "non_veto_cost_scale": 0.50, + } + + try: + vel = get_velocidade_atual_ms() + except Exception: + vel = 0.0 + + # ===================================================== + # 6) Construir grid de confiança/custo + # ===================================================== + t_grid0 = time.perf_counter() + grid_conf = self._construir_grid_confianca( + depth_frame_np, + segmentacao, + self.grid_ref, + self.grid_ref_shape, + deteccoes=deteccoes, + det_params=det_params + ) + t_grid1 = time.perf_counter() + + if grid_conf is None: + self.perf.inc("grid_conf_none") + time.sleep(0.005) + continue + + # Compatibilidade com performance antiga + t_grid_wall = time.time() + grid_conf["ultima_chamada"] = ( + self._ultima_analise_matriz_confianca + .get("ultima_chamada", t_wall0) + ) + self._calcular_performance(t_wall0, t_grid_wall, grid_conf) + + # ===================================================== + # 7) Fuser + # ===================================================== + t_fuser0 = time.perf_counter() + snapshot = self.data_fuser.update( + grid_conf, + velocidade_ms=vel, + status_seg=dados_visuais_seg.get("status_corredor") + ) + t_fuser1 = time.perf_counter() + + # ===================================================== + # 8) Atualizar caches internos + # ===================================================== + t_store0 = time.perf_counter() + with self._cache_lock: + self._ultimo_snapshot = snapshot + self._ultima_analise_matriz_confianca = grid_conf + self._grid_cache = { + "ts": time.time(), + "snapshot": snapshot, + "grid_conf": grid_conf, + } + self._nova_grid_conf_disponivel = True + t_store1 = time.perf_counter() + + # ===================================================== + # 9) Publicar Redis + # ===================================================== + t_pub0 = time.perf_counter() + self._set_pub_cache("matriz_confianca", snapshot) + t_pub1 = time.perf_counter() + + # ===================================================== + # 10) Registrar performance + # ===================================================== + t_loop1 = time.perf_counter() + agora = time.time() + + self.perf.tick( + "grid", + latencia_ms=(t_loop1 - t_loop0) * 1000.0, + get_depth_ms=(t_depth1 - t_depth0) * 1000.0, + cache_read_ms=(t_cache1 - t_cache0) * 1000.0, + grid_ref_ms=(t_ref1 - t_ref0) * 1000.0, + build_grid_ms=(t_grid1 - t_grid0) * 1000.0, + fuser_ms=(t_fuser1 - t_fuser0) * 1000.0, + store_ms=(t_store1 - t_store0) * 1000.0, + pub_cache_ms=(t_pub1 - t_pub0) * 1000.0, + redis_ms=0.0, + + depth_ts=depth_ts, + seg_ts=seg_ts, + det_ts=det_ts, + + idade_depth_ms=(agora - depth_ts) * 1000.0 if depth_ts else None, + idade_seg_ms=(agora - seg_ts) * 1000.0 if seg_ts else None, + idade_det_ms=(agora - det_ts) * 1000.0 if det_ts else None, + + sync_depth_seg_ms=abs(depth_ts - seg_ts) * 1000.0 if depth_ts and seg_ts else None, + sync_depth_det_ms=abs(depth_ts - det_ts) * 1000.0 if depth_ts and det_ts else None, + + n_dets=len(deteccoes), + ) + + except Exception as e: + self.mostrar_log(f"❌ Erro no loop_matriz_confianca: {e}") + + finally: + self._sleep_loop_adaptativo("grid", t_wall0, freq) + + threading.Thread(target=loop, daemon=True).start() + def _calcular_performance(self, t0, t1, analise): latencia = t1 - t0 @@ -418,88 +1613,6 @@ class CameraManager: def _log_performance(self, titulo, analise): return f"{titulo}: {analise.get('latencia', 0):.3f} s, {analise.get('fps', 0):.2f} FPS, {analise.get('freq', 0):.2f} Hz; " - - def _realizar_analises(self): - self._analise_segmentacao() - - self._analise_deteccao() - - if self._depth_frame_necessario: - depth_frame_np, depth_timestamp, depth_res = self.get_depth_frame() - else: - depth_frame_np = self._ultimo_depth_frame - - imu_roll = ContextoGlobalRedis.get_modulo(T_Code.Imu).get("roll_seg", 0) - self.grid_ref = self._ajustar_grid_ref_por_pitch(pitch_graus=imu_roll) - self._analise_matriz_confianca(depth_frame_np) - - - def _analise_segmentacao(self): - if self._analisando_segmentacao: return - self._analisando_segmentacao = True - try: - t0 = time.time() - predictions, ts, res = self.get_segmentation_predictions() - if ts == self._ts_segmentacao_anterior: return - dt = max(1e-6, ts - self._ts_segmentacao_anterior) if self._ts_segmentacao_anterior else 0.0 - fps = (1.0 / dt) if dt > 0 else 0.0 - self._ts_segmentacao_anterior = ts - if predictions is not None: - analise_segmentacao, log = self.segmentacao_manager.segmentar(predictions) - t1 = time.time() - if analise_segmentacao == None: self.mostrar_log(log) - analise_segmentacao["ultima_chamada"] = self._ultima_analise_segmentacao.get("ultima_chamada", t0) - self._calcular_performance(t0, t1, analise_segmentacao) - self._ultima_analise_segmentacao = analise_segmentacao - ContextoGlobalRedis.atualizar_ctx_dict( - CtxKey.DadosVisualWorker, - ts_analise=t1, - segmentacao=converter_valores_numpy(self._ultima_analise_segmentacao["dados_visuais"]) - ) - self._nova_segmentacao_disponivel = True - - # Enviar comando para atualizar os dados de controle sempre que um novo dado de segmentacao seja processado e a operacao seja do tipo MapeamentoVisual - #op_modo = ContextoGlobalRedis.get_operacao().get("modo", ModoOperacao.NaoDefinido.value) - #movimento_automatico = ContextoGlobalRedis.get_controle().get("movimento_automatico", False) - #if ModoOperacao(op_modo) == ModoOperacao.MapeamentoVisual and movimento_automatico and analise_segmentacao is not None: - # ContextoGlobalRedis.publicar_comando(CmdKey.ManagerWorkerRx, { "cmd": ManagerWorkerCommandType.AtualizarDadosControle.value }) - - except Exception as e: - self.mostrar_log(f"❌ Erro na segmentacao semantica: {e}") - finally: - self._analisando_segmentacao = False - #self.mostrar_log(f"Segmentacao concluida em {self._ultima_analise_segmentacao['latencia']:.4f} s, a {fps:.4f} FPS") - - def _analise_deteccao(self): - if self._analisando_deteccao or self.camera.modelo_ia_det is None: return - self._analisando_deteccao = True - try: - t0 = time.time() - dets, ts, meta = self.get_detections() - if ts == self._ts_deteccao_anterior: return - dt = max(1e-6, ts - self._ts_deteccao_anterior) if self._ts_deteccao_anterior else 0.0 - fps = (1.0 / dt) if dt > 0 else 0.0 - self._ts_deteccao_anterior = ts - if dets is not None: - t1 = time.time() - analise_deteccoes = { - "bboxes": dets - } - self._calcular_performance(t0, t1, analise_deteccoes) - self._ultima_analise_deteccao = analise_deteccoes - ContextoGlobalRedis.atualizar_ctx_dict( - CtxKey.DadosVisualWorker, - ts_analise=t1, - deteccao=converter_valores_numpy(dets) - ) - self._nova_deteccao_disponivel = True - - except Exception as e: - self.mostrar_log(f"❌ Erro na deteccao de objetos: {e}") - finally: - self._analisando_deteccao = False - #self.mostrar_log(f"Deteccao concluida em {self._ultima_analise_deteccao['latencia']:.4f} s, a {fps:.4f} FPS") - def _overlay_deteccoes( self, rgb_frame, @@ -661,58 +1774,6 @@ class CameraManager: except Exception as e: self.mostrar_log(f"Erro ao gerar overlay de deteccoes") - def _analise_matriz_confianca(self, depth_frame_np): - if self._analisando_matriz_confianca: return - self._analisando_matriz_confianca = True - try: - if depth_frame_np is None or depth_frame_np.size == 0: return - segmentacao = self._ultima_analise_segmentacao.get("classes") - if segmentacao is None: return - deteccoes = (self._ultima_analise_deteccao or {}).get("bboxes", []) - det_params = { - "w4": 0.18, - "thr_det_soft": 0.45, - "min_cell_coverage": 0.10, - "min_det_conf": 0.45, - "class_weights": { - "person": 1.0, - "dog": 0.7, - "cat": 0.5, - }, - "veto_labels": {"person"}, - "combine": "max", - "conf_drop_alpha": 0.0, - "only_veto_blocks_nav": True, - "non_veto_cost_scale": 0.50, - } - - vel = get_velocidade_atual_ms() - - t0 = time.time() - grid_conf = self._construir_grid_confianca(depth_frame_np, segmentacao, self.grid_ref, self.grid_ref_shape, deteccoes=deteccoes, det_params=det_params) - #self.mostrar_log(grid_conf) - if grid_conf is None: - return - t1 = time.time() - grid_conf["ultima_chamada"] = self._ultima_analise_matriz_confianca.get("ultima_chamada", t0) - self._calcular_performance(t0, t1, grid_conf) - snapshot = self.data_fuser.update(grid_conf, velocidade_ms=vel, status_seg=self._ultima_analise_segmentacao.get("dados_visuais", {}).get("status_corredor")) - self._ultimo_snapshot = snapshot - self._ultima_analise_matriz_confianca = grid_conf - - ContextoGlobalRedis.atualizar_ctx_dict( - CtxKey.DadosVisualWorker, - ts_analise=time.time(), - matriz_confianca=snapshot - ) - self._nova_grid_conf_disponivel = True - except Exception as e: - self.mostrar_log(f"❌ Erro na geracao da matriz de confianca: {e}") - finally: - self._depth_frame_necessario = True - self._analisando_matriz_confianca = False - #self.mostrar_log("Matriz de confianca concluida") - def _construir_grid_confianca( self, depth_mm, # np.ndarray (H,W) em mm ou None @@ -1338,3 +2399,101 @@ class CameraManager: except Exception as e: self.mostrar_log(f"❌ Erro ao salvar frames: {e}") return [] + + + def _set_pub_cache(self, chave, valor): + """ + Atualiza cache de publicação sem escrever no Redis. + chave: segmentacao, deteccao, matriz_confianca, performance_visual + """ + try: + agora = time.time() + + with self._pub_lock: + self._pub_cache[chave] = valor + self._pub_cache[f"ts_{chave}"] = agora + self._pub_cache[f"dirty_{chave}"] = True + self._pub_cache["ts_analise"] = agora + + except Exception as e: + self.mostrar_log(f"Erro ao atualizar pub_cache[{chave}]: {e}") + + def _montar_payload_publicacao_visual(self, publicar_tudo=False): + """ + Monta um payload único para o Redis. + Se publicar_tudo=False, manda apenas o que mudou. + """ + with self._pub_lock: + cache = dict(self._pub_cache) + + dirty_segmentacao = cache.get("dirty_segmentacao", False) + dirty_deteccao = cache.get("dirty_deteccao", False) + dirty_matriz = cache.get("dirty_matriz_confianca", False) + dirty_perf = cache.get("dirty_performance_visual", False) + + payload = { + "ts_analise": time.time(), + } + + if publicar_tudo or dirty_segmentacao: + if cache.get("segmentacao") is not None: + payload["segmentacao"] = cache["segmentacao"] + + if publicar_tudo or dirty_deteccao: + if cache.get("deteccao") is not None: + payload["deteccao"] = cache["deteccao"] + + if publicar_tudo or dirty_matriz: + if cache.get("matriz_confianca") is not None: + payload["matriz_confianca"] = cache["matriz_confianca"] + + if publicar_tudo or dirty_perf: + if cache.get("performance_visual") is not None: + payload["performance_visual"] = cache["performance_visual"] + + # timestamps auxiliares, leves e úteis + payload["ts_segmentacao"] = cache.get("ts_segmentacao", 0.0) + payload["ts_deteccao"] = cache.get("ts_deteccao", 0.0) + payload["ts_matriz_confianca"] = cache.get("ts_matriz_confianca", 0.0) + payload["ts_performance"] = cache.get("ts_performance_visual", 0.0) + + # limpa dirty somente das chaves que entraram no payload + if "segmentacao" in payload: + self._pub_cache["dirty_segmentacao"] = False + + if "deteccao" in payload: + self._pub_cache["dirty_deteccao"] = False + + if "matriz_confianca" in payload: + self._pub_cache["dirty_matriz_confianca"] = False + + if "performance_visual" in payload: + self._pub_cache["dirty_performance_visual"] = False + + return payload + + + def _sleep_loop_adaptativo(self, nome_tarefa, t0_wall, freq_fallback): + """ + Sleep baseado no GpuPriorityController. + + nome_tarefa: + - segmentacao + - grid + - deteccao + - publicacao + - analise + """ + try: + ctrl = getattr(self, "gpu_controller", None) + + if ctrl is not None: + ctrl.sleep_for_task(nome_tarefa, t0_wall, freq_fallback) + return + + except Exception as e: + self.mostrar_log(f"[GPU_CTRL] erro no sleep adaptativo {nome_tarefa}: {e}") + + latencia = time.time() - t0_wall + time.sleep(max(0.0, (1.0 / max(float(freq_fallback), 0.1)) - latencia)) + diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/config.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/config.py index 8e26513bc..312b2c52a 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/config.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/config.py @@ -40,50 +40,80 @@ _CONFIG_LOCK = threading.Lock() def load_seg_config(force_reload=False): global _CONFIG_CACHE, _CONFIG_MTIME with _CONFIG_LOCK: - #try: - # mtime = os.path.getmtime(_CONFIG_PATH) - # if force_reload or _CONFIG_CACHE is None or mtime != _CONFIG_MTIME: - # with open(_CONFIG_PATH, "r", encoding="utf-8") as f: - # _CONFIG_CACHE = json.load(f) - # _CONFIG_MTIME = mtime - #except Exception as e: - # mostrar_log(f"Erro ao ler config: {e}") - # if _CONFIG_CACHE is None: - # # Valores default se der ruim no primeiro load - # _CONFIG_CACHE = { - # "debug_visual": True, - # "frames_consecutivos": 3, - # "frames_histerese": 2, - # "min_area_px": 400, - # "max_area_frac": 0.2, - # "area_atuacao_bicos": 0.1, - # "ia_roi_begin": 0.0, - # "ia_roi_size": 1.0, - # "ia_resolution": [512,288], - # "erva_top_band_frac": 0.30, - # "erva_frac_ema": 0.3, - # "erva_thresh_vel_gain": 0.4, - # "min_frac_erva_global_on": 0.0020, - # "min_frac_erva_global_off": 0.0015, - # "min_frac_erva_top_on": 0.0015, - # "min_frac_erva_top_off": 0.0010, - # "min_frac_erva_por_bico": 0.02, - # "usar_morfologia": True, - # "kernel_morf": 3 - # } _CONFIG_CACHE = { "debug_visual": False, + "debug_perf": False, "ia_roi_begin": 0.0, "ia_roi_size": 1.0, + + "analise_fps": 8.0, + "grid_fps": 5.0, + "inferencia_fps": 8.0, + "deteccao_fps": 5.0, + "publicacao_fps": 15.0, + "gpu_priority": { + "enabled": True, + "update_interval_s": 1.0, + "log_interval_s": 3.0, + "log_periodic": False, + "log_warnings": True, + + "bad_cycles_to_degrade": 3, + "good_cycles_to_recover": 5, + "max_data_age_s": 3.0, + + "mode_configs": { + "normal": { + "segmentacao": 8.0, + "grid": 6.0, + "deteccao": 5.0, + "publicacao": 15.0, + "analise": 8.0 + }, + "eco": { + "segmentacao": 4.0, + "grid": 4.0, + "deteccao": 5.0, + "publicacao": 10.0, + "analise": 6.0 + }, + "safe": { + "segmentacao": 2.0, + "grid": 2.0, + "deteccao": 3.0, + "publicacao": 5.0, + "analise": 4.0 + }, + "critical": { + "segmentacao": 0.5, + "grid": 1.0, + "deteccao": 2.0, + "publicacao": 5.0, + "analise": 2.0 + } + } + }, + "ia_resolution": [1024,576], "seg_every_n": 1, - "det_every_n": 3, + "det_every_n": 1, + "use_amp": True, + "use_channels_last": True, + "use_compact_aux": True, + "debug_timing": False, + "runtime_fast": True, + "gerar_mask_color": False, + "gerar_debug_status": False, + "usar_connected_components": True, + "usar_corridor_grid": True } - _CONFIG_CACHE["ia_model_path"] = ContextoGlobalRedis.get_equipamento().get("path_ia_model_ruas_seg") - _CONFIG_CACHE["ia_labelmap_path"] = ContextoGlobalRedis.get_equipamento().get("path_ia_labelmap_ruas_seg") - _CONFIG_CACHE["ia_norm_stats_path"] = ContextoGlobalRedis.get_equipamento().get("path_ia_norm_stats_ruas_seg") - _CONFIG_CACHE["ia_backbone"] = ContextoGlobalRedis.get_equipamento().get("ia_backbone_ruas_seg") - + _equipamento = ContextoGlobalRedis.get_equipamento() + _CONFIG_CACHE["ia_mode"] = _equipamento.get("ia_mode_ruas") + _CONFIG_CACHE["ia_backbone"] = _equipamento.get("ia_backbone_ruas_seg") + _CONFIG_CACHE["ia_model_path"] = _equipamento.get("path_ia_model_ruas_seg") + _CONFIG_CACHE["ia_labelmap_path"] = _equipamento.get("path_ia_labelmap_ruas_seg") + _CONFIG_CACHE["ia_norm_stats_path"] = _equipamento.get("path_ia_norm_stats_ruas_seg") + return _CONFIG_CACHE def reload_seg_config(): @@ -97,7 +127,7 @@ def load_det_config(): "ia_roi_size": 1.0, "ia_resolution": [300,300], "seg_every_n": 1, - "det_every_n": 3, + "det_every_n": 1, "ia_conf": 0.5, "classes": [ "background", diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/processamento/segmentacao_semantica.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/processamento/segmentacao_semantica.py index 2bf10343f..39d90c376 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/processamento/segmentacao_semantica.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/processamento/segmentacao_semantica.py @@ -16,6 +16,12 @@ class SegmentacaoManager: def __init__(self, color_map, classes): from visual_worker.config import load_seg_config config = load_seg_config() + self.runtime_fast = bool(config.get("runtime_fast", True)) + self.gerar_mask_color = bool(config.get("gerar_mask_color", False)) + self.gerar_debug_status = bool(config.get("gerar_debug_status", False)) + self.usar_connected_components = bool(config.get("usar_connected_components", True)) + self.usar_corridor_grid = bool(config.get("usar_corridor_grid", True)) + resolucao = config.get("ia_resolution") self.color_map = color_map self.classes = classes @@ -49,40 +55,65 @@ class SegmentacaoManager: self._status_hist = deque(maxlen=self.max_len) self._status_final_hist = deque(maxlen=2) + self.debug_timing = bool(config.get("debug_timing_segmentacao", False)) - def segmentar(self, predictions): + + def segmentar(self, predictions, aux_result=None): try: - # 🔸 Constrói a máscara colorida e outras saídas com base na predictions já pronta - resultado = self._segmentar_predictions(predictions) + t0 = time.perf_counter() + + resultado = self._segmentar_predictions( + predictions, + gerar_mask_color=self.gerar_mask_color + ) + t_mask = time.perf_counter() + if resultado is None: print("[Erro] Segmentação vazia ou falhou") - return None + return None, self.log if predictions is None: print("[Erro] Máscara de classes não encontrada no resultado") - return None - - self.dados_visuais = self._analisar_corredor_visual(predictions) + return None, self.log + + self.dados_visuais = self._analisar_corredor_visual( + predictions, + aux_result=aux_result + ) + t_ana = time.perf_counter() + resultado["dados_visuais"] = self.dados_visuais + if getattr(self, "debug_timing", False): + print( + f"SEGMENTACAO_MANAGER | " + f"mask={(t_mask-t0)*1000:.1f}ms | " + f"analise={(t_ana-t_mask)*1000:.1f}ms | " + f"total={(t_ana-t0)*1000:.1f}ms" + ) + return resultado, self.log except Exception as e: self.log = f"❌ Erro na segmentação: {e}" return None, self.log - def _segmentar_predictions(self, predictions): + def _segmentar_predictions(self, predictions, gerar_mask_color=None): try: - self.pred_rgb[:] = self.lut[predictions] + if gerar_mask_color is None: + gerar_mask_color = self.gerar_mask_color - mask_color = self.pred_rgb - frame_color = encode_image_base64(mask_color) + mask_color = None + + if gerar_mask_color: + self.pred_rgb[:] = self.lut[predictions] + mask_color = self.pred_rgb return { "timestamp": time.time(), "frame": { "timestamp": time.time(), - "frame": frame_color + "frame": None }, "mask_color": mask_color, "classes": predictions @@ -92,12 +123,15 @@ class SegmentacaoManager: print(f"Erro ao processar predictions: {e}") return None - def _analisar_corredor_visual(self, predictions, grid_rows_y_px=None, near_is_bottom=True): + def _analisar_corredor_visual(self, predictions, aux_result=None, grid_rows_y_px=None, near_is_bottom=True): # --- inputs/base --- self.predictions = predictions H, W = self.predictions.shape cx_img = W // 2 + status_modelo_now, status_final_fast, status_before_fast, debug_fast = self._resolver_status_modelo_rapido(aux_result) + status_fast_ok = status_final_fast is not None + mask_corredor = self._extrair_corredor_principal(self.predictions).astype(bool) # --- score de corredor por grid (robusto) --- @@ -143,24 +177,9 @@ class SegmentacaoManager: if len(pts) >= 2: ys_fit = np.array([p[1] for p in pts], dtype=np.float32) xs_fit = np.array([p[0] for p in pts], dtype=np.float32) - n = len(ys_fit) - - # pesos: linhas "mais perto" pesam mais - if near_is_bottom: - wts = np.linspace(2.0, 1.0, n, dtype=np.float32) - else: - wts = np.linspace(1.0, 2.0, n, dtype=np.float32) - - Wm = np.diag(wts) - Y = ys_fit.reshape(-1, 1) - X = np.hstack([Y, np.ones_like(Y)]) - XtW = X.T @ Wm - beta = np.linalg.pinv(XtW @ X) @ (XtW @ xs_fit) - a = float(beta[0]) - - ang_rad = np.arctan(a) - # unwrap simples - ang_rad = (ang_rad + np.pi) % (2*np.pi) - np.pi + a = self._fit_linha_ponderada(ys_fit, xs_fit, near_is_bottom=near_is_bottom) + if a is not None: + ang_rad = np.arctan(a) # EMA em graus deg = float(np.degrees(ang_rad)) if getattr(self, "_ema_ang", None) is None: @@ -189,8 +208,16 @@ class SegmentacaoManager: self._ema_lat = (1 - self._ema_alpha_lat) * self._ema_lat + self._ema_alpha_lat * float(erro_lateral_pct) lat_out = round(float(self._ema_lat), 3) - mask_nav = (self.predictions == ClassesSegmentacao.NAVEGAVEL.value) - status_now, status_final, status_before, debug = self.classificar_status_corredor(mask_nav) + if status_fast_ok: + status_final = status_final_fast + status_before = status_before_fast + debug = debug_fast + else: + mask_nav = (self.predictions == ClassesSegmentacao.NAVEGAVEL.value) + status_now, status_final, status_before, debug = self.resolver_status_corredor( + mask_nav=mask_nav, + aux_result=aux_result + ) return { "timestamp": time.time(), @@ -199,12 +226,82 @@ class SegmentacaoManager: "erro_angular": ang_out, "erro_lateral_pct": lat_out, "status_corredor": status_final.value, + "status_corredor_nome": status_final.name, "status_corredor_anterior": status_before.value, + "status_corredor_anterior_nome": status_before.name, "status_corredor_debug": debug, "centros_corredor": centros, "larguras_px": larguras, "confianca": round(float(score), 3), } + + def resolver_status_corredor(self, mask_nav: np.ndarray, aux_result=None): + if aux_result and aux_result.get("type") == "label": + conf = float(aux_result.get("label_conf", 0.0)) + label_id = int(aux_result.get("label_id", StatusCarroMapa.Indefinido.value)) + + if conf >= 0.70: + status_now = StatusCarroMapa(label_id) + debug = { + "origem_status": "modelo", + "status_modelo": label_id, + "status_modelo_conf": conf, + "heuristica_executada": False, + } + + self._status_hist.append((status_now, self._now())) + status_final = self._maioria_ultimos() + self._status_final_hist.append(status_final) + status_before = self._status_final_hist[0] if len(self._status_final_hist) > 1 else status_final + + return status_now, status_final, status_before, debug + + status_heur, _, _, debug_heur = self.classificar_status_corredor(mask_nav) + + status_modelo = None + label_conf = 0.0 + label_probs = None + label_name = None + + if aux_result and aux_result.get("type") == "label": + try: + label_id = int(aux_result.get("label_id")) + label_conf = float(aux_result.get("label_conf", 0.0)) + label_probs = aux_result.get("label_probs") + label_name = aux_result.get("label_name") + status_modelo = StatusCarroMapa(label_id) + except Exception: + status_modelo = None + + if status_modelo is not None and label_conf >= 0.70: + status_now = status_modelo + origem = "modelo" + elif status_modelo is not None and label_conf >= 0.45: + status_now = status_modelo + origem = "modelo_baixa_conf" + else: + status_now = status_heur + origem = "heuristica" + + self._status_hist.append((status_now, self._now())) + status_final = self._maioria_ultimos() + + self._status_final_hist.append(status_final) + status_before = self._status_final_hist[0] if len(self._status_final_hist) > 1 else status_final + + debug = { + "origem_status": origem, + "status_modelo": status_modelo.value if status_modelo is not None else None, + "status_modelo_nome": status_modelo.name if status_modelo is not None else None, + "status_modelo_label_name": label_name, + "status_modelo_conf": round(label_conf, 4), + "status_modelo_probs": label_probs, + "status_heuristico": status_heur.value, + "status_heuristico_nome": status_heur.name, + "heuristica_debug": debug_heur, + } + + return status_now, status_final, status_before, debug def _extrair_corredor_principal(self, mask_nav): H, W = mask_nav.shape @@ -267,15 +364,22 @@ class SegmentacaoManager: row_h = H // rows col_w = W // cols - # fração de NAO NAVEGAVEL por célula + mask_nao = (mask_classes == ClassesSegmentacao.NAONAVEGAVEL.value).astype(np.uint8) + ii = cv2.integral(mask_nao) # shape (H+1, W+1) + frac = np.zeros((rows, cols), np.float32) + for i in range(rows): - y0, y1 = i*row_h, H if i==rows-1 else (i+1)*row_h + y0 = i * row_h + y1 = H if i == rows - 1 else (i + 1) * row_h + for j in range(cols): - x0, x1 = j*col_w, W if j==cols-1 else (j+1)*col_w - cell = mask_classes[y0:y1, x0:x1] - if cell.size: - frac[i, j] = np.mean(cell == ClassesSegmentacao.NAONAVEGAVEL.value) + x0 = j * col_w + x1 = W if j == cols - 1 else (j + 1) * col_w + + area = max(1, (y1 - y0) * (x1 - x0)) + s = self._sum_integral(ii, y0, y1, x0, x1) + frac[i, j] = s / float(area) # limiares por faixa (modelo em “A”: mais chão embaixo, mais cana no topo) near_lado_min, near_canal_max = 0.05, 0.70 @@ -767,3 +871,63 @@ class SegmentacaoManager: return overlay except Exception as e: print(f"Erro ao gerar display_segmentation_debug: {e}") + + + def _resolver_status_modelo_rapido(self, aux_result): + if not aux_result or aux_result.get("type") != "label": + return None, None, None, None + + conf = float(aux_result.get("label_conf", 0.0)) + label_id = int(aux_result.get("label_id", StatusCarroMapa.Indefinido.value)) + + if conf < 0.70: + return None, None, None, None + + status_now = StatusCarroMapa(label_id) + + self._status_hist.append((status_now, self._now())) + status_final = self._maioria_ultimos() + self._status_final_hist.append(status_final) + status_before = self._status_final_hist[0] if len(self._status_final_hist) > 1 else status_final + + debug = None + if self.gerar_debug_status: + debug = { + "origem_status": "modelo_fast", + "status_modelo": label_id, + "status_modelo_conf": conf, + "heuristica_executada": False, + } + + return status_now, status_final, status_before, debug + + def _fit_linha_ponderada(self, ys_fit, xs_fit, near_is_bottom=True): + n = len(ys_fit) + if n < 2: + return None + + if near_is_bottom: + w = np.linspace(2.0, 1.0, n, dtype=np.float32) + else: + w = np.linspace(1.0, 2.0, n, dtype=np.float32) + + y = ys_fit.astype(np.float32) + x = xs_fit.astype(np.float32) + + sw = np.sum(w) + y_mean = np.sum(w * y) / max(sw, 1e-6) + x_mean = np.sum(w * x) / max(sw, 1e-6) + + dy = y - y_mean + dx = x - x_mean + + denom = np.sum(w * dy * dy) + if abs(denom) < 1e-6: + return 0.0 + + a = np.sum(w * dy * dx) / denom + return float(a) + + def _sum_integral(self, ii, y0, y1, x0, x1): + return ii[y1, x1] - ii[y0, x1] - ii[y1, x0] + ii[y0, x0] + diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/weed_worker/camera_manager.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/weed_worker/camera_manager.py index 19eb9ed86..c84708f4a 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/weed_worker/camera_manager.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/weed_worker/camera_manager.py @@ -5,18 +5,32 @@ import time import cv2 import numpy as np -from camera_worker.raw_segformer_service import RawSegformerService + +from camera_worker.camera_multispectral import CameraMultispectral +from camera_worker.oak_fcc3_core.segformer_service import MultiSpecSegformerService + from shared.enums import StatusModulo, StatusOperacao, T_Code, TipoFrameCamera, WeedWorkerCommandType -from camera_worker.camera_gal import CameraGal from shared.contexto_global_redis import CmdKey, ContextoGlobalRedis, CtxKey from weed_worker.weed_detector import WeedDetector from visual_worker.utils import converter_valores_numpy +from shared.perf_monitor import VisualPerfMonitor class CameraManager: def __init__(self, mostrar_log): self.mostrar_log = mostrar_log self.mx_id = None self.model_svc = None + + self._loop_tensor_iniciado = False + self._loop_inferencia_iniciado = False + self._loop_deteccao_iniciado = False + self._loop_analise_iniciado = False + self._loop_stream_iniciado = False + self._loop_publicacao_iniciado = False + + self._vida_lock = threading.RLock() + self._fechando_camera = False + self.reiniciar_status() def reiniciar_status(self): @@ -24,16 +38,96 @@ class CameraManager: self.operante = False self.iniciando = False self.weed_detector = None + self.seg_config = None + self.qtd_bicos = 0 + self.debug_visual = False self._ultimo_rgb_frame = None self._ultima_analise = {} self._ts_segmentacao_anterior = 0 self._ts_ultima_analise = 0 self._ultimo_predictions = None + self._ultimo_predictions_full = None self._ultimo_raw_base = None self._ultimo_raw_input = None self._ultimo_controle = None + self.tipo_camera_atual = None self._analisando_segmentacao = False + self._ultimo_preview_ts = 0 + self._ultimo_preview_rgb = None + self._ultimo_preview_seg = None + self._ultimo_preview_overlay = None + self._ultimo_preview_debug = None + self._lock_tensor = threading.Lock() + self._tensor_pronto = None + self._tensor_res = None + self._tensor_ts = 0.0 + self._tensor_consumido_ts = 0.0 + self._fps_infer_last_ts = None + self._fps_infer_ema = 0.0 + self._ultimo_infer_ms = 0.0 + self._ultimo_infer_gpu_ms = 0.0 + self._ultimo_loop_analise_fps = 0.0 + self._em_warmup = False + self._falhas_utilizavel = 0 + self._fechando_camera = False + + self._pred_lock = threading.RLock() + self._pred_cache = { + "ts": 0.0, + "tensor_ts": 0.0, + "predictions": None, + "res": None, + "infer_ms": 0.0, + "infer_gpu_ms": 0.0, + "fps_model": 0.0, + } + self._pred_consumido_ts = 0.0 + + self._cache_lock = threading.RLock() + self._pub_lock = threading.RLock() + + self._pub_cache = { + "ts_analise": 0.0, + + "analise": None, + "controle_bicos": None, + "performance_weed": None, + "cmd_controle": None, + + "ts_analise_weed": 0.0, + "ts_controle_bicos": 0.0, + "ts_performance_weed": 0.0, + "ts_cmd_controle": 0.0, + + "dirty_analise": False, + "dirty_controle_bicos": False, + "dirty_performance_weed": False, + "dirty_cmd_controle": False, + } + + self._ultimo_pub_debug = { + "ts": 0.0, + "publicou": 0, + "redis_ms": 0.0, + "campos": 0, + } + + self._perf_weed = { + "fps_model": 0.0, + "fps_inferencia": 0.0, + "infer_ms": 0.0, + "infer_gpu_ms": 0.0, + "loop_ms": 0.0, + "tensor_ms": 0.0, + "detector_ms": 0.0, + "ctx_read_ms": 0.0, + "controle_ms": 0.0, + "pub_cache_ms": 0.0, + } + + self.perf = VisualPerfMonitor(janela=180) + def inicializar(self, mx_id): if self.iniciando: return @@ -52,11 +146,23 @@ class CameraManager: from weed_worker.config import load_seg_config seg_config = load_seg_config() + self.seg_config = seg_config try: - nova = CameraGal(self.mostrar_log, mx_id, raw_w=seg_config["ia_resolution"][0], raw_h=seg_config["ia_resolution"][1]) + nova = CameraMultispectral( + self.mostrar_log, + mx_id=mx_id, + module_calibration_json=seg_config.get("module_calibration_json"), + width=int(seg_config.get("camera_width", 1280)), + height=int(seg_config.get("camera_height", 800)), + fps=int(seg_config.get("camera_fps", 20)), + target_size=seg_config.get("ia_resolution", [1024, 640]), + ) + self.tipo_camera_atual = "multispectral" + if nova.iniciado: self.camera = nova + except Exception as e: self.mostrar_log(f"⚠️ Camera com ID {mx_id} não conectada: {e}") self.iniciando = False @@ -65,7 +171,7 @@ class CameraManager: self.mostrar_log(f"❌ Camera com ID {mx_id} não iniciada.") else: self.mostrar_log(f"📷 Camera selecionada: {self.camera.modelo} - {self.camera.mx_id}") - self.operante = True + self._ultima_analise = {} self._ultimo_rgb_frame = None self._ultimo_predictions = None @@ -73,17 +179,246 @@ class CameraManager: self._ultimo_raw_input = None self._ultimo_controle = None self._analisando_segmentacao = False + self.qtd_bicos = seg_config.get("qtd_bicos", 7) + self.debug_visual = seg_config.get("debug_visual", False) - self.model_svc = RawSegformerService(model_config=seg_config) - classes = self.model_svc.get_classes() - colormap_rgb = self.model_svc.get_colormap() - self.weed_detector = WeedDetector(color_map=colormap_rgb, classes=classes) + if not hasattr(self, "model_svc") or self.model_svc is None: + self.model_svc = MultiSpecSegformerService( + model_config=seg_config, + mostrar_log=self.mostrar_log + ) - self._iniciar_loop_analise_continua(20.0) - self._iniciar_loop_frame_stream(self.camera.stream._op_fps) + classes = self.model_svc.get_classes() + colormap_rgb = self.model_svc.get_colormap() + + if not hasattr(self, "weed_detector") or self.weed_detector is None: + self.weed_detector = WeedDetector( + color_map=colormap_rgb, + classes=classes + ) + + # Bloqueia runtime durante warmup. + self.operante = False + self._em_warmup = True + + warmup_ok = self._executar_warmup_camera_manager( + n_tensors=5, + n_infer=4, + n_detector=2, + timeout_s=8.0 + ) + + self._em_warmup = False + + if not warmup_ok: + self.mostrar_log("[weed][WARMUP] finalizou com alerta, liberando operação mesmo assim") + + self.operante = True + + self.debug_perf = bool(seg_config.get("debug_perf", False)) + freq_analise = float(seg_config.get("analise_fps", 20.0)) + freq_tensor = float(seg_config.get("tensor_fps", 20.0)) + freq_publicacao = float(seg_config.get("publicacao_fps", 15.0)) + freq_inferencia = float(seg_config.get("inferencia_fps", freq_analise)) + freq_deteccao = float(seg_config.get("deteccao_fps", freq_analise)) + + if not self._loop_stream_iniciado: + cam_atual = self.camera + if cam_atual is None: + self.mostrar_log("[INIT] não iniciou stream: camera None") + else: + stream = getattr(cam_atual, "stream", None) + stream_fps = getattr(stream, "_op_fps", 2.0) + self._iniciar_loop_frame_stream(stream_fps) + self._loop_stream_iniciado = True + + if not self._loop_publicacao_iniciado: + self._iniciar_loop_publicacao_weed(freq=freq_publicacao) + self._loop_publicacao_iniciado = True + + if not self._loop_tensor_iniciado: + self._iniciar_loop_captura_tensor(freq_tensor) + self._loop_tensor_iniciado = True + + if not self._loop_inferencia_iniciado: + self._iniciar_loop_inferencia(freq=freq_inferencia) + self._loop_inferencia_iniciado = True + + if not self._loop_deteccao_iniciado: + self._iniciar_loop_deteccao_weed(freq=freq_deteccao) + self._loop_deteccao_iniciado = True + + if not self._loop_analise_iniciado: + self._iniciar_loop_analise_continua(freq=15.0) + self._loop_analise_iniciado = True + self.iniciando = False self.atualizar_saude_camera() + def fechar_camera_manager(self, motivo=""): + try: + if self.camera is not None: + self.camera.parar() + except Exception as e: + self.mostrar_log(f"[weed] Erro ao fechar câmera: {e}") + + self.camera = None + self.camera_id = None + self.camera_manager_iniciado = False + + self.mostrar_log(f"[weed] Camera Manager fechado: {motivo}") + + def _executar_warmup_camera_manager( + self, + n_tensors=5, + n_infer=4, + n_detector=2, + timeout_s=8.0 + ): + if self.camera is None: + return False + + self.mostrar_log("[weed][WARMUP] iniciando warmup do CameraManager...") + + operante_anterior = self.operante + warmup_anterior = getattr(self, "_em_warmup", False) + + self.operante = False + self._em_warmup = True + + tensor5 = None + res = None + predictions = None + analise_completa = None + + try: + # ===================================================== + # 1) Aquecer captura/fusão multispectral + # ===================================================== + tensor_ok = 0 + + t0 = time.time() + while time.time() - t0 < timeout_s and tensor_ok < n_tensors: + try: + t_cap0 = time.perf_counter() + tensor5, res = self.camera.requisitar_tensor_multispec(force=True) + t_cap1 = time.perf_counter() + + if tensor5 is not None: + tensor_ok += 1 + + with self._lock_tensor: + self._tensor_pronto = tensor5 + self._tensor_res = res + self._tensor_ts = time.time() + + self.mostrar_log( + f"[weed][WARMUP] tensor {tensor_ok}/{n_tensors} " + f"total={(t_cap1 - t_cap0) * 1000.0:.1f}ms " + f"shape={getattr(tensor5, 'shape', None)}" + ) + + except Exception as e: + self.mostrar_log(f"[weed][WARMUP] tensor falhou: {e}") + + time.sleep(0.03) + + if tensor5 is None: + self.mostrar_log("[weed][WARMUP] abortado: sem tensor válido") + return False + + # ===================================================== + # 2) Aquecer modelo + # ===================================================== + infer_ok = 0 + + for i in range(max(0, n_infer)): + try: + t_inf0 = time.perf_counter() + predictions = self.model_svc.infer_tensor_fast( + tensor5, + keep_probs=False + ) + t_inf1 = time.perf_counter() + + infer_full = getattr(self.model_svc, "_ultimo_predictions_full", {}) or {} + infer_gpu_ms = infer_full.get("infer_ms", None) + + self._atualizar_fps_inferencia( + infer_ms=(t_inf1 - t_inf0) * 1000.0, + infer_gpu_ms=infer_gpu_ms + ) + + if predictions is not None: + infer_ok += 1 + self._ultimo_raw_base = tensor5 + self._ultimo_raw_input = tensor5 + self._ultimo_predictions = predictions + self._ultimo_predictions_full = infer_full + + self.mostrar_log( + f"[weed][WARMUP] infer {i+1}/{n_infer} " + f"total={(t_inf1 - t_inf0) * 1000.0:.1f}ms " + f"gpu={infer_gpu_ms if infer_gpu_ms is not None else -1}" + ) + + except Exception as e: + self.mostrar_log(f"[weed][WARMUP] infer falhou: {e}") + + time.sleep(0.03) + + if predictions is None: + self.mostrar_log("[weed][WARMUP] sem predictions válidas") + return False + + # ===================================================== + # 3) Aquecer WeedDetector + # ===================================================== + det_ok = 0 + + for i in range(max(0, n_detector)): + try: + t_det0 = time.perf_counter() + analise_completa = self.detectar_ervas(predictions, None) + t_det1 = time.perf_counter() + + if isinstance(analise_completa, dict): + det_ok += 1 + analise = analise_completa.get("dados_visuais", {}) + self._ultima_analise = analise_completa.copy() + self._set_pub_cache("analise", analise) + + self.mostrar_log( + f"[weed][WARMUP] detector {i+1}/{n_detector} " + f"total={(t_det1 - t_det0) * 1000.0:.1f}ms" + ) + + except Exception as e: + self.mostrar_log(f"[weed][WARMUP] detector falhou: {e}") + + time.sleep(0.03) + + self.mostrar_log( + f"[weed][WARMUP] finalizado | " + f"tensor_ok={tensor_ok}/{n_tensors} " + f"infer_ok={infer_ok}/{n_infer} " + f"det_ok={det_ok}/{n_detector}" + ) + + return True + + except Exception as e: + self.mostrar_log(f"[weed][WARMUP] erro geral: {e}") + return False + + finally: + self.operante = operante_anterior + self._em_warmup = warmup_anterior + + self.perf = VisualPerfMonitor(janela=180) + if self.camera is not None: + self.camera.perf = self.perf + def atualizar_saude_camera(self): self._ultima_saude_ts = time.time() #self.mostrar_log("Atualizando saude da camera...") @@ -96,6 +431,36 @@ class CameraManager: except Exception as e: self.mostrar_log(f"[saude] erro: {e}") + def _get_tensor_novo_para_inferencia(self): + with self._lock_tensor: + tensor5 = self._tensor_pronto + res = self._tensor_res + ts_tensor = self._tensor_ts + + if tensor5 is None or ts_tensor <= 0: + return None, None, None + + if ts_tensor == self._tensor_consumido_ts: + return None, None, res + + self._tensor_consumido_ts = ts_tensor + return tensor5, ts_tensor, res + + def _get_prediction_nova_para_deteccao(self): + with self._pred_lock: + cache = dict(self._pred_cache) + + pred_ts = float(cache.get("ts", 0.0) or 0.0) + + if pred_ts <= 0: + return None + + if pred_ts == self._pred_consumido_ts: + return None + + self._pred_consumido_ts = pred_ts + return cache + def get_rgb_frame(self): if self.camera is None: return None, None, None @@ -119,29 +484,102 @@ class CameraManager: return None, None, None try: - t0 = time.time() - raw4_base = self.camera.requisitar_frame_raw(force=True) - t1 = time.time() - r = raw4_base[0] - g = raw4_base[1] - ir = raw4_base[2] - b = raw4_base[3] - raw_input = self.model_svc.build_raw_input(r, g, ir, b) - t2 = time.time() - predictions = self.model_svc.infer_raw(raw_input) - ts = time.time() - res = {} + t_total0 = time.perf_counter() - self._ultimo_raw_base = raw4_base - self._ultimo_raw_input = raw_input - self._ultimo_predictions = predictions + if hasattr(self.camera, "requisitar_tensor_multispec"): + # ========================== + # 1) Captura + decode + fusão + # ========================== + t0 = time.perf_counter() + with self._lock_tensor: + tensor5 = self._tensor_pronto + res = self._tensor_res + ts_tensor = self._tensor_ts - #print(f"[PREDICTIONS] t_total: {ts - t0:.5f} s, t_req: {t1 - t0:.5f} s, t_build_raw: {t2 - t1:.5f} s, t_infer: {ts - t2:.5f} s") + if tensor5 is None or ts_tensor == self._tensor_consumido_ts: + return None, None, res + + self._tensor_consumido_ts = ts_tensor + t_capture_ms = (time.perf_counter() - t0) * 1000.0 + + #perf = res.get("perf", {}) if isinstance(res, dict) else {} + #self.mostrar_log( + # "[PERF][TENSOR] " + # f"get_decoded={perf.get('get_decoded_ms', -1):.1f}ms " + # f"build={perf.get('build_ms', -1):.1f}ms " + # f"validate={perf.get('validate_ms', -1):.1f}ms " + # f"post={perf.get('post_ms', -1):.1f}ms " + # f"preview={perf.get('preview_ms', -1):.1f}ms " + # f"total={perf.get('total_ms', -1):.1f}ms " + # f"origem={perf.get('origem_tensor')}" + #) + + if tensor5 is None: + return None, None, res + + # ========================== + # 2) Estatística simples + # ========================== + t0 = time.perf_counter() + sig = float(np.mean(tensor5[0])) if tensor5 is not None else -1 + t_stats_ms = (time.perf_counter() - t0) * 1000.0 + + # ========================== + # 3) Inferência IA + # ========================== + t0 = time.perf_counter() + predictions = self.model_svc.infer_tensor_fast(tensor5, keep_probs=False) + t_infer_ms = (time.perf_counter() - t0) * 1000.0 + + infer_full = getattr(self.model_svc, "_ultimo_predictions_full", {}) or {} + infer_gpu_ms = infer_full.get("infer_ms", None) + + self._atualizar_fps_inferencia( + infer_ms=t_infer_ms, + infer_gpu_ms=infer_gpu_ms + ) + + # ========================== + # 4) Cache local + # ========================== + t0 = time.perf_counter() + ts = time.time() + + self._ultimo_raw_base = tensor5 + self._ultimo_raw_input = tensor5 + self._ultimo_predictions = predictions + self._ultimo_predictions_full = getattr(self.model_svc, "_ultimo_predictions_full", None) + + # ========================== + # Log limitado + # ========================== + #t_cache_ms = (time.perf_counter() - t0) * 1000.0 + #t_total_ms = (time.perf_counter() - t_total0) * 1000.0 + #agora_log = time.time() + #if not hasattr(self, "_ultimo_log_multispec_ts"): + # self._ultimo_log_multispec_ts = 0 + #if (agora_log - self._ultimo_log_multispec_ts) >= 1.0: + # self._ultimo_log_multispec_ts = agora_log + # self.mostrar_log( + # "[PERF][SEG] " + # f"buffer_read_ms={t_capture_ms:.1f} " + # f"tensor_core_ms={res.get('perf', {}).get('total_ms', -1):.1f} " + # f"infer_call_ms={t_infer_ms:.1f} " + # f"infer_gpu_ms={infer_gpu_ms if infer_gpu_ms is not None else -1:.1f} " + # f"stats_ms={t_stats_ms:.1f} " + # f"cache_ms={t_cache_ms:.1f} " + # f"total_ms={t_total_ms:.1f} " + # f"fps_teorico={1000.0 / max(t_total_ms, 1e-6):.2f} " + # f"tensor_shape={tensor5.shape} " + # f"mean_R={sig:.6f} " + # f"core_dur={res.get('duracao') if isinstance(res, dict) else None}" + # ) + + if predictions is not None: + return predictions, ts, res + + return None, ts, res - if predictions is not None: - return predictions, ts, res - elif "X_LINK_ERROR" in res["erro"]: - self.reiniciar_status() except Exception as e: self.mostrar_log(f"Erro ao requisitar predictions: {e}") if "X_LINK_ERROR" in str(e): @@ -149,46 +587,200 @@ class CameraManager: return None, None, None - def get_selected_frame(self, _frame_type: TipoFrameCamera): - _frame = None - if (_frame_type in [TipoFrameCamera.Rgb, TipoFrameCamera.Segmentacao, TipoFrameCamera.Overlay, TipoFrameCamera.Debug]): - if self.model_svc is not None: - rgb_frame, seg_frame, overlay_frame, _, _ = self.model_svc.preview_infer_cached(self._ultimo_raw_input, self._ultimo_predictions, alpha=0.5) - if _frame_type == TipoFrameCamera.Rgb: - _frame = rgb_frame - elif _frame_type == TipoFrameCamera.Segmentacao: - _frame = seg_frame - elif _frame_type == TipoFrameCamera.Overlay: - _frame = overlay_frame - elif _frame_type == TipoFrameCamera.Debug: - self.weed_detector._mostrar_debug_bicos_overlay(overlay_frame, [], self._ultimo_controle, show=False) - _frame = self.weed_detector._dbg_img - elif _frame_type == TipoFrameCamera.Raw4: - _frame = self._ultimo_raw_base - return _frame + def get_debug_frame(self, mostrar = False): + metricas_perf = { + "fps_dbg": None, + "fps_infer": self._fps_infer_ema, + "infer_ms": self._ultimo_infer_ms, + "infer_gpu_ms": self._ultimo_infer_gpu_ms, + "fps_loop": self._ultimo_loop_analise_fps, + } + self._ultimo_preview_debug = self.weed_detector._mostrar_debug_bicos_overlay( + self._ultimo_preview_overlay, + [], + self._ultimo_controle or {}, + self.seg_config, + show=mostrar, + metricas_perf=metricas_perf + ) - def _iniciar_loop_analise_continua(self, freq): + return self._ultimo_preview_debug + + def get_selected_frame(self, _frame_type: TipoFrameCamera): + agora = time.time() + + if _frame_type in [TipoFrameCamera.Rgb, TipoFrameCamera.Segmentacao, TipoFrameCamera.Overlay, TipoFrameCamera.Debug]: + if self.model_svc is not None and self._ultimo_raw_input is not None: + # Limita preview/overlay a 5 FPS + if (agora - self._ultimo_preview_ts) < 0.20: + if _frame_type == TipoFrameCamera.Rgb and self._ultimo_preview_rgb is not None: + return self._ultimo_preview_rgb + if _frame_type == TipoFrameCamera.Segmentacao and self._ultimo_preview_seg is not None: + return self._ultimo_preview_seg + if _frame_type == TipoFrameCamera.Overlay and self._ultimo_preview_overlay is not None: + return self._ultimo_preview_overlay + if _frame_type == TipoFrameCamera.Debug and self._ultimo_preview_debug is not None: + return self._ultimo_preview_debug + + rgb_frame, seg_frame, overlay_frame, _, _ = self.model_svc.preview_infer_cached( + self._ultimo_raw_input, + self._ultimo_predictions, + alpha=0.5 + ) + + debug_frame = None + if _frame_type == TipoFrameCamera.Debug: + debug_frame = self.get_debug_frame() + + self._ultimo_preview_ts = agora + self._ultimo_preview_rgb = rgb_frame + self._ultimo_preview_seg = seg_frame + self._ultimo_preview_overlay = overlay_frame + self._ultimo_preview_debug = debug_frame + + frame = rgb_frame + if _frame_type == TipoFrameCamera.Rgb: + frame = rgb_frame + elif _frame_type == TipoFrameCamera.Segmentacao: + frame = seg_frame + elif _frame_type == TipoFrameCamera.Overlay: + frame = overlay_frame + elif _frame_type == TipoFrameCamera.Debug: + frame = debug_frame + return frame + + def _iniciar_loop_analise_continua(self, freq=15.0): def loop(): while True: - if self.camera is None: - time.sleep(5) - continue - t0 = time.time() + try: - status = StatusModulo((self.camera.ultima_saude or {}).get("status", StatusModulo.DESCONECTADO.value)) + if self.camera is None: + time.sleep(0.5) + continue + + status = StatusModulo( + (self.camera.ultima_saude or {}).get( + "status", + StatusModulo.DESCONECTADO.value + ) + ) + ts_status = (self.camera.ultima_saude or {}).get("timestamp", 0) - if status == StatusModulo.DESCONECTADO and (t0 - ts_status) > 5.0: + + if status == StatusModulo.DESCONECTADO and (t0 - ts_status) > 10.0: + try: + if getattr(self.camera, "imu", None): + self.camera.imu.parar() + except Exception: + pass + self.reiniciar_status() continue - elif self.operante and status == StatusModulo.OPERANTE and self.camera is not None: - self._realizar_analises() + + agora = time.time() + + if not hasattr(self, "_ultimo_perf_publish"): + self._ultimo_perf_publish = 0.0 + + if agora - self._ultimo_perf_publish >= 1.0: + self._ultimo_perf_publish = agora + + resumo = self.perf.resumo() + resumo["weed_cache"] = { + "tensor_ts": self._tensor_ts, + "tensor_age_ms": (time.time() - self._tensor_ts) * 1000.0 if self._tensor_ts else None, + "tensor_consumido_ts": self._tensor_consumido_ts, + "tem_tensor": self._tensor_pronto is not None, + } + resumo["pub_debug"] = getattr(self, "_ultimo_pub_debug", {}) + + self._set_pub_cache("performance_weed", resumo) + + if not self.debug_perf: + continue + + loops = resumo.get("loops", {}) + + def _fmt(v, casas=1, default=0.0): + try: + if v is None: + v = default + return f"{float(v):.{casas}f}" + except Exception: + return f"{default:.{casas}f}" + + def _m(loop, nome, stat="med", default=0.0): + try: + return loop.get("metrics_ms", {}).get(nome, {}).get(stat, default) + except Exception: + return default + + def _lat(loop, stat="med", default=0.0): + try: + return loop.get("latencia_ms", {}).get(stat, default) + except Exception: + return default + + def _per(loop, stat="med", default=0.0): + try: + return loop.get("periodo_ms", {}).get(stat, default) + except Exception: + return default + + def _fps(loop): + try: + return float(loop.get("fps_real", 0.0) or 0.0) + except Exception: + return 0.0 + + tensor = loops.get("tensor", {}) + inf = loops.get("inferencia", {}) + det = loops.get("deteccao", {}) + pub = loops.get("publicacao", {}) + stream = loops.get("stream", {}) + + pub_dbg = getattr(self, "_ultimo_pub_debug", {}) + + self.mostrar_log( + "[WEED_PERF] " + f"fps tensor={_fps(tensor):.1f} " + f"inf={_fps(inf):.1f} " + f"det={_fps(det):.1f} " + f"pub={_fps(pub):.1f} " + f"stream={_fps(stream):.1f} | " + f"period inf={_fmt(_per(inf))}ms " + f"det={_fmt(_per(det))}ms " + f"tensor={_fmt(_per(tensor))}ms" + ) + + self.mostrar_log( + "[WEED_DETAIL] " + f"INF total={_fmt(_lat(inf))} " + f"get_tensor={_fmt(_m(inf, 'get_tensor_ms'))} " + f"infer={_fmt(_m(inf, 'infer_ms'))} " + f"gpu={_fmt(_m(inf, 'infer_gpu_ms'))} | " + f"DET total={_fmt(_lat(det))} " + f"get_pred={_fmt(_m(det, 'get_pred_ms'))} " + f"detector={_fmt(_m(det, 'detector_ms'))} " + f"ctx={_fmt(_m(det, 'ctx_read_ms'))} " + f"ctrl={_fmt(_m(det, 'control_ms'))} " + f"cache={_fmt(_m(det, 'pub_cache_ms'))} | " + f"TENSOR total={_fmt(_lat(tensor))} " + f"core={_fmt(_m(tensor, 'tensor_core_ms'))} | " + f"PUB total={_fmt(_lat(pub))} redis={_fmt(_m(pub, 'redis_ms'))} " + f"debug_pub={pub_dbg.get('publicou', 0)} " + f"campos={pub_dbg.get('campos', 0)} " + f"redis_dbg={pub_dbg.get('redis_ms', 0):.1f}ms" + ) + except Exception as e: - self.mostrar_log(f"Erro no loop de analise continua: {e}") + self.mostrar_log(f"Erro no loop supervisor weed: {e}") + finally: latencia = time.time() - t0 - time.sleep(max(0, (1.0 / freq) - latencia)) - + time.sleep(max(0.0, (1.0 / freq) - latencia)) + threading.Thread(target=loop, daemon=True).start() def _iniciar_loop_frame_stream(self, freq): @@ -198,6 +790,7 @@ class CameraManager: time.sleep(5) continue t0 = time.time() + t_loop0 = time.perf_counter() try: _camera = (ContextoGlobalRedis.get_camera(self.mx_id) or {}) _stream_on = _camera.get("streaming", False) @@ -206,78 +799,458 @@ class CameraManager: _frame = self.get_selected_frame(_frame_type) self.camera.enviar_frame_tcp(_frame) - from weed_worker.config import load_seg_config - config = load_seg_config() - if (config.get("debug_visual")): - frame_dbg = self.get_selected_frame(TipoFrameCamera.Overlay) - if frame_dbg is not None: - self.weed_detector._mostrar_debug_bicos_overlay(frame_dbg, [], self._ultimo_controle, config, show=True) + if (self.debug_visual): + self.get_debug_frame(True) except Exception as e: self.mostrar_log(f"Erro no loop de stream: {e}") finally: + t_loop1 = time.perf_counter() + self.perf.tick( + "stream", + latencia_ms=(t_loop1 - t_loop0) * 1000.0, + ) latencia = time.time() - t0 freq = self.camera.stream._op_fps time.sleep(max(0, (1.0 / freq) - latencia)) threading.Thread(target=loop, daemon=True).start() - def _realizar_analises(self): - if self._analisando_segmentacao: return - self._analisando_segmentacao = True - try: - t0 = time.time() - predictions, ts, res = self.get_segmentation_predictions() - if ts == self._ts_segmentacao_anterior: - self._analisando_segmentacao = False - return - fps = 1.0 / (ts - self._ts_segmentacao_anterior) - self._ts_segmentacao_anterior = ts - if predictions is not None: - analise_completa = self.detectar_ervas(predictions, None) - analise = analise_completa.get("dados_visuais", {}) + def _iniciar_loop_publicacao_weed(self, freq=15.0): + def loop(): + ultimo_payload_forcado = 0.0 - ContextoGlobalRedis.atualizar_ctx_dict( - CtxKey.DadosWeedWorker, - ts_analise=ts, - fps_model=fps, - analise=converter_valores_numpy(analise) - ) + while True: + t0 = time.time() + t_loop0 = time.perf_counter() + + try: + if self.camera is None: + time.sleep(0.5) + continue + + if not self.operante: + time.sleep(0.2) + continue + + agora = time.time() + publicar_tudo = (agora - ultimo_payload_forcado) >= 1.0 + + payload_weed, payload_controle, cmd_controle = self._montar_payload_publicacao_weed( + publicar_tudo=publicar_tudo + ) + + publicou = 0 + campos = 0 + t_redis0 = time.perf_counter() + + # Dados do WeedWorker + if len(payload_weed.keys()) > 1: + ContextoGlobalRedis.atualizar_ctx_dict( + CtxKey.DadosWeedWorker, + **converter_valores_numpy(payload_weed) + ) + publicou = 1 + campos += len(payload_weed) + + # DadosControle + if payload_controle is not None: + ContextoGlobalRedis.atualizar_ctx_dict( + CtxKey.DadosControle, + **converter_valores_numpy(payload_controle) + ) + publicou = 1 + campos += len(payload_controle) + + # Comando para TX + if cmd_controle is not None: + ContextoGlobalRedis.publicar_comando( + CmdKey.WeedWorkerTx, + cmd_controle + ) + publicou = 1 + campos += 1 + + t_redis1 = time.perf_counter() + + if publicar_tudo: + ultimo_payload_forcado = agora + + self._ultimo_pub_debug = { + "ts": time.time(), + "publicou": publicou, + "campos": campos, + "redis_ms": (t_redis1 - t_redis0) * 1000.0 if publicou else 0.0, + } + + t_loop1 = time.perf_counter() + self.perf.tick( + "publicacao", + latencia_ms=(t_loop1 - t_loop0) * 1000.0, + redis_ms=(t_redis1 - t_redis0) * 1000.0 if publicou else 0.0, + publicou=publicou, + campos=campos, + ) + + except Exception as e: + self.mostrar_log(f"[weed] erro no loop_publicacao_weed: {e}") + + finally: + lat = time.time() - t0 + time.sleep(max(0.0, (1.0 / freq) - lat)) + + threading.Thread(target=loop, daemon=True).start() + + def _iniciar_loop_captura_tensor(self, freq=20.0): + def loop(): + while True: + if self.camera is None: + time.sleep(1) + continue + + t0 = time.perf_counter() + + try: + tensor5, res = self.camera.requisitar_tensor_multispec(force=True) + + if res.get("erro"): + erro = res.get("erro") + + if self.camera._is_erro_fatal_depthai(erro): + self.fechar_camera_manager(f"falha fatal DepthAI: {erro}") + return + + agora_log = time.time() + if not hasattr(self, "_ultimo_log_capture_perf_ts"): + self._ultimo_log_capture_perf_ts = 0.0 + + if agora_log - self._ultimo_log_capture_perf_ts >= 1.0: + self._ultimo_log_capture_perf_ts = agora_log + + #perf = res.get("perf", {}) if isinstance(res, dict) else {} + #self.mostrar_log( + # "[PERF][CAPTURE] " + # f"total={perf.get('total_ms', -1):.1f}ms " + # f"get_decoded={perf.get('get_decoded_ms', -1):.1f}ms " + # f"build={perf.get('build_ms', -1):.1f}ms " + # f"fps_capture={1000.0 / max(perf.get('total_ms', 1), 1e-6):.2f} " + # f"shape={None if tensor5 is None else tensor5.shape}" + #) + + if tensor5 is not None: + with self._lock_tensor: + self._tensor_pronto = tensor5 + self._tensor_res = res + self._tensor_ts = time.time() + + t_loop1 = time.perf_counter() + perf = res.get("perf", {}) if isinstance(res, dict) else {} + self.perf.tick( + "tensor", + latencia_ms=(t_loop1 - t0) * 1000.0, + tensor_core_ms=float(perf.get("total_ms", 0.0) or 0.0), + get_decoded_ms=float(perf.get("get_decoded_ms", 0.0) or 0.0), + build_ms=float(perf.get("build_ms", 0.0) or 0.0), + validate_ms=float(perf.get("validate_ms", 0.0) or 0.0), + post_ms=float(perf.get("post_ms", 0.0) or 0.0), + preview_ms=float(perf.get("preview_ms", 0.0) or 0.0), + frame_ts=self._tensor_ts, + idade_frame_ms=(time.time() - self._tensor_ts) * 1000.0 if self._tensor_ts else None, + ) + + except Exception as e: + self.mostrar_log(f"[CAPTURE] erro: {e}") + + lat = time.perf_counter() - t0 + time.sleep(max(0.0, (1.0 / freq) - lat)) + + threading.Thread(target=loop, daemon=True).start() + + def _iniciar_loop_inferencia(self, freq=20.0): + def loop(): + while True: + t0_wall = time.time() + t_loop0 = time.perf_counter() + + if self.iniciando: + time.sleep(0.05) + continue + + try: + if not self.operante or self.camera is None: + time.sleep(0.2) + continue + + # ===================================================== + # 1) Pegar tensor novo + # ===================================================== + t_get0 = time.perf_counter() + tensor5, tensor_ts, res = self._get_tensor_novo_para_inferencia() + t_get1 = time.perf_counter() + + if tensor5 is None or tensor_ts is None: + self.perf.inc("infer_sem_tensor_novo") + time.sleep(0.005) + continue + + # ===================================================== + # 2) Inferência IA + # ===================================================== + t_inf0 = time.perf_counter() + predictions = self.model_svc.infer_tensor_fast( + tensor5, + keep_probs=False + ) + t_inf1 = time.perf_counter() + + infer_ms = (t_inf1 - t_inf0) * 1000.0 + + infer_full = getattr(self.model_svc, "_ultimo_predictions_full", {}) or {} + infer_gpu_ms = infer_full.get("infer_ms", None) + + self._atualizar_fps_inferencia( + infer_ms=infer_ms, + infer_gpu_ms=infer_gpu_ms + ) + + if predictions is None: + self.perf.inc("infer_predictions_none") + continue + + # ===================================================== + # 3) Atualizar caches locais + # ===================================================== + pred_ts = time.time() + + with self._pred_lock: + self._pred_cache = { + "ts": pred_ts, + "tensor_ts": tensor_ts, + "predictions": predictions, + "res": res, + "infer_ms": float(infer_ms), + "infer_gpu_ms": float(infer_gpu_ms or 0.0), + "fps_model": float(self._fps_infer_ema or 0.0), + } + + self._ultimo_raw_base = tensor5 + self._ultimo_raw_input = tensor5 + self._ultimo_predictions = predictions + self._ultimo_predictions_full = infer_full + + t_loop1 = time.perf_counter() + + self.perf.tick( + "inferencia", + latencia_ms=(t_loop1 - t_loop0) * 1000.0, + get_tensor_ms=(t_get1 - t_get0) * 1000.0, + infer_ms=infer_ms, + infer_gpu_ms=float(infer_gpu_ms or 0.0), + tensor_ts=tensor_ts, + frame_ts=pred_ts, + idade_tensor_ms=(time.time() - tensor_ts) * 1000.0 if tensor_ts else None, + ) + + except Exception as e: + self.mostrar_log(f"❌ Erro no loop_inferencia weed: {e}") + + finally: + latencia = time.time() - t0_wall + time.sleep(max(0.0, (1.0 / freq) - latencia)) + + threading.Thread(target=loop, daemon=True).start() + + def _iniciar_loop_deteccao_weed(self, freq=20.0): + def loop(): + while True: + t0_wall = time.time() + t_loop0 = time.perf_counter() + + if self.iniciando: + time.sleep(0.05) + continue + + try: + if not self.operante or self.camera is None: + time.sleep(0.2) + continue + + # ===================================================== + # 1) Pegar prediction nova + # ===================================================== + t_pred0 = time.perf_counter() + pred_cache = self._get_prediction_nova_para_deteccao() + t_pred1 = time.perf_counter() + + if pred_cache is None: + self.perf.inc("det_sem_prediction_nova") + time.sleep(0.005) + continue + + predictions = pred_cache.get("predictions") + pred_ts = float(pred_cache.get("ts", 0.0) or 0.0) + tensor_ts = float(pred_cache.get("tensor_ts", 0.0) or 0.0) + + if predictions is None: + self.perf.inc("det_predictions_none") + continue + + # ===================================================== + # 2) WeedDetector + # ===================================================== + t_det0 = time.perf_counter() + analise_completa = self.detectar_ervas(predictions, None) + t_det1 = time.perf_counter() + + if not isinstance(analise_completa, dict): + self.mostrar_log("[WEED] análise inválida retornada pelo WeedDetector") + continue + + analise = analise_completa.get("dados_visuais", {}) + t_detector_ms = (t_det1 - t_det0) * 1000.0 + + # ===================================================== + # 3) Conversão numpy + # ===================================================== + t_conv0 = time.perf_counter() + analise_convertida = converter_valores_numpy(analise) + t_conv1 = time.perf_counter() + + # ===================================================== + # 4) Cache publicação da análise + # ===================================================== + t_pub_analise0 = time.perf_counter() + self._set_pub_cache("analise", { + "ts_analise": pred_ts, + "fps_model": pred_cache.get("fps_model", 0.0), + "fps_inferencia": self._fps_infer_ema, + "infer_ms": pred_cache.get("infer_ms", 0.0), + "infer_gpu_ms": pred_cache.get("infer_gpu_ms", 0.0), + "analise": analise_convertida, + }) + t_pub_analise1 = time.perf_counter() + + # ===================================================== + # 5) Estado operação/controle + # ===================================================== + t_ctx0 = time.perf_counter() + _operacao = ContextoGlobalRedis.get_operacao() + status_operacao = StatusOperacao( + _operacao.get("status", StatusOperacao.NaoIniciado.value) + ) + finalizando = _operacao.get("finalizando", False) + pulverizador_automatico = ContextoGlobalRedis.get_controle().get( + "pulverizador_automatico", + False + ) + t_ctx1 = time.perf_counter() + + # ===================================================== + # 6) Controle dos bicos + # ===================================================== + t_ctrl0 = time.perf_counter() + controle_detectado = analise.get("controle") or {} + + if status_operacao == StatusOperacao.EmAndamento and not finalizando: + atuacao_bicos = controle_detectado + else: + atuacao_bicos = {i: False for i in controle_detectado.keys()} + + if not atuacao_bicos: + atuacao_bicos = {i: False for i in range(self.qtd_bicos)} - _operacao = ContextoGlobalRedis.get_operacao() - status_operacao = StatusOperacao(_operacao.get("status", StatusOperacao.NaoIniciado.value)) - finalizando = _operacao.get("finalizando", False) - pulverizador_automatico = ContextoGlobalRedis.get_controle().get("pulverizador_automatico", False) - if status_operacao == StatusOperacao.EmAndamento and not finalizando: - atuacao_bicos = analise.get("controle") - else: - atuacao_bicos = {i: False for i in analise.get("controle", {}).keys()} analise["controle"] = atuacao_bicos - - self._ultimo_controle = atuacao_bicos + self._ultimo_controle = atuacao_bicos + t_ctrl1 = time.perf_counter() - ContextoGlobalRedis.atualizar_ctx_dict( - CtxKey.DadosControle, - controle_bicos=atuacao_bicos - ) + # ===================================================== + # 7) Cache publicação controle + # ===================================================== + t_pub_ctrl0 = time.perf_counter() + self._set_pub_cache("controle_bicos", atuacao_bicos) + t_pub_ctrl1 = time.perf_counter() - if pulverizador_automatico: - ContextoGlobalRedis.publicar_comando(CmdKey.WeedWorkerTx, { "cmd": WeedWorkerCommandType.EnviarDadosControle.value, "params": atuacao_bicos }) - - self._ultima_analise = analise_completa.copy() - except Exception as e: - self.mostrar_log(f"❌ Erro na segmentacao semantica: {e}") - finally: - self._ts_ultima_analise = t0 - self._analisando_segmentacao = False + # ===================================================== + # 8) Cache comando TX + # ===================================================== + t_pub_cmd0 = time.perf_counter() + if pulverizador_automatico: + self._set_pub_cache("cmd_controle", { + "cmd": WeedWorkerCommandType.EnviarDadosControle.value, + "params": atuacao_bicos + }) + t_pub_cmd1 = time.perf_counter() + + self._ultima_analise = analise_completa.copy() + + t_loop1 = time.perf_counter() + total_ms = (t_loop1 - t_loop0) * 1000.0 + + self._ultimo_loop_analise_fps = 1000.0 / max(total_ms, 1e-6) + + self._perf_weed = { + "fps_model": pred_cache.get("fps_model", 0.0), + "fps_inferencia": self._fps_infer_ema, + "infer_ms": pred_cache.get("infer_ms", 0.0), + "infer_gpu_ms": pred_cache.get("infer_gpu_ms", 0.0), + + "detector_ms": t_detector_ms, + "convert_ms": (t_conv1 - t_conv0) * 1000.0, + "ctx_read_ms": (t_ctx1 - t_ctx0) * 1000.0, + "control_ms": (t_ctrl1 - t_ctrl0) * 1000.0, + + "pub_analise_ms": (t_pub_analise1 - t_pub_analise0) * 1000.0, + "pub_controle_ms": (t_pub_ctrl1 - t_pub_ctrl0) * 1000.0, + "pub_cmd_ms": (t_pub_cmd1 - t_pub_cmd0) * 1000.0, + + "total_ms": total_ms, + "fps_loop": self._ultimo_loop_analise_fps, + "pub_debug": getattr(self, "_ultimo_pub_debug", {}), + "pred_age_ms": (time.time() - pred_ts) * 1000.0 if pred_ts else None, + "tensor_age_ms": (time.time() - tensor_ts) * 1000.0 if tensor_ts else None, + } + + self.perf.tick( + "deteccao", + latencia_ms=total_ms, + get_pred_ms=(t_pred1 - t_pred0) * 1000.0, + detector_ms=t_detector_ms, + convert_ms=(t_conv1 - t_conv0) * 1000.0, + ctx_read_ms=(t_ctx1 - t_ctx0) * 1000.0, + control_ms=(t_ctrl1 - t_ctrl0) * 1000.0, + pub_analise_ms=(t_pub_analise1 - t_pub_analise0) * 1000.0, + pub_controle_ms=(t_pub_ctrl1 - t_pub_ctrl0) * 1000.0, + pub_cmd_ms=(t_pub_cmd1 - t_pub_cmd0) * 1000.0, + pub_cache_ms=( + (t_pub_analise1 - t_pub_analise0) + + (t_pub_ctrl1 - t_pub_ctrl0) + + (t_pub_cmd1 - t_pub_cmd0) + ) * 1000.0, + infer_ms=pred_cache.get("infer_ms", 0.0), + infer_gpu_ms=pred_cache.get("infer_gpu_ms", 0.0), + pred_ts=pred_ts, + tensor_ts=tensor_ts, + frame_ts=pred_ts, + idade_frame_ms=(time.time() - pred_ts) * 1000.0 if pred_ts else None, + idade_tensor_ms=(time.time() - tensor_ts) * 1000.0 if tensor_ts else None, + ) + + except Exception as e: + self.mostrar_log(f"❌ Erro no loop_deteccao_weed: {e}") + + finally: + latencia = time.time() - t0_wall + time.sleep(max(0.0, (1.0 / freq) - latencia)) + + threading.Thread(target=loop, daemon=True).start() def detectar_ervas(self, predictions, rgb_frame): if self.weed_detector is None: self.mostrar_log("WeedDetector não inicializado!") - return [] + return None try: return self.weed_detector.detectar(predictions, rgb_frame) except Exception as e: self.mostrar_log(f"Erro na detecção de ervas: {e}") - return [] + return None def salvar_frames(self, tipos: list, nome: str, pasta="frames_salvos"): if not self.operante: @@ -309,4 +1282,83 @@ class CameraManager: return [] + def _atualizar_fps_inferencia(self, infer_ms=None, infer_gpu_ms=None, alpha=0.2): + agora = time.time() + if self._fps_infer_last_ts is not None: + dt = agora - self._fps_infer_last_ts + if dt > 1e-6: + fps_inst = 1.0 / dt + if self._fps_infer_ema <= 0: + self._fps_infer_ema = fps_inst + else: + self._fps_infer_ema = (1.0 - alpha) * self._fps_infer_ema + alpha * fps_inst + + self._fps_infer_last_ts = agora + + if infer_ms is not None: + self._ultimo_infer_ms = float(infer_ms) + + if infer_gpu_ms is not None: + self._ultimo_infer_gpu_ms = float(infer_gpu_ms) + + return self._fps_infer_ema + + + + def _set_pub_cache(self, chave, valor): + try: + agora = time.time() + + with self._pub_lock: + self._pub_cache[chave] = valor + self._pub_cache[f"ts_{chave}"] = agora + self._pub_cache[f"dirty_{chave}"] = True + self._pub_cache["ts_analise"] = agora + + except Exception as e: + self.mostrar_log(f"[weed] erro ao atualizar pub_cache[{chave}]: {e}") + + def _montar_payload_publicacao_weed(self, publicar_tudo=False): + with self._pub_lock: + cache = dict(self._pub_cache) + + payload_weed = { + "ts_analise": time.time(), + } + + payload_controle = None + cmd_controle = None + + if publicar_tudo or cache.get("dirty_analise", False): + if cache.get("analise") is not None: + payload_weed["analise"] = cache["analise"] + payload_weed["ts_analise_weed"] = cache.get("ts_analise", 0.0) + + if publicar_tudo or cache.get("dirty_performance_weed", False): + if cache.get("performance_weed") is not None: + payload_weed["performance_weed"] = cache["performance_weed"] + payload_weed["ts_performance_weed"] = cache.get("ts_performance_weed", 0.0) + + if publicar_tudo or cache.get("dirty_controle_bicos", False): + if cache.get("controle_bicos") is not None: + payload_controle = { + "controle_bicos": cache["controle_bicos"] + } + + if cache.get("dirty_cmd_controle", False): + cmd_controle = cache.get("cmd_controle") + + if "analise" in payload_weed: + self._pub_cache["dirty_analise"] = False + + if "performance_weed" in payload_weed: + self._pub_cache["dirty_performance_weed"] = False + + if payload_controle is not None: + self._pub_cache["dirty_controle_bicos"] = False + + if cmd_controle is not None: + self._pub_cache["dirty_cmd_controle"] = False + + return payload_weed, payload_controle, cmd_controle diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/weed_worker/config.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/weed_worker/config.py index 589e364e2..affc7f890 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/weed_worker/config.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/weed_worker/config.py @@ -42,47 +42,44 @@ _CONFIG_LOCK = threading.Lock() def load_seg_config(force_reload=False): global _CONFIG_CACHE, _CONFIG_MTIME with _CONFIG_LOCK: - #try: - # mtime = os.path.getmtime(_CONFIG_PATH) - # if force_reload or _CONFIG_CACHE is None or mtime != _CONFIG_MTIME: - # with open(_CONFIG_PATH, "r", encoding="utf-8") as f: - # _CONFIG_CACHE = json.load(f) - # _CONFIG_MTIME = mtime - #except Exception as e: - # mostrar_log(f"Erro ao ler config: {e}") - # if _CONFIG_CACHE is None: - # # Valores default se der ruim no primeiro load - # _CONFIG_CACHE = { - # "debug_visual": True, - # "frames_consecutivos": 3, - # "frames_histerese": 2, - # "min_area_px": 400, - # "max_area_frac": 0.2, - # "ia_roi_begin": 0.0, - # "ia_roi_size": 1.0, - # "ia_resolution": [512,288], - # "erva_top_band_frac": 0.30, - # "erva_frac_ema": 0.3, - # "erva_thresh_vel_gain": 0.4, - # "min_frac_erva_global_on": 0.0020, - # "min_frac_erva_global_off": 0.0015, - # "min_frac_erva_top_on": 0.0015, - # "min_frac_erva_top_off": 0.0010, - # "min_frac_erva_por_bico": 0.02, - # "usar_morfologia": True, - # "kernel_morf": 3 - # } _CONFIG_CACHE = { "debug_visual": False, + "debug_perf": False, "frames_consecutivos": 3, "frames_histerese": 2, "min_area_px": 400, "max_area_frac": 0.2, "ia_roi_begin": 0.0, "ia_roi_size": 1.0, - "ia_resolution": [672,544], + + "analise_fps": 15.0, + "inferencia_fps": 15.0, + "deteccao_fps": 15.0, + "tensor_fps": 18.0, + "publicacao_fps": 15.0, + + "tipo_camera_solo": "multispectral", + "camera_width": 1280, + "camera_height": 800, + "fps": 40, + "ia_resolution": [1024,640], "ia_channels": 5, - "ia_use_ndvi": True, + "ia_input_channels": ["R", "G", "B", "RE", "NIR"], + "ia_use_ndvi": False, + "amp": True, + "fold_input_norm": True, + "runtime_mode": "target_direct", + "prediction_contract": "target_binary", + "output_mask_fullres": False, + "lowres_argmax": True, + "trust_input": True, + "channels_last": False, + "model_half": True, + "sync_for_timing": False, + "torch_compile": False, + "torch_compile_mode": "reduce-overhead", + "return_full_fast": False, + "erva_top_band_frac": 0.30, "erva_frac_ema": 0.3, "erva_thresh_vel_gain": 0.4, @@ -102,7 +99,38 @@ def load_seg_config(force_reload=False): "cana_halo_px": 5, "min_area_erva_px": 80, "erva_thresh_vel_gain_local": 0.6, - "k_roi_shift_px_per_vnorm": 24.0 + "k_roi_shift_px_per_vnorm": 24.0, + + "heads": { + "semantic": { + "enabled": True, + "type": "multiclass", + "num_classes": 3, + "classes": {"chao": 0, "cana": 1, "erva": 2}, + "ignore_index": 255 + }, + "vegetation": { + "enabled": True, + "type": "binary", + "num_classes": 2, + "classes": {"background": 0, "vegetation": 1}, + "ignore_index": 255 + }, + "cana": { + "enabled": True, + "type": "binary", + "num_classes": 2, + "classes": {"not_cana": 0, "cana": 1}, + "ignore_index": 255 + }, + "target": { + "enabled": True, + "type": "binary", + "num_classes": 2, + "classes": {"background": 0, "target": 1}, + "ignore_index": 255 + }, + } } dadosAtu = ContextoGlobalRedis.get_operacao().get("Atu", {}) contexto = ContextoGlobalRedis.get_contexto() @@ -113,12 +141,39 @@ def load_seg_config(force_reload=False): _CONFIG_CACHE["ia_model_path"] = ContextoGlobalRedis.get_equipamento().get("path_ia_model_ervas") _CONFIG_CACHE["ia_labelmap_path"] = ContextoGlobalRedis.get_equipamento().get("path_ia_labelmap_ervas") _CONFIG_CACHE["ia_norm_stats_path"] = ContextoGlobalRedis.get_equipamento().get("path_ia_norm_stats_ervas") + _CONFIG_CACHE["ia_module_params_path"] = ContextoGlobalRedis.get_equipamento().get("path_ia_module_params_ervas") _CONFIG_CACHE["ia_backbone"] = ContextoGlobalRedis.get_equipamento().get("ia_backbone_ervas") - _CONFIG_CACHE["faixa_atuacao_bicos"] = dadosAtu.get("percent_vertical_deteccao", 0.7) + _CONFIG_CACHE["faixa_atuacao_bicos"] = dadosAtu.get("percent_vertical_deteccao", 0.7) _CONFIG_CACHE["area_atuacao_bicos"] = dadosAtu.get("height_area_deteccao", 0.1) _CONFIG_CACHE["min_frac_erva_por_bico_on"] = dadosAtu.get("pct_erva_bico_on", 0.02) _CONFIG_CACHE["min_frac_erva_por_bico_off"] = dadosAtu.get("pct_erva_bico_off", 0.01) + + # ============================================================ + # Compatibilidade MultiSpecSegformerService + # ============================================================ + input_channels = _CONFIG_CACHE.get("ia_input_channels", ["R", "G", "B", "RE", "NIR"]) + if isinstance(input_channels, str): + input_channels = [c.strip().upper() for c in input_channels.split(",") if c.strip()] + else: + input_channels = [str(c).upper() for c in input_channels] + + _CONFIG_CACHE["input_channels"] = input_channels + _CONFIG_CACHE["channels"] = int(_CONFIG_CACHE.get("ia_channels") or len(input_channels)) + + if _CONFIG_CACHE["channels"] != len(input_channels): + mostrar_log( + f"[WARN] ia_channels={_CONFIG_CACHE['channels']} diferente de " + f"len(input_channels)={len(input_channels)}. Usando len(input_channels)." + ) + _CONFIG_CACHE["channels"] = len(input_channels) + + _CONFIG_CACHE["backbone"] = _CONFIG_CACHE.get("ia_backbone") or "nvidia/mit-b1" + _CONFIG_CACHE["ckpt"] = _CONFIG_CACHE.get("ia_model_path") + _CONFIG_CACHE["norm_stats_path"] = _CONFIG_CACHE.get("ia_norm_stats_path") + _CONFIG_CACHE["module_calibration_json"] = (_CONFIG_CACHE.get("ia_module_params_path")) + _CONFIG_CACHE["camera_fps"] = int(_CONFIG_CACHE.get("fps")) + return _CONFIG_CACHE def reload_seg_config(): diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/weed_worker/weed_detector.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/weed_worker/weed_detector.py index 240c8dc7e..eae408a0b 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/weed_worker/weed_detector.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/weed_worker/weed_detector.py @@ -16,15 +16,41 @@ class WeedDetector: resolucao = config.get("ia_resolution") self.color_map = color_map self.classes = classes + self.runtime_mode = str(config.get("runtime_mode", "semantic")).lower() + self.prediction_contract = str(config.get("prediction_contract", "") or "").lower() + if not self.prediction_contract: + if self.runtime_mode in ("target_direct", "direct_target", "target_head", "target", "spray", "operational"): + self.prediction_contract = "target_binary" + else: + self.prediction_contract = "semantic" + if self.prediction_contract in ("target_binary", "binary_target", "target"): + self.classes = {"background": 0, "target": 1} + self.color_map = [ + (30, 30, 30), # background + (255, 70, 30), # target + ] + else: + self.classes = classes + self.color_map = color_map self.resolucao = (resolucao[0], resolucao[1]) - self.color_lut = np.array(self.color_map, np.uint8) + # LUT completa 0..255 em BGR, segura para OpenCV e ignore_id. self.lut = np.zeros((256, 3), dtype=np.uint8) - for i, color in enumerate(color_map): - #self.lut[i] = color - self.lut[i] = (color[2], color[1], color[0]) # converte pra (B, G, R) + + for i, color in enumerate(self.color_map): + if i >= 256: + break + + # color_map vem em RGB. OpenCV/debug/base64 atual usa BGR. + r, g, b = int(color[0]), int(color[1]), int(color[2]) + self.lut[i] = (b, g, r) + IGNORE_ID = 255 self.lut[IGNORE_ID] = (255, 255, 255) + # Mantém compatibilidade com métodos antigos de debug. + # Antes era array curto; agora fica LUT completa também. + self.color_lut = self.lut + self._reiniciar_deteccoes() self.use_mock = False self.img_mock = "C:\\ZendionInc\\agrobot_base\\AgroBase\\AgroBase\\bin\\x64\\Debug\\Operacoes\\25_07_2025_14_39_14\\Cam0\\85_rgb.jpeg" @@ -52,11 +78,39 @@ class WeedDetector: self.ervas_registradas_bico = [set() for _ in range(qtd_bicos)] self.pred_rgb = np.empty((self.resolucao[1], self.resolucao[0], 3), dtype=np.uint8) - # LUTs + # LUTs de interpretação da máscara de entrada. + # semantic: + # 0=chao, 1=cana, 2=erva + # target_binary: + # 0=background, 1=target/alvo pulverizável self._is_weed = np.zeros(256, dtype=bool) - self._is_weed[int(ClassesSegmentacao.ERVA.value)] = True self._is_cane = np.zeros(256, dtype=bool) - self._is_cane[int(ClassesSegmentacao.CANA.value)] = True + + runtime_mode = str(config.get("runtime_mode", getattr(self, "runtime_mode", "semantic"))).lower() + contract = str(config.get("prediction_contract", getattr(self, "prediction_contract", "")) or "").lower() + + if not contract: + if runtime_mode in ("target_direct", "direct_target", "target_head", "target", "spray", "operational"): + contract = "target_binary" + else: + contract = "semantic" + + self.prediction_contract = contract + + if contract in ("target_binary", "binary_target", "target"): + # A máscara já é o alvo final: 1 = pulverizável. + self._is_weed[1] = True + + # Não há classe cana nessa saída. + # O veto por cana fica naturalmente inativo porque mask_cana será tudo False. + self._is_cane[:] = False + + elif contract in ("semantic", "semantic_3class"): + self._is_weed[int(ClassesSegmentacao.ERVA.value)] = True + self._is_cane[int(ClassesSegmentacao.CANA.value)] = True + + else: + raise RuntimeError(f"prediction_contract inválido: {contract}") self._inv_total = 1.0 / (self.resolucao[1] * self.resolucao[0]) @@ -427,7 +481,7 @@ class WeedDetector: except Exception as e: print(f"Erro ao mostrar debug: {e}") - def _mostrar_debug_bicos_overlay(self, overlay_bgr, detections, atuacao_bicos, config=None, show: bool = True): + def _mostrar_debug_bicos_overlay(self, overlay_bgr, detections, atuacao_bicos, config=None, show: bool = True, metricas_perf=None): """ Versão otimizada do debug: recebe o overlay BGR já montado (RGB + segmentação) e só desenha faixa, bicos, bboxes e HUD. @@ -545,6 +599,12 @@ class WeedDetector: (0, 255, 0), 2 ) + + metricas_perf = metricas_perf or {} + fps_infer = float(metricas_perf.get("fps_infer") or 0.0) + infer_ms = float(metricas_perf.get("infer_ms") or 0.0) + infer_gpu_ms = float(metricas_perf.get("infer_gpu_ms") or 0.0) + fps_loop = float(metricas_perf.get("fps_loop") or 0.0) cv2.putText( self._dbg_img, f"Dbg FPS: {dbg_fps:.1f}", @@ -554,13 +614,34 @@ class WeedDetector: (0, 255, 0), 2 ) + cv2.putText( + self._dbg_img, + f"Infer FPS: {fps_infer:.1f} | infer: {infer_ms:.1f}ms", + (10, 90), + cv2.FONT_HERSHEY_SIMPLEX, + 0.75, + (0, 255, 255), + 2 + ) + cv2.putText( + self._dbg_img, + f"Loop FPS: {fps_loop:.1f} | GPU: {infer_gpu_ms:.1f}ms", + (10, 120), + cv2.FONT_HERSHEY_SIMPLEX, + 0.75, + (0, 255, 255), + 2 + ) if (show): cv2.imshow("Debug Weed Worker", self._dbg_img) cv2.waitKey(1) + return self._dbg_img + except Exception as e: print(f"Erro ao mostrar debug (overlay): {e}") + return None def _fps_update(self, last_ts_attr: str, ema_attr: str, alpha: float = 0.2): """Atualiza e retorna FPS (EMA) baseado no timestamp anterior salvo em self"""