From c805f6103b96e171af2f816796cdce28f2b8fb3e Mon Sep 17 00:00:00 2001 From: Diego Freitas Date: Fri, 30 Jan 2026 16:22:44 -0300 Subject: [PATCH] Implementacao do novo modelo de segmentacao ao visual worker --- .../workers/camera_worker/camera_oak.py | 4 +- .../workers/visual_worker/camera_manager.py | 1341 +++++------------ .../Scripts/workers/visual_worker/config.py | 6 +- .../processamento/segmentacao_semantica.py | 710 ++++----- Python/OAK/datasets/_2_rename_new_images.py | 91 ++ .../datasets/_4_group_images_by_class_raw.py | 3 +- Python/OAK/datasets/_5_augmentation_raw.py | 5 +- Python/OAK/datasets/_6_normalize_raw.py | 3 +- Python/OAK/datasets/_7_split_raw.py | 3 +- Python/OAK/datasets/_8_train_segformer_b3.py | 2 +- Python/OAK/datasets/_9_test_segformer_b3.py | 242 ++- Python/OAK/datasets/config.json | 10 +- .../{config_oak.json => config_gal.json} | 10 +- 13 files changed, 978 insertions(+), 1452 deletions(-) create mode 100644 Python/OAK/datasets/_2_rename_new_images.py rename Python/OAK/datasets/{config_oak.json => config_gal.json} (67%) 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 5d217d4cc..7ddfce1bf 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 @@ -11,7 +11,7 @@ import cv2 GST_LAUNCH = r"C:\Program Files\gstreamer\1.0\msvc_x86_64\bin\gst-launch-1.0.exe" class CameraOak: - def __init__(self, mostrar_log, mx_id, modelo_ia_seg=None, modelo_ia_det=None): + def __init__(self, mostrar_log, mx_id, modelo_ia_seg=None, modelo_ia_det=None, iniciar_imu=False): self.mostrar_log = mostrar_log self.modelo_ia_seg = modelo_ia_seg self.modelo_ia_det = modelo_ia_det @@ -81,7 +81,7 @@ class CameraOak: _camera["modelo"] = self.modelo _camera["dispositivo"] = self.dispositivo.value _camera["tem_depht"] = self.tem_depth - _camera["tem_imu"] = self.tem_imu + _camera["tem_imu"] = iniciar_imu and self.tem_imu # Criar processo para transmissao de video try: 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 518ef785b..c22826370 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 @@ -12,6 +12,7 @@ from visual_worker.processamento.analise_solo import AnaliseSoloManager from visual_worker.processamento.analise_anomalias import AnaliseAnomaliasManager from visual_worker.processamento.radar_top_down import Radar2DManager from visual_worker.processamento.segmentacao_semantica import ClassesSegmentacao, SegmentacaoManager +from visual_worker.processamento.segformer_runner import SegformerNavRunner from visual_worker.processamento.costmap_fuser import CostmapFuser, unpack_snapshot from shared.enums import StatusModulo, T_Code, CameraFrameType from shared.utils import analisar_linhas_por_profundidade, decode_image_base64, encode_image_base64, fazer_overlay, get_velocidade_atual_ms @@ -50,12 +51,11 @@ class CameraManager: if not _camera_conectada: return - self.iniciando = True if mx_id == None: - self.iniciando = False #self.mostrar_log(f"❌ Camera não definida.") return - + + self.iniciando = True self.mx_id = mx_id from visual_worker.config import load_seg_config, load_det_config @@ -63,33 +63,29 @@ class CameraManager: det_config = load_det_config() try: - nova = CameraOak(self.mostrar_log, mx_id, modelo_ia_seg=seg_config, modelo_ia_det=det_config) + nova = CameraOak(self.mostrar_log, mx_id, modelo_ia_seg=None, modelo_ia_det=det_config) 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 return + if self.camera is None: self.mostrar_log(f"❌ Camera com ID {mx_id} não iniciada.") else: self.mostrar_log(f"📷 Camera visual selecionada: {self.camera.modelo} - {self.camera.mx_id}") self.largura_robo_m = ContextoGlobalRedis.get(CtxKey.DadosEquipamento, {}).get("largura", 0.85) - self.grid_ref_shape = (15, 10) - self.grid_ref = self._gerar_grid_referencia_geometrico() - self.depth_referencia = None - self.setores_referencia = None - self.anomalias_manager = AnaliseAnomaliasManager() - self.solo_manager = AnaliseSoloManager() - self.segmentacao_manager = SegmentacaoManager(self.camera.modelo_ia_seg.get("colormap_rgb"), self.camera.modelo_ia_seg.get("classes")) - self.radar_manager = Radar2DManager() - self.data_fuser = CostmapFuser(grid_shape=self.grid_ref_shape, K=3, M=2, fuse_method="max", block_thr=0.7, 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.operante = True self._timestamp_analise = None - self._ultima_analise_anomalias = {} - self._ultima_analise_solo = {} - self._ultima_analise_radar = {} + self.grid_ref_shape = (15, 10) + self.grid_ref = self._gerar_grid_referencia_geometrico() + self.data_fuser = CostmapFuser(grid_shape=self.grid_ref_shape, K=3, M=2, fuse_method="max", block_thr=0.7, 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) + self._ultima_analise_segmentacao = {} self._ultima_analise_deteccao = {} self._ultima_analise_matriz_confianca = {} @@ -97,15 +93,13 @@ class CameraManager: self._ts_segmentacao_anterior = 0 self._ts_deteccao_anterior = 0 self._ultimo_rgb_frame = None - self._depth_frame_necessario = True + self._ultimo_depth_frame = None self._rgb_frame_necessario = True + self._depth_frame_necessario = True self._nova_segmentacao_disponivel = False self._nova_deteccao_disponivel = False self._nova_grid_conf_disponivel = False - self._analisando_anomalias = False - self._analisando_solo = False - self._analisando_radar = False self._analisando_matriz_custo = False self._analisando_matriz_confianca = False self._analisando_segmentacao = False @@ -184,9 +178,13 @@ class CameraManager: return None, None, None try: - predictions, res = self.camera.requisitar_segmentacao() + #predictions, res = self.camera.requisitar_segmentacao() + #if predictions is not None: + # return predictions, self.camera.timestamp_ultima_segmentacao, res + rgb_frame, _ts, res = self.get_rgb_frame() + predictions, ts, roi_resized, (y_fim, y_inicio) = self.seg_runner.infer_ids(rgb_frame) if predictions is not None: - return predictions, self.camera.timestamp_ultima_segmentacao, res + return predictions, ts, res elif "X_LINK_ERROR" in res["erro"]: self.reiniciar_status() except Exception as e: @@ -295,7 +293,6 @@ class CameraManager: 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() @@ -305,54 +302,10 @@ class CameraManager: depth_frame_np, depth_timestamp, depth_res = self.get_depth_frame() else: depth_frame_np = self._ultimo_depth_frame - parametros_camera = self.camera.parametros - fov_h = parametros_camera["fov_h"] - distancia_max_m = parametros_camera["distancia_maxima"] / 1000.0 - self._analise_matriz_confianca(depth_frame_np, distancia_max_m, fov_h) + + self._analise_matriz_confianca(depth_frame_np) - def _realizar_analises_async(self): - executor = self._pool - tarefas = [] - - parametros_camera = self.camera.parametros - fov_h = parametros_camera["fov_h"] - distancia_max_m = parametros_camera["distancia_maxima"] / 1000.0 - percentual_solo = parametros_camera["percentual_altura_solo"] / 100.0 - - if self._depth_frame_necessario: - depth_frame_np, depth_timestamp, depth_res = self.get_depth_frame() - else: - depth_frame_np = self._ultimo_depth_frame - - #tarefas.append(executor.submit(self._analise_radar, depth_frame_np, fov_h, distancia_max_m)) - - if True or not self._nova_segmentacao_disponivel: - if not self._analisando_segmentacao: - tarefas.append(executor.submit(self._analise_segmentacao)) - - if self._nova_segmentacao_disponivel: - if not self._analisando_matriz_confianca: - self._nova_segmentacao_disponivel = False - tarefas.append(executor.submit(self._analise_matriz_confianca, depth_frame_np, distancia_max_m, fov_h)) - - if False and self._nova_grid_conf_disponivel: - limiar_conf: float = 0.4 - matriz_conf = self._ultima_analise_matriz_confianca["matriz"] - if not self._analisando_anomalias: - self._nova_grid_conf_disponivel = False - limiar_delta = calcular_threshold_anomalias() - largura_min: float = 0.15 - altura_min: float = 0.15 - tarefas.append(executor.submit(self._analise_anomalias, matriz_conf, limiar_delta, limiar_conf, distancia_max_m, largura_min, altura_min)) - if not self._analisando_solo: - self._nova_grid_conf_disponivel = False - tarefas.append(executor.submit(self._analise_solo, matriz_conf, percentual_solo, limiar_conf, fov_h)) - if not self._analisando_matriz_custo: - self._nova_grid_conf_disponivel = False - tarefas.append(executor.submit(self._analise_matriz_custo, matriz_conf, fov_h)) - - def _analise_segmentacao(self): if self._analisando_segmentacao: return self._analisando_segmentacao = True @@ -362,7 +315,7 @@ class CameraManager: if ts == self._ts_segmentacao_anterior: return self._ts_segmentacao_anterior = ts if predictions is not None: - rgb_frame, _ts, res = self.get_rgb_frame() + #rgb_frame, _ts, res = self.get_rgb_frame() analise_segmentacao, log = self.segmentacao_manager.segmentar(predictions) t1 = time.time() if analise_segmentacao == None: self.mostrar_log(log) @@ -378,9 +331,7 @@ class CameraManager: from visual_worker.config import load_seg_config if load_seg_config().get("debug_visual", False): - #key, vis = self.debug_show_costmap(rgb_frame=self._ultimo_rgb_frame, grid_dict=self._ultima_analise_matriz_confianca, grid_shape=self.grid_ref_shape, window_name="viz MPC", wait=1, text_mode="mini") - #key, vis = self.debug_show_visualworker(frame_bgr=self._ultimo_rgb_frame, grid=self._ultima_analise_matriz_confianca, wait=1, text_mode="full", draw_grid=True, draw_cells=True, draw_legend=True) - self.segmentacao_manager.display_segmentation_debug(self._ultimo_rgb_frame, 150) + self.segmentacao_manager.display_segmentation_debug(self._ultimo_rgb_frame, self.largura_robo_m, self.grid_ref) # 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) @@ -394,47 +345,6 @@ class CameraManager: self._analisando_segmentacao = False #self.mostrar_log(f"Segmentacao concluida em {self._ultima_analise_segmentacao['latencia']:.4f} s, a {fps:.4f} FPS") - def _analise_matriz_confianca(self, depth_frame_np, dist_max, fov_h): - 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.get("bboxes") - - vel = get_velocidade_atual_ms() - - t0 = time.time() - #grid_conf = self._gerar_grid_confianca(depth_frame_np, segmentacao, dist_max) - grid_conf = self._construir_grid_confianca(depth_frame_np, segmentacao, self.grid_ref, self.grid_ref_shape, self.camera.modelo_ia_seg.get("classes"), deteccoes=deteccoes) - #self.mostrar_log(grid_conf) - 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) - self._ultima_analise_matriz_confianca = grid_conf - #self._ultima_analise_segmentacao["corredor_perfil"] = self.segmentacao_manager.calcular_perfil_corredor(matriz, fov_h) - ContextoGlobalRedis.atualizar_ctx_dict( - CtxKey.DadosVisualWorker, - ts_analise=time.time(), - matriz_confianca=snapshot - #perfil_corredor__segmentacao=converter_valores_numpy(self._ultima_analise_segmentacao.get("corredor_perfil", [])) - ) - self._nova_grid_conf_disponivel = True - #self._mostrar_debug_grid_confianca(self._ultimo_rgb_frame, grid_conf["matriz"], True, self._ultima_analise_segmentacao["mask_color"]) - #key, vis = self.debug_show_visualworker(frame_bgr=self._ultimo_rgb_frame, grid=grid_conf, wait=1, text_mode="full", draw_grid=True, draw_cells=True, draw_legend=True) - - from visual_worker.config import load_seg_config - if load_seg_config().get("debug_visual", False): - vis, metrics = self.debug_blockage_imshow(self._ultimo_rgb_frame, snapshot, velocidade_media=vel) - 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 _analise_deteccao(self): if self._analisando_deteccao or self.camera.modelo_ia_det is None: return self._analisando_deteccao = True @@ -467,464 +377,230 @@ class CameraManager: self._analisando_deteccao = False #self.mostrar_log(f"Deteccao concluida em {self._ultima_analise_deteccao['latencia']:.4f} s, a {fps:.4f} FPS") - def _analise_anomalias(self, grid_conf, limiar_delta, limiar_conf, dist_max, largura_min, altura_min): - if self._analisando_anomalias: - return - self._analisando_anomalias = True + def _overlay_deteccoes( + self, + rgb_frame, + dets, + conf_thr=0.5, # limiar de confiança pra desenhar + roi_frac=None, # (rx1,ry1,rx2,ry2) normalizado da ROI usada no detector (ex.: (0.0,y1,1.0,y2)) + show=True, # se True, faz cv2.imshow + janela="det", # nome da janela + fps_state=None, # dict estado do FPS (persistido fora), ex.: {} + ): + """ + dets: lista de dicts no formato: + { + "label_id": int, + "label": str|None, + "conf": float, + "bbox_norm": [x0,y0,x1,y1] # 0..1 relativo ao input do detector (na ROI) + # opcional: "bbox_full": [x0,y0,x1,y1] em px do frame completo + } + Retorna: (frame_com_overlay, fps_state, keep_loop_bool) + """ try: - t0 = time.time() - #analise_anomalias = self.anomalias_manager.detectar_anomalias(depth_frame, self.depth_referencia, limiar, distancia_max, largura_min, altura_min, parametros_camera) - #analise_anomalias = self.anomalias_manager.analisar_anomalias(depth_frame_np, self.depth_referencia, limiar, distancia_max_m, largura_min, altura_min) - analise_anomalias = self.anomalias_manager.analisar_anomalias_grid(grid_conf, limiar_delta, limiar_conf, (640, 480), dist_max, largura_min, altura_min) - t1 = time.time() - analise_anomalias["ultima_chamada"] = self._ultima_analise_anomalias.get("ultima_chamada", t0) - self._calcular_performance(t0, t1, analise_anomalias) - self._ultima_analise_anomalias = analise_anomalias - ContextoGlobalRedis.atualizar_ctx_dict( - CtxKey.DadosVisualWorker, - ts_analise=time.time(), - anomalias__deteccoes=converter_valores_numpy(self._ultima_analise_anomalias["deteccoes"]) - ) - except Exception as e: - self.mostrar_log(f"❌ Erro ao analisar anomalias: {e}") - finally: - self._analisando_anomalias = False - #self.mostrar_log("Analise de anomalias concluida") + img = cv2.resize(rgb_frame.copy(), (1280, 720)) + H, W = img.shape[:2] - def _analise_solo(self, grid_conf, percentual_solo, limiar_conf, fov_h): - if self._analisando_solo: - return - self._analisando_solo = True - try: - t0 = time.time() - analise_solo = self.solo_manager.analisar_solo(grid_conf, percentual_solo, limiar_conf, fov_h) - t1 = time.time() - analise_solo["ultima_chamada"] = self._ultima_analise_solo.get("ultima_chamada", t0) - self._calcular_performance(t0, t1, analise_solo) - self._ultima_analise_solo = analise_solo - ContextoGlobalRedis.atualizar_ctx_dict( - CtxKey.DadosVisualWorker, - ts_analise=time.time(), - solo=converter_valores_numpy(self._ultima_analise_solo) - ) - except Exception as e: - self.mostrar_log(f"❌ Erro ao analisar solo: {e}") - finally: - self._analisando_solo = False - #self.mostrar_log("Analise do solo concluida") + # paleta simples por classe + palette = [ + (255, 56, 56), (255, 157, 151), (72, 249, 10), (0, 255, 0), (0, 0, 255), + (255, 0, 255), (0, 255, 255), (255, 191, 0), (52, 148, 230), (147, 112, 219) + ] - def _analise_radar(self, depth_frame_np, fov_h, dist_max): - if self._analisando_radar: - return - self._analisando_radar = True + def _map_bbox_norm_to_full(bn): + # bn é [x0n,y0n,x1n,y1n] relativo ao input da ROI (0..1) + x0n, y0n, x1n, y1n = bn + if roi_frac is not None: + rx1, ry1, rx2, ry2 = roi_frac + sx, sy = (rx2 - rx1), (ry2 - ry1) + x0 = int(round((rx1 + x0n * sx) * W)) + y0 = int(round((ry1 + y0n * sy) * H)) + x1 = int(round((rx1 + x1n * sx) * W)) + y1 = int(round((ry1 + y1n * sy) * H)) + else: + x0 = int(round(x0n * W)) + y0 = int(round(y0n * H)) + x1 = int(round(x1n * W)) + y1 = int(round(y1n * H)) + # clamp + x0 = max(0, min(W - 1, x0)); x1 = max(0, min(W - 1, x1)) + y0 = max(0, min(H - 1, y0)); y1 = max(0, min(H - 1, y1)) + return x0, y0, x1, y1 + + def _put_label(img, text, x, y, bg): + (tw, th), _ = cv2.getTextSize(text, cv2.FONT_HERSHEY_SIMPLEX, 0.5, 1) + cv2.rectangle(img, (x, max(0, y - th - 6)), (x + tw + 6, y), bg, -1) + cv2.putText(img, text, (x + 3, y - 4), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 0), 1, cv2.LINE_AA) + + # desenhar ROI (opcional, ajuda debug) + if roi_frac is not None: + rx1, ry1, rx2, ry2 = roi_frac + x0r, y0r = int(rx1 * W), int(ry1 * H) + x1r, y1r = int(rx2 * W), int(ry2 * H) + cv2.rectangle(img, (x0r, y0r), (x1r, y1r), (60, 60, 60), 1) + + # desenhar detecções + for d in dets: + if d.get("conf", 0.0) < conf_thr: + continue + + # bbox em px do frame + if "bbox_full" in d and d["bbox_full"]: + x0, y0, x1, y1 = d["bbox_full"] + # clamp se necessário + x0 = max(0, min(W - 1, int(x0))); x1 = max(0, min(W - 1, int(x1))) + y0 = max(0, min(H - 1, int(y0))); y1 = max(0, min(H - 1, int(y1))) + else: + bn = d.get("bbox_norm", None) + if not bn: + continue + x0, y0, x1, y1 = _map_bbox_norm_to_full(bn) + + if x1 <= x0 or y1 <= y0: + continue + + lid = int(d.get("label_id", -1)) + color = palette[lid % len(palette)] if lid >= 0 else (0, 255, 0) + + cv2.rectangle(img, (x0, y0), (x1, y1), color, 2) + + name = d.get("label", None) + txt = f"{name or f'id:{lid}'} {d.get('conf', 0.0):.2f}" + _put_label(img, txt, x0, y0, color) + + # FPS (EMA) + now = time.monotonic() + if fps_state is None: + fps_state = {} + t_prev = fps_state.get("t_prev") + fps_ema = fps_state.get("fps_ema") + if t_prev is not None: + dt = now - t_prev + if dt > 0: + fps_inst = 1.0 / dt + alpha = 0.90 + fps_ema = fps_inst if fps_ema is None else (alpha * fps_ema + (1 - alpha) * fps_inst) + fps_state["t_prev"] = now + fps_state["fps_ema"] = fps_ema + + if fps_ema: + cv2.putText(img, f"FPS: {fps_ema:.1f}", (10, 22), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (50, 220, 50), 2, cv2.LINE_AA) + + keep = True + if show: + cv2.imshow(janela, img) + k = cv2.waitKey(1) & 0xFF + keep = (k != 27) # ESC para sair + + return img, fps_state, keep + 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 + 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.get("bboxes") + + vel = get_velocidade_atual_ms() + t0 = time.time() - depth_frame = cp.asarray(depth_frame_np) - analise_radar = self.radar_manager.analisar_radar_2d(depth_frame, fov_h, dist_max) + grid_conf = self._construir_grid_confianca(depth_frame_np, segmentacao, self.grid_ref, self.grid_ref_shape, deteccoes=deteccoes) + #self.mostrar_log(grid_conf) t1 = time.time() - analise_radar["ultima_chamada"] = self._ultima_analise_radar.get("ultima_chamada", t0) - self._calcular_performance(t0, t1, analise_radar) - self._ultima_analise_radar = analise_radar + 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) + self._ultima_analise_matriz_confianca = grid_conf + ContextoGlobalRedis.atualizar_ctx_dict( CtxKey.DadosVisualWorker, ts_analise=time.time(), - anomalias__sombras=converter_valores_numpy(self._ultima_analise_radar.get("analise", {}).get("bboxes_sombra", [])), - perfil_corredor__profundidade=converter_valores_numpy(self._ultima_analise_radar.get("analise", {}).get("corredor_perfil", [])) + matriz_confianca=snapshot ) + self._nova_grid_conf_disponivel = True + + from visual_worker.config import load_seg_config + if load_seg_config().get("debug_visual", False): + vis, metrics = self.debug_blockage_imshow(self._ultimo_rgb_frame, snapshot, velocidade_media=vel) except Exception as e: - self.mostrar_log(f"❌ Erro ao analisar radar 2d: {e}") + self.mostrar_log(f"❌ Erro na geracao da matriz de confianca: {e}") finally: self._depth_frame_necessario = True - self._analisando_radar = False - #self.mostrar_log("Analise de radar concluida") - - def _analise_matriz_custo(self, grid_conf, fov_h): - if self._analisando_matriz_custo: - return - self._analisando_matriz_custo = True - try: - t0 = time.time() - largura_robo = self.largura_robo_m - matriz_custo = self._gerar_matriz_custo_fundida(grid_conf, largura_robo, fov_h) - t1 = time.time() - matriz_custo["ultima_chamada"] = self._ultima_analise_matriz_custo.get("ultima_chamada", t0) - self._calcular_performance(t0, t1, matriz_custo) - self._ultima_analise_matriz_custo = matriz_custo - matriz = matriz_custo["matriz"] - ContextoGlobalRedis.atualizar_ctx_dict( - CtxKey.DadosVisualWorker, - ts_analise=time.time(), - matrizes__custo=converter_valores_numpy(matriz) - ) - except Exception as e: - self.mostrar_log(f"Erro ao gerar matriz de custo: {e}") - finally: - self._analisando_matriz_custo = False - #self.mostrar_log("Matriz de custo concluida") - - - def salvar_frames(self, tipos: list, nome: str, pasta="frames_salvos"): - if not self.operante: - return [] - - try: - os.makedirs(pasta, exist_ok=True) - frames_salvos = [] - - frame_name = datetime.datetime.now().strftime('%Y%m%d_%H%M%S') - if nome is not None and nome != "": - frame_name = nome - - if CameraFrameType.Rgb.value in tipos: - rgb_frame, rgb_ts, rgb_res = self.get_rgb_frame() - if rgb_frame is not None and rgb_res["frame_valido"]: - nome_rgb = f"{frame_name}_rgb.jpeg" - caminho_rgb = os.path.join(pasta, nome_rgb) - cv2.imwrite(caminho_rgb, rgb_frame) - frames_salvos.append(caminho_rgb) - - if CameraFrameType.Heatmap.value in tipos: - heatmap_frame, heatmap_ts, heatmap_res = self.get_heatmap_frame() - if heatmap_frame is not None and heatmap_res["frame_valido"]: - nome_heatmap = f"{frame_name}_heatmap.jpeg" - caminho_heatmap = os.path.join(pasta, nome_heatmap) - cv2.imwrite(caminho_heatmap, heatmap_frame) - frames_salvos.append(caminho_heatmap) - - if CameraFrameType.RadarTopDown.value in tipos: - frame = self._salvar_frame_analise(frame_name, self._ultima_analise_radar, "radar", pasta) - if frame is not None: - frames_salvos.append(frame) - - if CameraFrameType.Segmentacao.value in tipos: - frame = self._salvar_frame_analise(frame_name, self._ultima_analise_segmentacao, "segmentacao", pasta) - if frame is not None: - frames_salvos.append(frame) - - return frames_salvos - except Exception as e: - self.mostrar_log(f"❌ Erro ao salvar frames: {e}") - return [] - - def _salvar_frame_analise(self, frame_name, analise, sulfix, pasta): - base64frame = analise.get("frame", {}).get("frame") - frame = decode_image_base64(base64frame) - if frame is not None: - nome_analise = f"{frame_name}_{sulfix}.jpeg" - caminho_analise = os.path.join(pasta, nome_analise) - cv2.imwrite(caminho_analise, frame) - return caminho_analise - return None - - - def _gerar_matriz_custo_fundida(self, matriz_conf, largura_robo_m, fov_h): - # Valores podem variar de -1.5 ~ 6.0 - try: - altura_grid = len(matriz_conf) - largura_grid = len(matriz_conf[0]) - matriz_custo = [] - linhas_info = analisar_linhas_por_profundidade(matriz_conf, "prof_ref", fov_h) - for i in range(altura_grid): - linha_custo = [] - escala_x = linhas_info[i]["escala_x"] - for j in range(largura_grid): - cel = matriz_conf[i][j] - prof_ref = cel.get("prof_ref", 0.0) - pos_pct = j / largura_grid - # 🟢 Custo Baseado em Segmentação - peso_seg = 2.5 - _is = cel.get("indice_seg_chao", 1.0) - custo_segmentacao = (1.0 - _is) * peso_seg - # 🟡 Custo por Confiabilidade - peso_conf = 2.0 - _ic = cel.get("indice_confiabilidade", 1.0) - custo_confianca = (1.0 - _ic) * peso_conf - # 🔵 Custo por delta de profundidade (suavidade) - peso_delta = 1.0 - delta_max = 1.0 - _delta = abs(max(0, min(cel.get("prof_delta", 0.0), delta_max))) - custo_delta = _delta * peso_delta - # 🟣 Penalidade leve nas bordas (ajuda na centralização) - peso_borda = 0.5 # 0 no centro, peso_borda nos limites - penalidade_borda = peso_borda * (1 - (1 - 2 * abs(pos_pct - 0.5)))**2 - # 🧠 Bonificação por estar no caminho do mapa (adicionamos depois) - bonificacao_mapa = 0.0 - custo_total = ( - custo_segmentacao + - custo_confianca + - custo_delta + - penalidade_borda + - bonificacao_mapa # negativa quando for aplicada - ) - cel_custo = { - "linha": i, - "coluna": j, - "custo": custo_total, - "distancia_m": prof_ref, - "escala_x": escala_x, - "indice_seg_chao": _is, - "indice_confiabilidade": _ic, - "prof_delta": _delta, - "em_caminho": False - } - linha_custo.append(cel_custo) - matriz_custo.append(linha_custo) - - # 🔵 Após montar, adiciona bonificação do caminho do mapa (se houver) - pontos_xy = ContextoGlobalRedis.get_operacao().get("pontos_mapa", []) - #print(f"Pontos do mapa: {len(pontos_xy)}") - if len(pontos_xy) > 0: - _gps = ContextoGlobalRedis.get_modulo(T_Code.Gps) - lat_atual = _gps.get("lat", 0) - lon_atual = _gps.get("lon", 0) - theta_robo = np.radians(_gps.get("theta", 0.0)) - - # Converte usando mesmo ponto de referência do mapa - p_ref = ContextoGlobalRedis.get_operacao().get("ponto_mapa_ref", (0, 0)) - gps_handler = GPSHandler(p_ref[0], p_ref[1]) - x_robo, z_robo = gps_handler.converter_latlon_para_xz(lat_atual, lon_atual) - - idx_proximo = ContextoGlobalRedis.get_contexto().get("Trajetoria", {}).get("idx_proximo_ponto", 0) - pontos_plotar = [] - distancia_max = ContextoGlobalRedis.get_operacao().get("Snr", {}).get("distancia_maxima", 5000) / 1000.0 - - #print(f"idx_proximo: {idx_proximo}, theta: {round(np.degrees(theta_robo), 2)}") - - for i in range(idx_proximo, len(pontos_xy)): - p = pontos_xy[i] - x_mapa, z_mapa = p["xy"] - - x_rel, z_rel = gps_handler.converter_para_referencial_robo(x_mapa, z_mapa, x_robo, z_robo, theta_robo) - #print(f"Ponto: {i}, x_rel: {x_rel}, z_rel: {z_rel}, x_robo: {x_robo}, z_robo: {z_robo}") - if z_rel <= 0: - continue # está atrás do robô, ignora - - dist = np.sqrt(x_rel**2 + z_rel**2) - #print(f"Distancia: {dist}") - if dist <= distancia_max: - # Substitui ponto original por ponto no referencial do robô - p_relativo = { - "xy": (x_rel, z_rel), - "tipo": p.get("tipo", 3), - "distanciaMargem": p.get("distanciaMargem", 0.7) - } - pontos_plotar.append(p_relativo) - else: - break # parou de estar no alcance da câmera - - #print(f"Pontos no radar: {len(pontos_plotar)}") - matriz_custo = self._aplicar_bonus_caminho(matriz_custo, pontos_plotar, largura_robo_m) - - return { - "timestamp": time.time(), - "matriz": matriz_custo - } - except Exception as e: - self.mostrar_log(f"❌ Erro ao gerar matriz de custo: {e}") - return { - "timestamp": time.time(), - "matriz": [[]] - } - - def _aplicar_bonus_caminho(self, matriz_custo, caminho_xy, largura_robo_m=0.85, margem_segurança_m=0.1): - try: - altura_grid = len(matriz_custo) - largura_grid = len(matriz_custo[0]) - faixa_total = largura_robo_m + margem_segurança_m # largura total da faixa bonificada - for ponto in caminho_xy: - x_m, z_m = ponto["xy"][0], ponto["xy"][1] - #print(f"x_m: {round(x_m,2)}, z_m: {round(z_m,2)}") - for i in range(altura_grid): - for j in range(largura_grid): - cel = matriz_custo[i][j] - escala_x = cel["escala_x"] - centro_j = (largura_grid - 1) / 2 # funciona para par e ímpar - x_cel = (j - centro_j) * escala_x - z_cel = cel["distancia_m"] - dist_lateral = abs(x_cel - x_m) - dist_frontal = abs(z_cel - z_m) - # 🔹 Bonifica as células dentro da largura do robô + margem - if dist_frontal < 0.5 and dist_lateral <= faixa_total / 2: - cel["custo"] -= 1.5 # bônus - cel["em_caminho"] = True - return matriz_custo - except Exception as e: - self.mostrar_log(f"❌ Erro ao aplicar bonus de caminho: {e}") - return matriz_custo - - def _mostrar_debug_matriz_custo_fundida(self, rgb_frame: np.ndarray, matriz_custo: list, exibir_debug: bool = False): - try: - if not exibir_debug: - return - - img_debug = cv2.resize(rgb_frame.copy(), (1280, 720)) - overlay = img_debug.copy() - - altura, largura, _ = img_debug.shape - grid_h = len(matriz_custo) - grid_w = len(matriz_custo[0]) - - if grid_h == 0 or grid_w == 0: - return - - h_step = altura // grid_h - w_step = largura // grid_w - - todos_custos = [cel["custo"] for linha in matriz_custo for cel in linha] - custo_min = min(todos_custos) - custo_max = max(todos_custos) - delta_custo = custo_max - custo_min if custo_max != custo_min else 1.0 - - mostrar_texto = grid_h <= 20 and grid_w <= 20 - - for i in range(grid_h): - for j in range(grid_w): - cel = matriz_custo[i][j] - y0, y1 = i * h_step, (i + 1) * h_step - x0, x1 = j * w_step, (j + 1) * w_step - - custo = cel["custo"] - norm = (custo - custo_min) / delta_custo - r = int(255 * norm) - g = int(255 * (1.0 - norm)) - cor = (0, g, r) - - cv2.rectangle(overlay, (x0, y0), (x1, y1), cor, -1) - - if cel.get("em_caminho", False): - cv2.rectangle(overlay, (x0, y0), (x1, y1), (255, 100, 0), 1) # contorno azul - - if mostrar_texto: - pos_x, pos_y = x0 + 2, y0 + 12 - cv2.putText(overlay, f"{custo:.2f}", (pos_x, pos_y), cv2.FONT_HERSHEY_SIMPLEX, 0.35, (255, 255, 255), 1) - - # Aplica overlay com transparência - alpha = 0.4 - cv2.addWeighted(overlay, alpha, img_debug, 1 - alpha, 0, img_debug) - - # Linha central do robô - cv2.line(img_debug, (largura // 2, 0), (largura // 2, altura), (150, 150, 150), 1) - - # Se tiver ponto do caminho, desenha seta direcional - try: - pontos_xy = ContextoGlobalRedis.get_operacao().get("pontos_mapa", []) - _gps = ContextoGlobalRedis.get_modulo(T_Code.Gps) - if len(pontos_xy) > 0 and _gps: - lat, lon = _gps.get("lat", 0), _gps.get("lon", 0) - theta_robo = np.radians(_gps.get("theta", 0.0)) - from shared.gps_handler import GPSHandler - p_ref = ContextoGlobalRedis.get_operacao().get("ponto_mapa_ref", (0, 0)) - handler = GPSHandler(p_ref[0], p_ref[1]) - x_robo, z_robo = handler.converter_latlon_para_xz(lat, lon) - - # Pega o próximo ponto (idx já considerado pelo seu sistema) - idx_proximo = ContextoGlobalRedis.get_contexto().get("Trajetoria", {}).get("idx_proximo_ponto", 0) - #idx_proximo -= 1 - if idx_proximo < len(pontos_xy): - p = pontos_xy[idx_proximo]["xy"] - dx, dz = p[0] - x_robo, p[1] - z_robo - x_rel, z_rel = handler.converter_para_referencial_robo(p[0], p[1], x_robo, z_robo, theta_robo) - - angulo = np.arctan2(x_rel, z_rel) # com x e z invertidos, pois Z vai para cima - - # Posição da seta no topo da imagem - centro_x = largura // 2 - centro_y = 50 - raio = 50 # tamanho da seta - destino_x = int(centro_x + raio * np.sin(angulo)) - destino_y = int(centro_y - raio * np.cos(angulo)) - - cv2.arrowedLine(img_debug, (centro_x, centro_y), (destino_x, destino_y), (255, 0, 255), 2, tipLength=0.3) - cv2.circle(img_debug, (centro_x, centro_y), 4, (255, 0, 255), -1) - - pos_x, pos_y = centro_x - 20, centro_y + 12 - cv2.putText(img_debug, f"{idx_proximo}, {np.degrees(angulo):.2f}, {z_rel:.2f} m", (pos_x, pos_y), cv2.FONT_HERSHEY_SIMPLEX, 0.35, (255, 0, 255), 1) - except Exception as e: - self.mostrar_log(f"❌ Erro ao desenhar seta de direção: {e}") - cv2.imshow("Matriz de Custo (Debug)", img_debug) - cv2.waitKey(1) - except Exception as e: - self.mostrar_log(f"❌ Erro ao mostrar debug da matriz de custo: {e}") - - + self._analisando_matriz_confianca = False + #self.mostrar_log("Matriz de confianca concluida") def _construir_grid_confianca( self, - depth_mm, # np.ndarray (H,W) em milímetros (0/NaN = inválido) - seg_ids_512x288, # np.ndarray (288,512) + depth_mm, # np.ndarray (H,W) em milímetros (0/NaN = inválido) ou None se não usar depth + seg_ids, # np.ndarray (H1,W1) com ids de classe grid_ref, # np.ndarray (grid_h,) OU (grid_h, grid_w) em metros grid_shape=(15, 10), # (cols=15, rows=10) - class_ids=None, # {'rua':X, 'cana':Y, 'obs':Z} valid_mm=(300, 10000), - range_m=(0.5, 5.0), min_valid_frac=0.30, conf_params=(0.30, 0.80),# t0,t1 p/ mapear %depth_valido -> conf_depth w=(0.6, 0.3, 0.1), # pesos do custo: classe, anom, (1-conf) anom_tau_min=0.22, anom_satur_m=0.50, - # -------- NOVO: detecções -------- + # -------- detecções -------- deteccoes=None, # lista de dicts: {label_id,label,conf,bbox_norm:[x0n,y0n,x1n,y1n],bbox_px:[x0,y0,x1,y1]} - det_params=None # dict com hiperparâmetros (ver defaults abaixo) + det_params=None, # dict com hiperparâmetros (ver defaults abaixo) + usar_depth: bool = True # NOVO: se False, ignora depth e anomalia ): """ Retorna dict com arrays (grid_h, grid_w): - pct_rua, pct_cana, pct_obs, z_med, z_ref, depth_valid_frac, - conf, anom, custo, navegavel, - # --- NOVOS (debug/uso opcional) --- - det_cov_max, det_conf_max, det_score + pct_navegavel, pct_nao_navegavel, z_med, z_ref, depth_valid_frac, + conf, anom, custo, navegavel, + det_cov_max, det_conf_max, det_score, det_top_label_id, det_top_conf + + Se usar_depth=False, os campos z_med/depth_valid_frac/anom serão pouco informativos + (z_med NaN, depth_valid_frac=0, anom=0) e a confiança/conf/custo + serão baseados apenas em segmentação + detecções. """ try: - if class_ids is None: - class_ids = {'rua': 0, 'cana': 1, 'obs': 2} - # ---------- defaults detecção ---------- _det = { - # peso da penalização no custo - "w4": 0.25, # quão forte a detecção pesa no custo (0..1) - # limiar pra "bloquear" navegação só por detecção - "thr_det_block": 0.35, # se det_score >= isso, célula deixa de ser navegável - # mínimo de interseção da bbox com a célula pra considerar (fração da célula) + "w4": 0.25, # quão forte a detecção pesa no custo (0..1) + "thr_det_block": 0.35, # se det_score >= isso, célula deixa de ser navegável "min_cell_coverage": 0.10, - # confiança mínima da bbox pra considerar "min_det_conf": 0.35, - # pesos por classe (se não souber o id, usa 1.0) - "class_weights": {}, # ex: {'person':1.0,'car':0.9,'dog':0.6} - # classes que vetam (tratadas como peso 1.0 e sem atenuação) + "class_weights": {}, # ex: {'person':1.0,'car':0.9,'dog':0.6} "veto_labels": set(["person"]), - # como combinar múltiplas bboxes na célula: "max" ou "sum_clamped" - "combine": "max", - # derrubar um pouco a conf_cell quando há detecção - "conf_drop_alpha": 0.15, # 0 = não derruba; 0.15 = derruba 15% * det_score + "combine": "max", # "max" ou "sum_clamped" + "conf_drop_alpha": 0.15, # 0 = não derruba; 0.15 = derruba 15% * det_score } if det_params: _det.update(det_params) grid_w, grid_h = grid_shape - H0, W0 = depth_mm.shape + H1, W1 = seg_ids.shape - # --- 1) Resize depth para 512x288 --- - d_small = cv2.resize(depth_mm, (512, 288), interpolation=cv2.INTER_NEAREST).astype(np.float32) - d_small[(d_small < valid_mm[0]) | (d_small > valid_mm[1])] = np.nan + # --- 1) Depth: opcional --- + d_small = None + if usar_depth and depth_mm is not None: + d_small = cv2.resize(depth_mm, (W1, H1), interpolation=cv2.INTER_NEAREST).astype(np.float32) + d_small[(d_small < valid_mm[0]) | (d_small > valid_mm[1])] = np.nan # --- 2) Bordas da grid --- - x_edges = np.linspace(0, 512, grid_w + 1, dtype=int) - y_edges = np.linspace(0, 288, grid_h + 1, dtype=int) + x_edges = np.linspace(0, W1, grid_w + 1, dtype=int) + y_edges = np.linspace(0, H1, grid_h + 1, dtype=int) - # --- 3) Saídas --- - pct_rua = np.zeros((grid_h, grid_w), np.float32) - pct_cana = np.zeros((grid_h, grid_w), np.float32) - pct_obs = np.zeros((grid_h, grid_w), np.float32) - z_med = np.full((grid_h, grid_w), np.nan, np.float32) - depth_valid_frac = np.zeros((grid_h, grid_w), np.float32) + # --- 3) Saídas base --- + pct_navegavel = np.zeros((grid_h, grid_w), np.float32) + pct_nao_navegavel = np.zeros((grid_h, grid_w), np.float32) + z_med = np.full((grid_h, grid_w), np.nan, np.float32) + depth_valid_frac = np.zeros((grid_h, grid_w), np.float32) # --- 3b) mapas da detecção (debug/uso) --- - det_cov_max = np.zeros((grid_h, grid_w), np.float32) # cobertura máxima (0..1) - det_conf_max = np.zeros((grid_h, grid_w), np.float32) # conf máx (0..1) - det_score = np.zeros((grid_h, grid_w), np.float32) # score combinado (0..1) - det_top_label_id = -np.ones((grid_h, grid_w), np.int32) # -1 = nenhuma - det_top_conf = np.zeros((grid_h, grid_w), np.float32) # conf da dominante + det_cov_max = np.zeros((grid_h, grid_w), np.float32) # cobertura máxima (0..1) + det_conf_max = np.zeros((grid_h, grid_w), np.float32) # conf máx (0..1) + det_score = np.zeros((grid_h, grid_w), np.float32) # score combinado (0..1) + det_top_label_id = -np.ones((grid_h, grid_w), np.int32) # -1 = nenhuma + det_top_conf = np.zeros((grid_h, grid_w), np.float32) # --- 4) grid_ref 2D --- if grid_ref.ndim == 1: @@ -936,47 +612,49 @@ class CameraManager: if Z_ref.shape != (grid_h, grid_w): raise ValueError(f"grid_ref 2D deve ser {(grid_h, grid_w)}, veio {Z_ref.shape}") - # pré-slices por linha + # --- 5) Loop por célula: seg + (depth se ativo) --- for j in range(grid_h): - y0, y1 = y_edges[j], y_edges[j+1] - seg_row = seg_ids_512x288[y0:y1, :] - depth_row = d_small[y0:y1, :] - row_h = max(1, y1 - y0) + y0, y1 = int(y_edges[j]), int(y_edges[j+1]) + seg_row = seg_ids[y0:y1, :] + depth_row = d_small[y0:y1, :] if d_small is not None else None + for i in range(grid_w): - x0, x1 = x_edges[i], x_edges[i+1] - col_w = max(1, x1 - x0) - - seg_block = seg_row[:, x0:x1] - depth_block = depth_row[:, x0:x1] + x0, x1 = int(x_edges[i]), int(x_edges[i+1]) + seg_block = seg_row[:, x0:x1] n = seg_block.size if n == 0: continue # % por classe - n_rua = np.count_nonzero(seg_block == ClassesSegmentacao.RUA.value) - n_cana = np.count_nonzero(seg_block == ClassesSegmentacao.CANA.value) - n_obs = np.count_nonzero(seg_block == ClassesSegmentacao.OBSTACULO.value) - pct_rua[j, i] = n_rua / n - pct_cana[j, i] = n_cana / n - pct_obs[j, i] = n_obs / n + n_nav = np.count_nonzero(seg_block == ClassesSegmentacao.NAVEGAVEL.value) + n_naonav = np.count_nonzero(seg_block == ClassesSegmentacao.NAONAVEGAVEL.value) + pct_navegavel[j, i] = n_nav / n + pct_nao_navegavel[j, i] = n_naonav / n - # depth - vals = depth_block[~np.isnan(depth_block)] - valid = vals.size - depth_valid_frac[j, i] = valid / n - if valid >= max(int(min_valid_frac * n), 1): - z_med[j, i] = np.nanmedian(vals) / 1000.0 + # depth (apenas se usando) + if depth_row is not None: + depth_block = depth_row[:, x0:x1] + vals = depth_block[~np.isnan(depth_block)] + valid = vals.size + depth_valid_frac[j, i] = valid / n + if valid >= max(int(min_valid_frac * n), 1): + z_med[j, i] = np.nanmedian(vals) / 1000.0 # mm -> m - # --- 6) Confiança --- + # --- 6) Confiança da célula --- t0, t1 = conf_params - conf_seg = np.maximum.reduce([pct_rua, pct_cana, pct_obs]) - conf_dep = np.clip((depth_valid_frac - t0) / (t1 - t0), 0.0, 1.0) - conf_cell = 0.6 * conf_seg + 0.4 * conf_dep + conf_seg = np.maximum.reduce([pct_navegavel, pct_nao_navegavel]) + + if usar_depth and d_small is not None: + conf_dep = np.clip((depth_valid_frac - t0) / (t1 - t0), 0.0, 1.0) + conf_cell = 0.6 * conf_seg + 0.4 * conf_dep + else: + # sem depth: confiança baseada só na segmentação + conf_dep = np.zeros_like(conf_seg, dtype=np.float32) + conf_cell = conf_seg.copy() # --- 6b) Rasterizar detecções (opcional) --- if deteccoes: - # percorre bboxes e projeta para grid for det in deteccoes: conf = float(det.get("conf", 0.0)) if conf < _det["min_det_conf"]: @@ -986,13 +664,15 @@ class CameraManager: w_class = _det["class_weights"].get(label, 1.0) veto = (label in _det["veto_labels"]) - # caixa em px (melhor usar bbox_px se já veio arredondado no teu pipeline) + # bbox em px if "bbox_px" in det and det["bbox_px"]: x0p, y0p, x1p, y1p = det["bbox_px"] else: x0n, y0n, x1n, y1n = det["bbox_norm"] - x0p = int(np.clip(x0n * 512, 0, 511)); x1p = int(np.clip(x1n * 512, 0, 512)) - y0p = int(np.clip(y0n * 288, 0, 287)); y1p = int(np.clip(y1n * 288, 0, 288)) + x0p = int(np.clip(x0n * W1, 0, W1 - 1)) + x1p = int(np.clip(x1n * W1, 0, W1)) + y0p = int(np.clip(y0n * H1, 0, H1 - 1)) + y1p = int(np.clip(y1n * H1, 0, H1)) if x1p <= x0p or y1p <= y0p: continue @@ -1000,88 +680,90 @@ class CameraManager: if bbox_area <= 1.0: continue - # descobre células que sobrepõem a bbox - # índices i (colunas) e j (linhas) candidatas + # células candidatas i0 = max(0, np.searchsorted(x_edges, x0p, side="right") - 1) i1 = min(grid_w-1, np.searchsorted(x_edges, x1p, side="left")) j0 = max(0, np.searchsorted(y_edges, y0p, side="right") - 1) j1 = min(grid_h-1, np.searchsorted(y_edges, y1p, side="left")) for j in range(j0, j1+1): - y0, y1 = y_edges[j], y_edges[j+1] + y0c, y1c = int(y_edges[j]), int(y_edges[j+1]) for i in range(i0, i1+1): - x0, x1 = x_edges[i], x_edges[i+1] - # interseção - ix0 = max(x0, x0p); ix1 = min(x1, x1p) - iy0 = max(y0, y0p); iy1 = min(y1, y1p) + x0c, x1c = int(x_edges[i]), int(x_edges[i+1]) + + ix0 = max(x0c, x0p); ix1 = min(x1c, x1p) + iy0 = max(y0c, y0p); iy1 = min(y1c, y1p) if ix1 <= ix0 or iy1 <= iy0: continue inter = float((ix1 - ix0) * (iy1 - iy0)) - # cobertura em relação à célula (mais conservador que em relação à bbox) - cell_area = float((x1 - x0) * (y1 - y0)) + cell_area = float((x1c - x0c) * (y1c - y0c)) if cell_area <= 0: continue cov = inter / cell_area - if cov < _det["min_cell_coverage"]: continue - # score local da detecção nesta célula - # se for classe vetada, zera atenuações base = conf if not veto else 1.0 s = base * cov * w_class + det_cov_max[j, i] = max(det_cov_max[j, i], cov) det_conf_max[j, i] = max(det_conf_max[j, i], conf) if _det["combine"] == "sum_clamped": det_score[j, i] = np.clip(det_score[j, i] + s, 0.0, 1.0) - else: # "max" + else: det_score[j, i] = max(det_score[j, i], s) - # critério: escolhe como dominante a de MAIOR (conf * cov) keyval = conf * cov if keyval > det_top_conf[j, i]: det_top_conf[j, i] = keyval det_top_label_id[j, i] = int(det.get("label_id", -1)) - # opcional: derruba um pouco a confiança onde há detecção if _det["conf_drop_alpha"] > 0.0: conf_cell = np.clip(conf_cell * (1.0 - _det["conf_drop_alpha"] * det_score), 0.0, 1.0) - # --- 7) Anomalia (igual à tua) --- - delta = Z_ref - z_med - delta = np.where(np.isnan(z_med), 0.0, np.maximum(delta, 0.0)) - anom_raw = np.clip(delta / anom_satur_m, 0.0, 1.0) - anom = anom_raw * (delta > anom_tau_min).astype(np.float32) * conf_dep + # --- 7) Anomalia de solo --- + if usar_depth and d_small is not None: + delta = Z_ref - z_med + delta = np.where(np.isnan(z_med), 0.0, np.maximum(delta, 0.0)) + anom_raw = np.clip(delta / anom_satur_m, 0.0, 1.0) + anom = anom_raw * (delta > anom_tau_min).astype(np.float32) * conf_dep + else: + anom = np.zeros_like(pct_navegavel, dtype=np.float32) # --- 8) Custo e navegabilidade --- - nao_rua = 1.0 - pct_rua + nao_navegavel = 1.0 - pct_navegavel w1, w2, w3 = w - custo = w1 * nao_rua + w2 * anom + w3 * (1.0 - conf_cell) + custo = w1 * nao_navegavel + w2 * anom + w3 * (1.0 - conf_cell) - # penalização por detecção (se houver) if deteccoes: custo = np.clip(custo + _det["w4"] * det_score, 0.0, 1.0) - # regra de navegabilidade com detecção (bloqueia se score alto) if deteccoes: - navegavel = (pct_rua >= 0.55) & (anom < 0.4) & (conf_cell >= 0.5) & (det_score < _det["thr_det_block"]) + navegavel = ( + (pct_navegavel >= 0.55) & + (anom < 0.4) & + (conf_cell >= 0.5) & + (det_score < _det["thr_det_block"]) + ) else: - navegavel = (pct_rua >= 0.55) & (anom < 0.4) & (conf_cell >= 0.5) + navegavel = ( + (pct_navegavel >= 0.55) & + (anom < 0.4) & + (conf_cell >= 0.5) + ) return { - "pct_rua": pct_rua, - "pct_cana": pct_cana, - "pct_obs": pct_obs, + "pct_navegavel": pct_navegavel, + "pct_nao_navegavel": pct_nao_navegavel, "z_med": z_med, "z_ref": Z_ref, "depth_valid_frac": depth_valid_frac, - "conf": np.clip(custo*0 + conf_cell, 0.0, 1.0), # garante 0..1 + "conf": np.clip(conf_cell, 0.0, 1.0), "anom": np.clip(anom, 0.0, 1.0), "custo": np.clip(custo, 0.0, 1.0), "navegavel": navegavel.astype(np.uint8), - # ---- extras p/ debug/telemetria ---- "det_cov_max": det_cov_max, "det_conf_max": det_conf_max, "det_score": det_score, @@ -1091,304 +773,6 @@ class CameraManager: except Exception as e: self.mostrar_log(f"Erro ao construir grid de confianca: {e}") return None - - def _put_text_centered(self, img, text, cx, cy, font_scale=0.4, thickness=1, color=(255,255,255), outline=True): - font = cv2.FONT_HERSHEY_SIMPLEX - (tw, th), baseline = cv2.getTextSize(text, font, font_scale, thickness) - x = int(cx - tw/2) - y = int(cy + th/2) - if outline: - cv2.putText(img, text, (x+1, y+1), font, font_scale, (0,0,0), thickness+2, cv2.LINE_AA) - cv2.putText(img, text, (x, y), font, font_scale, color, thickness, cv2.LINE_AA) - - def make_costmap_overlay( - self, - rgb_frame, # HxWx3 (BGR se veio do OpenCV) - grid_dict, # dict com arrays (grid_h, grid_w): "custo","conf","anom","navegavel","z_med","z_ref","pct_rua","pct_cana","pct_obs" - grid_shape=(15,10), # (cols, rows) - alpha=0.45, # transparência do heatmap - draw_borders=True, - draw_cells=True, # desenha retângulos por célula - draw_text_mode="mini", # "off" | "mini" | "full" - draw_legend=True - ): - #rgb_frame = cv2.resize(rgb_frame, (1080, 720), interpolation=cv2.INTER_AREA) - H, W = rgb_frame.shape[:2] - grid_w, grid_h = grid_shape - custo = grid_dict["custo"].astype(np.float32) # (grid_h, grid_w) [0..1] - conf = grid_dict.get("conf", None) - anom = grid_dict.get("anom", None) - nav = grid_dict.get("navegavel", None) - z_med = grid_dict.get("z_med", None) - z_ref = grid_dict.get("z_ref", None) - p_rua = grid_dict.get("pct_rua", None) - p_can = grid_dict.get("pct_cana", None) - p_obs = grid_dict.get("pct_obs", None) - - # 1) heatmap (upscale por vizinho p/ manter blocos nítidos) - cost_u8 = np.clip(custo * 255.0, 0, 255).astype(np.uint8) - cost_up = cv2.resize(cost_u8, (W, H), interpolation=cv2.INTER_NEAREST) - try: - cmap = cv2.COLORMAP_TURBO - except AttributeError: - cmap = cv2.COLORMAP_JET - heat = cv2.applyColorMap(cost_up, cmap) - - # 2) overlay - out = cv2.addWeighted(heat, alpha, rgb_frame, 1.0 - alpha, 0) - - # 3) grid edges - x_edges = (np.linspace(0, W, grid_w+1)).astype(int) - y_edges = (np.linspace(0, H, grid_h+1)).astype(int) - - if draw_borders: - for x in x_edges: - cv2.line(out, (x, 0), (x, H-1), (50,50,50), 1, cv2.LINE_AA) - for y in y_edges: - cv2.line(out, (0, y), (W-1, y), (50,50,50), 1, cv2.LINE_AA) - - # 4) por-célula (bordas coloridas + textos) - if draw_cells: - for j in range(grid_h): - y0, y1 = y_edges[j], y_edges[j+1] - cy = (y0 + y1) // 2 - for i in range(grid_w): - x0, x1 = x_edges[i], x_edges[i+1] - cx = (x0 + x1) // 2 - - c = float(custo[j, i]) - # bordinha: navegável verde, bloqueado vermelho, neutro cinza - if nav is not None: - if nav[j, i]: - color = (60, 200, 60) - else: - color = (20, 20, 220) if c > 0.7 else (90, 90, 90) - else: - color = (90, 90, 90) - cv2.rectangle(out, (x0, y0), (x1-1, y1-1), color, 1) - - if draw_text_mode != "off": - # texto curtinho (mini) ou completo (full) - if draw_text_mode == "mini": - txt = f"C{int(round(c*100))}" - if conf is not None: - txt += f" cf{int(round(conf[j,i]*100))}" - if anom is not None: - txt += f" A{int(round(anom[j,i]*100))}" - self._put_text_centered(out, txt, cx, cy, font_scale=0.32, thickness=1, color=(255,255,255), outline=True) - elif draw_text_mode == "full": - line1 = f"C{int(round(c*100))}" - if conf is not None: line1 += f" cf{conf[j,i]:.2f}" - if anom is not None: line1 += f" A{int(round(anom[j,i]*100))}" - self._put_text_centered(out, line1, cx, cy-8, font_scale=0.38, thickness=1, - color=(255,255,255), outline=True) - line2 = "" - if z_med is not None and z_ref is not None and not np.isnan(z_med[j,i]): - line2 = f"Z{z_med[j,i]:.1f}/{z_ref[j,i]:.1f}m" - elif z_ref is not None: - line2 = f"Zref {z_ref[j,i]:.1f}m" - if line2: - self._put_text_centered(out, line2, cx, cy+8, font_scale=0.36, thickness=1, - color=(255,255,255), outline=True) - if p_rua is not None and p_can is not None and p_obs is not None: - line3 = f"R{int(p_rua[j,i]*100)} C{int(p_can[j,i]*100)} O{int(p_obs[j,i]*100)}" - self._put_text_centered(out, line3, cx, cy+22, font_scale=0.34, thickness=1, - color=(230,230,230), outline=True) - - # 5) legenda opcional - if draw_legend: - pad = 8 - x0, y0 = pad, pad - cv2.rectangle(out, (x0-4, y0-4), (x0+160, y0+72), (0,0,0), -1) - cv2.putText(out, "Legenda:", (x0, y0+12), cv2.FONT_HERSHEY_SIMPLEX, 0.45, (255,255,255), 1, cv2.LINE_AA) - cv2.putText(out, "C= custo (0..1)", (x0, y0+28), cv2.FONT_HERSHEY_SIMPLEX, 0.42, (200,200,200), 1, cv2.LINE_AA) - cv2.putText(out, "cf= confianca", (x0, y0+44), cv2.FONT_HERSHEY_SIMPLEX, 0.42, (200,200,200), 1, cv2.LINE_AA) - cv2.putText(out, "A= anomalia", (x0, y0+60), cv2.FONT_HERSHEY_SIMPLEX, 0.42, (200,200,200), 1, cv2.LINE_AA) - - return out - - def debug_show_costmap(self, rgb_frame, grid_dict, grid_shape=(15,10), window_name="debug_costmap", wait=1, text_mode="mini"): - vis = self.make_costmap_overlay(rgb_frame, grid_dict, grid_shape=grid_shape, alpha=0.45, draw_borders=True, draw_cells=True, draw_text_mode=text_mode, draw_legend=True) - cv2.namedWindow(window_name, cv2.WINDOW_NORMAL) - cv2.imshow(window_name, vis) - key = cv2.waitKey(wait) & 0xFF - return key, vis - - - - def _normalize_cost_robusto(self, C: np.ndarray): - C = np.asarray(C, dtype=np.float32) - p5, p95 = np.percentile(C, [5, 95]) - cmin, cmax = float(p5), float(max(p95, p5 + 1e-6)) - N = (C - cmin) / (cmax - cmin + 1e-9) - return np.clip(N, 0.0, 1.0), cmin, cmax - - def make_visualworker_overlay( - self, - frame_bgr, # frame da câmera (BGR, HxWx3) - grid, # dict: {"Custo": Hc x Wc, "Navegavel": Hc x Wc, ...} - alpha_cost=0.45, # transparência do heatmap - draw_grid=True, - draw_cells=True, - text_mode="mini", # "off" | "mini" | "full" - draw_legend=True, - nav_overlay_alpha=0.35 # opacidade do overlay vermelho p/ NAV=False - ): - """ - Sobrepõe Custo (+ Navegável) do Visual Worker no frame RGB, em tempo real. - Retorna uma imagem BGR com o overlay. - """ - C = np.asarray(grid.get("custo", None), dtype=np.float32) - if C is None or C.size == 0: - return frame_bgr.copy() - - NAV = grid.get("navegavel", None) - Hc, Wc = C.shape - Hf, Wf = frame_bgr.shape[:2] - - # 1) heatmap de custo (robusto a outliers) - N, cmin, cmax = self._normalize_cost_robusto(C) # [0..1] - u8 = (N * 255.0).astype(np.uint8) - up = cv2.resize(u8, (Wf, Hf), interpolation=cv2.INTER_NEAREST) - try: - cmap = cv2.COLORMAP_TURBO - except AttributeError: - cmap = cv2.COLORMAP_JET - heat = cv2.applyColorMap(up, cmap) - out = cv2.addWeighted(heat, alpha_cost, frame_bgr, 1.0 - alpha_cost, 0) - - # 2) overlay de não-navegável (vermelho) - if NAV is not None: - bad = (~np.asarray(NAV, dtype=bool)).astype(np.uint8) * 255 # 255 = bloqueado - bad_up = cv2.resize(bad, (Wf, Hf), interpolation=cv2.INTER_NEAREST) - mask = bad_up.astype(bool) - red = out.copy() - red[mask] = (0, 0, 255) - out = cv2.addWeighted(red, nav_overlay_alpha, out, 1.0 - nav_overlay_alpha, 0) - - # 3) grade - x_edges = np.linspace(0, Wf, Wc + 1).astype(int) - y_edges = np.linspace(0, Hf, Hc + 1).astype(int) - if draw_grid: - for x in x_edges: - cv2.line(out, (x, 0), (x, Hf - 1), (60, 60, 60), 1, cv2.LINE_AA) - for y in y_edges: - cv2.line(out, (0, y), (Wf - 1, y), (60, 60, 60), 1, cv2.LINE_AA) - - # 4) textos por célula (mini/full) - if draw_cells and text_mode != "off": - conf = grid.get("conf", None) - anom = grid.get("anom", None) - z_med = grid.get("z_med", None) - z_ref = grid.get("z_ref", None) - p_rua = grid.get("pct_rua", None) - p_can = grid.get("pct_cana", None) - p_obs = grid.get("pct_obs", None) - - for j in range(Hc): - y0, y1 = y_edges[j], y_edges[j + 1] - cy = (y0 + y1) // 2 - for i in range(Wc): - x0, x1 = x_edges[i], x_edges[i + 1] - cx = (x0 + x1) // 2 - - # moldura fina, cor conforme NAV - if NAV is not None: - if NAV[j, i]: - color = (60, 200, 60) - else: - color = (20, 20, 220) if C[j, i] > (cmin + 0.7*(cmax-cmin)) else (90, 90, 90) - else: - color = (90, 90, 90) - cv2.rectangle(out, (x0, y0), (x1 - 1, y1 - 1), color, 1, cv2.LINE_AA) - - # textos - if text_mode == "mini": - # custo em 0..100 + check NAV - c100 = int(round(N[j, i] * 100)) - txt = f"C{c100}" - if NAV is not None: - txt += " N" if NAV[j, i] else " B" - self._put_text_centered(out, txt, cx, cy, font_scale=0.32, thickness=1, - color=(255, 255, 255), outline=True) - elif text_mode == "full": - l1 = f"C{N[j,i]*100:.0f}" - if conf is not None: l1 += f" cf{conf[j,i]:.2f}" - if anom is not None: l1 += f" A{anom[j,i]*100:.0f}" - self._put_text_centered(out, l1, cx, cy - 8, font_scale=0.38, thickness=1, - color=(255, 255, 255), outline=True) - l2 = "" - if z_med is not None and z_ref is not None and not np.isnan(z_med[j,i]): - l2 = f"Z{z_med[j,i]:.1f}/{z_ref[j,i]:.1f}m" - elif z_ref is not None: - l2 = f"Zref {z_ref[j,i]:.1f}m" - if l2: - self._put_text_centered(out, l2, cx, cy + 8, font_scale=0.36, thickness=1, - color=(255, 255, 255), outline=True) - if p_rua is not None and p_can is not None and p_obs is not None: - l3 = f"R{int(p_rua[j,i]*100)} C{int(p_can[j,i]*100)} O{int(p_obs[j,i]*100)}" - self._put_text_centered(out, l3, cx, cy + 22, font_scale=0.34, thickness=1, - color=(230, 230, 230), outline=True) - - # 5) DistânciasRef por linha (se disponível) - dist_ref = grid.get("DistanciasRef", None) - if dist_ref is not None and len(dist_ref) == Hc: - for j in range(Hc): - y = (y_edges[j] + y_edges[j + 1]) // 2 - s = f"{float(dist_ref[j]):.2f}m" - cv2.putText(out, s, (5, y + 10), cv2.FONT_HERSHEY_SIMPLEX, 0.42, (0, 0, 0), 2, cv2.LINE_AA) - cv2.putText(out, s, (5, y + 10), cv2.FONT_HERSHEY_SIMPLEX, 0.42, (255, 255, 255), 1, cv2.LINE_AA) - - # 6) legenda - if draw_legend: - pad = 8 - x0, y0 = pad, pad - cv2.rectangle(out, (x0 - 4, y0 - 4), (x0 + 190, y0 + 74), (0, 0, 0), -1) - cv2.putText(out, "Legenda", (x0, y0 + 14), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255,255,255), 1, cv2.LINE_AA) - cv2.putText(out, "C: custo (0..1) norm.", (x0, y0 + 32), cv2.FONT_HERSHEY_SIMPLEX, 0.42, (200,200,200), 1, cv2.LINE_AA) - cv2.putText(out, "N navegavel / B bloqueado", (x0, y0 + 50), cv2.FONT_HERSHEY_SIMPLEX, 0.42, (200,200,200), 1, cv2.LINE_AA) - cv2.putText(out, "turbo=baixo jet=alto", (x0, y0 + 68), cv2.FONT_HERSHEY_SIMPLEX, 0.38, (160,160,160), 1, cv2.LINE_AA) - - return out - - def debug_show_visualworker( - self, - frame_bgr, - grid, - window_name="VW Cost/NAV", - wait=1, - **overlay_kwargs - ): - vis = self.make_visualworker_overlay(frame_bgr, grid, **overlay_kwargs) - cv2.namedWindow(window_name, cv2.WINDOW_NORMAL) - cv2.imshow(window_name, vis) - key = cv2.waitKey(wait) & 0xFF - return key, vis - - - def _colorize_masks(self, anom_f, custo_f, conf_f, thr_anom_block=0.50, thr_cost_block=0.65, thr_conf_low=0.35): - """Overlay BGR com soma segura (clamped) nas regiões de máscara.""" - H, W = anom_f.shape - over = np.zeros((H, W, 3), np.uint8) - - mask_anom = (anom_f >= thr_anom_block) - mask_cost = (custo_f >= thr_cost_block) - mask_conf = (conf_f < thr_conf_low) - - def add_color(mask, bgr): - if not np.any(mask): - return - # soma segura com clip - tmp = over[mask].astype(np.int16) - tmp += np.array(bgr, dtype=np.int16) - np.clip(tmp, 0, 255, out=tmp) - over[mask] = tmp.astype(np.uint8) - - add_color(mask_anom, (255, 0, 255)) # magenta - add_color(mask_cost, ( 0,165,255)) # laranja - add_color(mask_conf, (255, 0, 0)) # azul - - return over, mask_anom, mask_cost, mask_conf def debug_blockage_imshow( self, @@ -1532,8 +916,7 @@ class CameraManager: y0 += bar_h + line_gap + 14 # SideBias - put_text_outlined(vis, f"SideBias {side_val:+.2f}", (x0, y0), - font, font_scale, (255,255,255), thick) + put_text_outlined(vis, f"SideBias {side_val:+.2f}", (x0, y0), font, font_scale, (255,255,255), thick) cx = x0 + bar_w//2 y_bar = y0 + 14 # linha base -1..+1 @@ -1545,8 +928,7 @@ class CameraManager: cv2.circle(vis, (bx, y_bar), 5, (0,255,0), -1) # L / R - put_text_outlined(vis, f"L:{left_frac:.2f} R:{right_frac:.2f}", (x0, y_bar + 20), - font, font_scale, (255,255,255), thick) + put_text_outlined(vis, f"L:{left_frac:.2f} R:{right_frac:.2f}", (x0, y_bar + 20), font, font_scale, (255,255,255), thick) # --- DECISION HUD: PARAR / LIVRE --- # empurra um pouco pra baixo do L/R @@ -1597,124 +979,81 @@ class CameraManager: self.mostrar_log(f"Erro ao criar debug blockage imshow: {e}") return None, None + def _colorize_masks(self, anom_f, custo_f, conf_f, thr_anom_block=0.50, thr_cost_block=0.65, thr_conf_low=0.35): + """Overlay BGR com soma segura (clamped) nas regiões de máscara.""" + H, W = anom_f.shape + over = np.zeros((H, W, 3), np.uint8) - def _overlay_deteccoes( - self, - rgb_frame, - dets, - conf_thr=0.5, # limiar de confiança pra desenhar - roi_frac=None, # (rx1,ry1,rx2,ry2) normalizado da ROI usada no detector (ex.: (0.0,y1,1.0,y2)) - show=True, # se True, faz cv2.imshow - janela="det", # nome da janela - fps_state=None, # dict estado do FPS (persistido fora), ex.: {} - ): - """ - dets: lista de dicts no formato: - { - "label_id": int, - "label": str|None, - "conf": float, - "bbox_norm": [x0,y0,x1,y1] # 0..1 relativo ao input do detector (na ROI) - # opcional: "bbox_full": [x0,y0,x1,y1] em px do frame completo - } - Retorna: (frame_com_overlay, fps_state, keep_loop_bool) - """ + mask_anom = (anom_f >= thr_anom_block) + mask_cost = (custo_f >= thr_cost_block) + mask_conf = (conf_f < thr_conf_low) + + def add_color(mask, bgr): + if not np.any(mask): + return + # soma segura com clip + tmp = over[mask].astype(np.int16) + tmp += np.array(bgr, dtype=np.int16) + np.clip(tmp, 0, 255, out=tmp) + over[mask] = tmp.astype(np.uint8) + + add_color(mask_anom, (255, 0, 255)) # magenta + add_color(mask_cost, ( 0,165,255)) # laranja + add_color(mask_conf, (255, 0, 0)) # azul + + return over, mask_anom, mask_cost, mask_conf + + + + def salvar_frames(self, tipos: list, nome: str, pasta="frames_salvos"): + if not self.operante: + return [] + try: - img = cv2.resize(rgb_frame.copy(), (1280, 720)) - H, W = img.shape[:2] + os.makedirs(pasta, exist_ok=True) + frames_salvos = [] - # paleta simples por classe - palette = [ - (255, 56, 56), (255, 157, 151), (72, 249, 10), (0, 255, 0), (0, 0, 255), - (255, 0, 255), (0, 255, 255), (255, 191, 0), (52, 148, 230), (147, 112, 219) - ] + frame_name = datetime.datetime.now().strftime('%Y%m%d_%H%M%S') + if nome is not None and nome != "": + frame_name = nome - def _map_bbox_norm_to_full(bn): - # bn é [x0n,y0n,x1n,y1n] relativo ao input da ROI (0..1) - x0n, y0n, x1n, y1n = bn - if roi_frac is not None: - rx1, ry1, rx2, ry2 = roi_frac - sx, sy = (rx2 - rx1), (ry2 - ry1) - x0 = int(round((rx1 + x0n * sx) * W)) - y0 = int(round((ry1 + y0n * sy) * H)) - x1 = int(round((rx1 + x1n * sx) * W)) - y1 = int(round((ry1 + y1n * sy) * H)) - else: - x0 = int(round(x0n * W)) - y0 = int(round(y0n * H)) - x1 = int(round(x1n * W)) - y1 = int(round(y1n * H)) - # clamp - x0 = max(0, min(W - 1, x0)); x1 = max(0, min(W - 1, x1)) - y0 = max(0, min(H - 1, y0)); y1 = max(0, min(H - 1, y1)) - return x0, y0, x1, y1 + if CameraFrameType.Rgb.value in tipos: + rgb_frame, rgb_ts, rgb_res = self.get_rgb_frame() + if rgb_frame is not None and rgb_res["frame_valido"]: + nome_rgb = f"{frame_name}_rgb.jpeg" + caminho_rgb = os.path.join(pasta, nome_rgb) + cv2.imwrite(caminho_rgb, rgb_frame) + frames_salvos.append(caminho_rgb) - def _put_label(img, text, x, y, bg): - (tw, th), _ = cv2.getTextSize(text, cv2.FONT_HERSHEY_SIMPLEX, 0.5, 1) - cv2.rectangle(img, (x, max(0, y - th - 6)), (x + tw + 6, y), bg, -1) - cv2.putText(img, text, (x + 3, y - 4), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 0), 1, cv2.LINE_AA) + if CameraFrameType.Heatmap.value in tipos: + heatmap_frame, heatmap_ts, heatmap_res = self.get_heatmap_frame() + if heatmap_frame is not None and heatmap_res["frame_valido"]: + nome_heatmap = f"{frame_name}_heatmap.jpeg" + caminho_heatmap = os.path.join(pasta, nome_heatmap) + cv2.imwrite(caminho_heatmap, heatmap_frame) + frames_salvos.append(caminho_heatmap) - # desenhar ROI (opcional, ajuda debug) - if roi_frac is not None: - rx1, ry1, rx2, ry2 = roi_frac - x0r, y0r = int(rx1 * W), int(ry1 * H) - x1r, y1r = int(rx2 * W), int(ry2 * H) - cv2.rectangle(img, (x0r, y0r), (x1r, y1r), (60, 60, 60), 1) + if CameraFrameType.RadarTopDown.value in tipos: + frame = self._salvar_frame_analise(frame_name, self._ultima_analise_radar, "radar", pasta) + if frame is not None: + frames_salvos.append(frame) - # desenhar detecções - for d in dets: - if d.get("conf", 0.0) < conf_thr: - continue + if CameraFrameType.Segmentacao.value in tipos: + frame = self._salvar_frame_analise(frame_name, self._ultima_analise_segmentacao, "segmentacao", pasta) + if frame is not None: + frames_salvos.append(frame) - # bbox em px do frame - if "bbox_full" in d and d["bbox_full"]: - x0, y0, x1, y1 = d["bbox_full"] - # clamp se necessário - x0 = max(0, min(W - 1, int(x0))); x1 = max(0, min(W - 1, int(x1))) - y0 = max(0, min(H - 1, int(y0))); y1 = max(0, min(H - 1, int(y1))) - else: - bn = d.get("bbox_norm", None) - if not bn: - continue - x0, y0, x1, y1 = _map_bbox_norm_to_full(bn) - - if x1 <= x0 or y1 <= y0: - continue - - lid = int(d.get("label_id", -1)) - color = palette[lid % len(palette)] if lid >= 0 else (0, 255, 0) - - cv2.rectangle(img, (x0, y0), (x1, y1), color, 2) - - name = d.get("label", None) - txt = f"{name or f'id:{lid}'} {d.get('conf', 0.0):.2f}" - _put_label(img, txt, x0, y0, color) - - # FPS (EMA) - now = time.monotonic() - if fps_state is None: - fps_state = {} - t_prev = fps_state.get("t_prev") - fps_ema = fps_state.get("fps_ema") - if t_prev is not None: - dt = now - t_prev - if dt > 0: - fps_inst = 1.0 / dt - alpha = 0.90 - fps_ema = fps_inst if fps_ema is None else (alpha * fps_ema + (1 - alpha) * fps_inst) - fps_state["t_prev"] = now - fps_state["fps_ema"] = fps_ema - - if fps_ema: - cv2.putText(img, f"FPS: {fps_ema:.1f}", (10, 22), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (50, 220, 50), 2, cv2.LINE_AA) - - keep = True - if show: - cv2.imshow(janela, img) - k = cv2.waitKey(1) & 0xFF - keep = (k != 27) # ESC para sair - - return img, fps_state, keep + return frames_salvos except Exception as e: - self.mostrar_log(f"Erro ao gerar overlay de deteccoes") - \ No newline at end of file + self.mostrar_log(f"❌ Erro ao salvar frames: {e}") + return [] + + def _salvar_frame_analise(self, frame_name, analise, sulfix, pasta): + base64frame = analise.get("frame", {}).get("frame") + frame = decode_image_base64(base64frame) + if frame is not None: + nome_analise = f"{frame_name}_{sulfix}.jpeg" + caminho_analise = os.path.join(pasta, nome_analise) + cv2.imwrite(caminho_analise, frame) + return caminho_analise + return None 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 6328153fe..8adf90f25 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 @@ -75,11 +75,13 @@ def load_seg_config(force_reload=False): "debug_visual": True, "ia_roi_begin": 0.0, "ia_roi_size": 1.0, - "ia_resolution": [512,288], - "det_every_n": 1, + "ia_resolution": [1024,576], + "seg_every_n": 1, + "det_every_n": 3, } _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_backbone"] = ContextoGlobalRedis.get_equipamento().get("ia_backbone_seg", "nvidia/segformer-b0-finetuned-ade-512-512") return _CONFIG_CACHE 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 5b89e12e9..494f006c7 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 @@ -8,9 +8,8 @@ from shared.utils import encode_image_base64 from shared.enums import StatusCarroMapa class ClassesSegmentacao(IntEnum): - RUA = 0 - CANA = 1 - OBSTACULO = 2 + NAONAVEGAVEL = 0 + NAVEGAVEL = 1 class SegmentacaoManager: @@ -49,34 +48,6 @@ class SegmentacaoManager: self.max_len = max_len or int(self.janela_s * fps_esperado * 1.5) self._status_hist = deque(maxlen=self.max_len) - # histerese por banda - self._has_top_prev = False - self._has_bot_prev = False - self._thr_top_on = 0.20 - self._thr_top_off = 0.10 - self._thr_bot_on = 0.20 - self._thr_bot_off = 0.10 - - def _segmentar_predictions(self, predictions): - try: - self.pred_rgb[:] = self.lut[predictions] - - mask_color = self.pred_rgb - frame_color = encode_image_base64(mask_color) - - return { - "timestamp": time.time(), - "frame": { - "timestamp": time.time(), - "frame": frame_color - }, - "mask_color": mask_color, - "classes": predictions - } - - except Exception as e: - print(f"Erro ao processar predictions: {e}") - return None def segmentar(self, predictions): try: @@ -99,326 +70,26 @@ class SegmentacaoManager: self.log = f"❌ Erro na segmentação: {e}" return None, self.log - def calcular_perfil_corredor(self, matriz_conf, fov_h_rad): - altura_grid = len(matriz_conf) - largura_grid = len(matriz_conf[0]) - perfil = [] + def _segmentar_predictions(self, predictions): + try: + self.pred_rgb[:] = self.lut[predictions] - for i in range(altura_grid): - x_esquerda = None - x_direita = None + mask_color = self.pred_rgb + frame_color = encode_image_base64(mask_color) - # Coleta todos os valores válidos de profundidade da linha - profundidades = [cel.get("prof_ref", 0.0) for cel in matriz_conf[i] if cel.get("prof_ref", 0.0) > 0] + return { + "timestamp": time.time(), + "frame": { + "timestamp": time.time(), + "frame": frame_color + }, + "mask_color": mask_color, + "classes": predictions + } - if len(profundidades) == 0: - continue - - # Usa a mediana como valor de z mais estável - z = float(np.median(profundidades)) - - for j in range(largura_grid): - cel = matriz_conf[i][j] - ind_chao = cel.get("indice_seg_chao", 0.0) - if ind_chao < 0.3: - continue - - # Ângulo relativo do centro da célula - ang_normalizado = (j + 0.5 - (largura_grid / 2)) / (largura_grid / 2) - ang_rad = ang_normalizado * (fov_h_rad / 2) - - # Posição X central da célula - x_centro = np.tan(ang_rad) * z - - # Ângulo por célula para deslocamento parcial - escala_angular = np.tan(fov_h_rad / largura_grid) * z - - if j < largura_grid / 2: - x_real = x_centro - ind_chao * (escala_angular / 1.6) - if x_esquerda is None or x_real < x_esquerda: - x_esquerda = x_real - else: - x_real = x_centro + ind_chao * (escala_angular / 1.6) - if x_direita is None or x_real > x_direita: - x_direita = x_real - - if x_esquerda is not None and x_direita is not None: - esquerda_m = abs(x_esquerda) - direita_m = x_direita - largura = direita_m + esquerda_m - else: - esquerda_m = direita_m = largura = 0.0 - - perfil.append({ - "distancia_m": round(z, 3), - "esquerda_m": round(float(esquerda_m), 3), - "direita_m": round(float(direita_m), 3), - "largura_m": round(float(largura), 3) - }) - - return perfil - - - - @staticmethod - def _now(): - # monotonic evita saltos de relógio - return time.monotonic() - - def _bool_hysteresis(self, prev: bool, x: float, thr_on: float, thr_off: float) -> bool: - return (x >= (thr_off if prev else thr_on)) - - def _maioria_ultimos(self, janela_s: float | None = None) -> StatusCarroMapa: - """Maioria ponderada pelos últimos 'janela_s' segundos. - Se 'janela_s' None → usa self.janela_s.""" - J = self.janela_s if janela_s is None else float(janela_s) - t_now = self._now() - - # 1) limpa itens FORA da janela - while self._status_hist and (t_now - self._status_hist[0][1] > J): - self._status_hist.popleft() - - if not self._status_hist: - # fallback razoável - return StatusCarroMapa.Direcionando - - # 2) maioria simples (pode trocar por peso exponencial se quiser) - cont = {} - for st, _t in self._status_hist: - cont[st] = cont.get(st, 0) + 1 - - # regra de desempate: prioriza estado mais recente em caso de empate - top_freq = max(cont.values()) - empatados = [st for st, c in cont.items() if c == top_freq] - if len(empatados) == 1: - return empatados[0] - else: - # desempata olhando do fim pro início (mais recente primeiro) - for st, _t in reversed(self._status_hist): - if st in empatados: - return st - - def classificar_por_cana(self, mask_cana, mask_main=None, near_is_bottom=True): - H, W = mask_cana.shape - mid = H // 2 - bottom = mask_cana[mid:, :] if near_is_bottom else mask_cana[:mid, :] - top = mask_cana[:mid, :] if near_is_bottom else mask_cana[mid:, :] - - area_top = top.size - area_bot = bottom.size - p_top = float(top.sum()) / max(1, area_top) - p_bot = float(bottom.sum()) / max(1, area_bot) - - has_top = self._bool_hysteresis(self._has_top_prev, p_top, self._thr_top_on, self._thr_top_off) - has_bot = self._bool_hysteresis(self._has_bot_prev, p_bot, self._thr_bot_on, self._thr_bot_off) - self._has_top_prev, self._has_bot_prev = has_top, has_bot - - if not has_top and not has_bot: - status_now = StatusCarroMapa.Direcionando - elif has_top and not has_bot: - status_now = StatusCarroMapa.EntrandoRua - elif has_bot and not has_top: - status_now = StatusCarroMapa.SaindoRua - else: - status_now = StatusCarroMapa.CaminhandoRua - - # empilha (status, timestamp) - self._status_hist.append((status_now, self._now())) - - status_final = self._maioria_ultimos() # maioria na janela fixa - return status_now, status_final, p_top, p_bot, has_top, has_bot - - - - def _extrair_corredor_principal(self, mask_classes): - H, W = mask_classes.shape - cx_img = W // 2 - - mask_rua = (mask_classes == ClassesSegmentacao.RUA.value).astype(np.uint8) - - # optional: fecha buracos pequenos - # kernel = np.ones((3,3), np.uint8) - # mask_rua = cv2.morphologyEx(mask_rua, cv2.MORPH_CLOSE, kernel, iterations=1) - - num_labels, labels, stats, _ = cv2.connectedComponentsWithStats(mask_rua, connectivity=8) - if num_labels <= 1: - return np.zeros_like(mask_rua, dtype=np.uint8) - - best_i, best_score = -1, -1e9 - - # pesos do score - w_area = 1.0 - w_center = 1.0 - w_bottom = 1.0 - w_vertical = 0.5 - w_prev = 0.75 - - prev_cx = self._prev_cx - - for i in range(1, num_labels): # 0 = fundo - x, y, w, h, area = stats[i] - if area < 50: # lixo - continue - - cx = x + w // 2 - # normalizações 0..1 - area_n = area / float(H * W) - center_n = 1.0 - min(1.0, abs(cx - cx_img) / (W * 0.5)) - vertical = h / max(1.0, w) # alongamento vertical - bottom_touch = 1.0 if (y + h >= H - 2) else 0.0 - - prev_bias = 0.0 - if prev_cx is not None: - prev_bias = 1.0 - min(1.0, abs(cx - prev_cx) / (W * 0.5)) - - score = (w_area*area_n + - w_center*center_n + - w_bottom*bottom_touch + - w_vertical*vertical + - w_prev*prev_bias) - - if score > best_score: - best_score = score - best_i = i - - self._prev_cx = None - if best_i == -1: - return np.zeros_like(mask_rua, dtype=np.uint8) - - # guarda cx do blob vencedor pro próximo frame - bx, by, bw, bh, _ = stats[best_i] - self._prev_cx = bx + bw // 2 - self._prev_y0 = by - - return (labels == best_i).astype(np.uint8) - - def _get_scanlines_y(self, H, grid_rows_y_px=None, near_is_bottom=True): - if grid_rows_y_px and len(grid_rows_y_px) >= 5: - ys = [int(np.clip(y, 0, H-1)) for y in grid_rows_y_px] - # reordena do "perto" para o "longe" - ys = sorted(ys, reverse=near_is_bottom) - return ys - # fallback por frações - fracs = [0.98, 0.85, 0.70, 0.55, 0.40, 0.25, 0.10] - return [min(H-1, max(0, int(H*f))) for f in fracs] - - def _analisar_corredor_visual_old(self, predictions, grid_rows_y_px=None, near_is_bottom=True): - self.predictions = predictions - H, W = self.predictions.shape - cx_img = W // 2 - - # 1) máscara do corredor principal - mask_main = self._extrair_corredor_principal(self.predictions).astype(bool) - mask_cana = (self.predictions == ClassesSegmentacao.CANA.value) - - # 2) scanlines (grid ou fallback) - ys = self._get_scanlines_y(H, grid_rows_y_px, near_is_bottom) - - centros, larguras = [], [] - for y in ys: - row = mask_main[y] - idx = np.flatnonzero(row) - if idx.size == 0: - centros.append((None, y)) - larguras.append(0) - else: - x0, x1 = idx[0], idx[-1] - cx = (x0 + x1) // 2 - w = (x1 - x0 + 1) - centros.append((int(cx), y)) - larguras.append(int(w)) - - # 3) Ângulo do corredor (rad) via ajuste linear x(y) - pts = [(x, y) for (x, y) in centros if x is not None] - ang_rad = None - 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) - - # pesos: linhas mais próximas ao robô pesam mais - # (primeiros ys na lista são "perto" se near_is_bottom=True) - n = len(ys_fit) - wts = np.linspace(1.0, 2.0, n).astype(np.float32) # simples e eficaz - - # polyfit ponderado (equivalente com normal equations) - # x = a*y + b - Wm = np.diag(wts) - Y = ys_fit.reshape(-1,1) - X = np.hstack([Y, np.ones_like(Y)]) - # a, b = (X^T W X)^-1 X^T W x - XtW = X.T @ Wm - beta = np.linalg.pinv(XtW @ X) @ (XtW @ xs_fit) - a = float(beta[0]) - - ang_rad = np.arctan(a) - # wrap correto em radianos - ang_rad = (ang_rad + np.pi) % (2*np.pi) - np.pi - - # 4) Erro lateral (% da largura na base) - erro_lateral_pct = 0.0 - if centros and centros[0][0] is not None: - x_base, y_base = centros[0] - w_base = max(1, larguras[0]) - err_px = cx_img - x_base - erro_lateral_pct = (err_px / w_base) * 100.0 - - # 5) Suavização (EMA) - if ang_rad is not None: - deg = np.degrees(ang_rad) - if self._ema_ang is None: - self._ema_ang = deg - else: - a = self._ema_alpha_ang - # unwrap simples para evitar saltos de ±180 - delta = ((deg - self._ema_ang + 180) % 360) - 180 - self._ema_ang = self._ema_ang + a * delta - ang_out = round(self._ema_ang, 3) - else: - ang_out = None - - if True: # sempre temos lateral pct numérico - if self._ema_lat is None: - self._ema_lat = erro_lateral_pct - else: - a = self._ema_alpha_lat - self._ema_lat = (1 - a) * self._ema_lat + a * erro_lateral_pct - lat_out = round(float(self._ema_lat), 3) - - # 6) Status baseado em presença nas scanlines - # proximidade: usa 2 mais perto e 2 mais longe - near_valid = sum(1 for (x,_) in centros[:2] if x is not None) - far_valid = sum(1 for (x,_) in centros[-2:] if x is not None) - any_valid = sum(1 for (x,_) in centros if x is not None) - - # --- STATUS pela regra das metades (CANA) --- - status_now, status_final, p_top, p_bot, has_top, has_bot = self._classificar_status_por_cana( - mask_cana.astype(np.uint8), mask_main, near_is_bottom=near_is_bottom - ) - - # 7) Persistência (maioria em N frames) - self._status_hist.append(status_now) - status_final = max(set(self._status_hist), key=self._status_hist.count) - self._last_status = status_final - - # 8) Confiança - confianca = any_valid / max(1, len(centros)) - - return { - "height": H, - "width": W, - "erro_angular": ang_out, - "erro_lateral_pct": lat_out, - "status_corredor": status_final.value, - "centros_corredor": centros, - "larguras_px": larguras, - "confianca": round(float(sum(1 for (x,_) in centros if x is not None) / max(1, len(centros))), 3), - - # DEBUG/telemetria úteis - "p_cana_top": round(float(p_top), 3), - "p_cana_bottom": round(float(p_bot), 3), - "has_top": bool(has_top), - "has_bottom": bool(has_bot), - } + except Exception as e: + print(f"Erro ao processar predictions: {e}") + return None def _analisar_corredor_visual(self, predictions, grid_rows_y_px=None, near_is_bottom=True): # --- inputs/base --- @@ -426,21 +97,18 @@ class SegmentacaoManager: H, W = self.predictions.shape cx_img = W // 2 - # máscara do "corredor principal" (telemetria/ângulo); pode vir vazia - mask_main = self._extrair_corredor_principal(self.predictions).astype(bool) - # opcional (se ainda usar em outros pontos) - mask_cana = (self.predictions == ClassesSegmentacao.CANA.value) + mask_corredor = self._extrair_corredor_principal(self.predictions).astype(bool) # --- score de corredor por grid (robusto) --- - score, centers_col, left_col, right_col, valid_row = self._corridor_score_grid(self.predictions) + score, centers_col, left_col, right_col = self._corridor_score_grid(self.predictions) # --- scanlines para centro/ângulo (telemetria) --- ys = self._get_scanlines_y(H, grid_rows_y_px, near_is_bottom) centros, larguras = [], [] - if mask_main.any(): + if mask_corredor.any(): for y in ys: - row = mask_main[y] + row = mask_corredor[y] idx = np.flatnonzero(row) if idx.size == 0: centros.append((None, y)) @@ -478,9 +146,9 @@ class SegmentacaoManager: # pesos: linhas "mais perto" pesam mais if near_is_bottom: - wts = np.linspace(1.0, 2.0, n, dtype=np.float32) # crescente da base ao topo do vetor pts - else: 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) @@ -521,7 +189,8 @@ 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) - status_now, status_final, p_top, p_bot, has_top, has_bot = self.classificar_por_cana(mask_cana, mask_main) + mask_nav = (self.predictions == ClassesSegmentacao.NAVEGAVEL.value) + status_now, status_final, prob, debug = self.classificar_status_corredor(mask_nav) return { "timestamp": time.time(), @@ -529,75 +198,74 @@ class SegmentacaoManager: "width": W, "erro_angular": ang_out, "erro_lateral_pct": lat_out, - "status_corredor": status_final.value, # mantém teu contrato atual (enum -> int) + "status_corredor": status_final.value, "centros_corredor": centros, "larguras_px": larguras, - "confianca": round(float(score), 3), # agora a confiança é o score de corredor - # (se quiser debugar) "near_ok": near_ok, "far_ok": far_ok, + "confianca": round(float(score), 3), } + def _extrair_corredor_principal(self, mask_nav): + H, W = mask_nav.shape + cx_img = W // 2 - def display_segmentation_debug(self, frame, largura_robo_px): - try: - original = cv2.resize(frame, self.resolucao) + num_labels, labels, stats, _ = cv2.connectedComponentsWithStats(mask_nav, connectivity=8) + if num_labels <= 1: + return np.zeros_like(mask_nav, dtype=np.uint8) - seg_color = np.zeros_like(original) - for class_id, color in enumerate(self.color_map): - seg_color[self.predictions == class_id] = color + best_i, best_score = -1, -1e9 - overlay = cv2.addWeighted(original, 0.5, seg_color, 0.5, 0) + # pesos do score + w_area = 1.0 + w_center = 1.0 + w_bottom = 1.0 + w_vertical = 0.5 + w_prev = 0.75 - centros_corredor = self.dados_visuais.get("centros_corredor") + prev_cx = self._prev_cx - # 1. Linha do centro do corredor + largura do robô - if centros_corredor and len(centros_corredor) >= 2: - for ponto in centros_corredor: - x, y = ponto - if x is None or y is None: continue - cv2.circle(overlay, ponto, 4, (0, 255, 255), -1) - for i in range(len(centros_corredor) - 1): - cv2.line(overlay, centros_corredor[i], centros_corredor[i + 1], (0, 255, 255), 2) - for ponto in centros_corredor: - x, y = ponto - if x is None or y is None: continue - cv2.line(overlay, (x - largura_robo_px // 2, y), (x + largura_robo_px // 2, y), (255, 0, 255), 1) + for i in range(1, num_labels): # 0 = fundo + x, y, w, h, area = stats[i] + if area < 50: # lixo + continue - # 4. Texto de métricas - if self.dados_visuais: - erro_lateral_pct = self.dados_visuais["erro_lateral_pct"] - erro_angular = np.degrees(self.dados_visuais["erro_angular"]) if self.dados_visuais["erro_angular"] else 0 - texto = [ - f"Erro angular: {erro_angular:.2f} graus", - f"Erro lateral: {erro_lateral_pct:.2f} %", - f"Status: {StatusCarroMapa(self.dados_visuais['status_corredor']).name}" - ] - for i, t in enumerate(texto): - cv2.putText(overlay, t, (10, 25 + i * 25), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 255), 2) + cx = x + w // 2 + # normalizações 0..1 + area_n = area / float(H * W) + center_n = 1.0 - min(1.0, abs(cx - cx_img) / (W * 0.5)) + vertical = h / max(1.0, w) # alongamento vertical + bottom_touch = 1.0 if (y + h >= H - 2) else 0.0 - # 5. Legenda das classes - legenda_inicio_y = 140 - for i, cor in enumerate(self.color_map): - nome = self.classes[i] - pos_y = legenda_inicio_y + i * 30 - cv2.rectangle(overlay, (10, pos_y - 15), (30, pos_y + 5), cor, -1) - cv2.putText(overlay, nome, (40, pos_y + 2), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255,255,255), 1, cv2.LINE_AA) + prev_bias = 0.0 + if prev_cx is not None: + prev_bias = 1.0 - min(1.0, abs(cx - prev_cx) / (W * 0.5)) - # Mostrar - cv2.imshow("Segmentacao - Overlay", overlay) - cv2.waitKey(1) - #cv2.destroyAllWindows() - except Exception as e: - print(f"Erro ao gerar display_segmentation_debug: {e}") + score = (w_area*area_n + + w_center*center_n + + w_bottom*bottom_touch + + w_vertical*vertical + + w_prev*prev_bias) + + if score > best_score: + best_score = score + best_i = i + + self._prev_cx = None + if best_i == -1: + return np.zeros_like(mask_nav, dtype=np.uint8) + + # guarda cx do blob vencedor pro próximo frame + bx, by, bw, bh, _ = stats[best_i] + self._prev_cx = bx + bw // 2 + self._prev_y0 = by + + return (labels == best_i).astype(np.uint8) def _corridor_score_grid(self, mask_classes, rows=6, cols=21): - CANA = ClassesSegmentacao.CANA.value - RUA = ClassesSegmentacao.RUA.value - H, W = mask_classes.shape row_h = H // rows col_w = W // cols - # fração de CANA por célula + # fração de NAO NAVEGAVEL por célula 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 @@ -605,11 +273,11 @@ class SegmentacaoManager: 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 == CANA) + frac[i, j] = np.mean(cell == ClassesSegmentacao.NAONAVEGAVEL.value) # limiares por faixa (modelo em “A”: mais chão embaixo, mais cana no topo) near_lado_min, near_canal_max = 0.05, 0.70 - far_lado_min, far_canal_max = 0.45, 0.15 + far_lado_min, far_canal_max = 0.25, 0.15 idx = np.linspace(0, 1, rows) # 0=embaixo (perto), 1=topo (longe) lado_min = near_lado_min + (far_lado_min - near_lado_min) * idx canal_max = near_canal_max + (far_canal_max - near_canal_max) * idx @@ -670,7 +338,225 @@ class SegmentacaoManager: left_col[i], right_col[i] = lf, rf score = valid_row.mean() # 0..1 - return float(score), centers, left_col, right_col, valid_row + return float(score), centers, left_col, right_col + def _get_scanlines_y(self, H, grid_rows_y_px=None, near_is_bottom=True): + if grid_rows_y_px and len(grid_rows_y_px) >= 5: + ys = [int(np.clip(y, 0, H-1)) for y in grid_rows_y_px] + # reordena do "perto" para o "longe" + ys = sorted(ys, reverse=near_is_bottom) + return ys + # fallback por frações + fracs = [0.98, 0.85, 0.70, 0.55, 0.40, 0.25, 0.10] + return [min(H-1, max(0, int(H*f))) for f in fracs] + + def classificar_status_corredor(self, mask_nav: np.ndarray): + """ + mask_nav: (H,W) com 1 = navegável, 0 = não-navegável + Retorna: + status_now : StatusCarroMapa + status_final : StatusCarroMapa (igual ao now, sem histerese por enquanto) + probs : dict[StatusCarroMapa, float] (aqui só 1.0 pro escolhido) + debug : métricas pra log + """ + H, W = mask_nav.shape + nav = (mask_nav > 0).astype(np.float32) + + def faixa_mean(y0, y1): + fatia = nav[y0:y1, :] + if fatia.size == 0: + return 0.0 + return float(fatia.mean()) + + # corta em 3 faixas: far (topo), mid (meio), near (embaixo) + y_far_top = 0 + y_far_bot = int(0.2 * H) + y_mid_top = y_far_bot + y_mid_bot = int(0.4 * H) + y_near_top = y_mid_bot + y_near_bot = H + + nav_far = faixa_mean(y_far_top, y_far_bot) + nav_mid = faixa_mean(y_mid_top, y_mid_bot) + nav_near = faixa_mean(y_near_top, y_near_bot) + nav_global = float(nav.mean()) if nav.size > 0 else 0.0 + + # limiares & delta + THR_NAV_ALTO = 0.95 # "quase tudo navegável" + THR_NAV_BAIXO = 0.30 # "quase nada navegável" + THR_NEAR_ALTO = 1.00 # near "100%" + DELTA = 0.05 # diferença mínima pra considerar > de verdade + + def maior_que(a, b): + return a > min(b - DELTA, 1.0) + + def maior_igual_que(a, b): + return a >= min(b - DELTA, 1.0) + + status = None + + # 1) PARADO: quase todo frame não navegável + if nav_global < THR_NAV_BAIXO: + status = StatusCarroMapa.Parado + + # 2) DIRECIONANDO: quase todo frame navegável + elif nav_global > THR_NAV_ALTO: + status = StatusCarroMapa.Direcionando + + else: + # 3) ENTRANDO RUA + cond_near_alto = nav_near >= THR_NEAR_ALTO + cond_near_gt_mid = maior_igual_que(nav_near, nav_mid) + cond_mid_gt_far = maior_que(nav_mid, nav_far) + + # 4) SAINDO RUA + cond_near_gt_mid2 = True or maior_que(nav_near, nav_mid) + cond_far_gt_mid = maior_que(nav_far, nav_mid) + + if cond_near_alto and cond_near_gt_mid and cond_mid_gt_far: + status = StatusCarroMapa.EntrandoRua + + elif cond_near_gt_mid2 and cond_far_gt_mid: + status = StatusCarroMapa.SaindoRua + + # 5) CAMINHANDO RUA (cone "normal" NEAR > MID > FAR) + elif cond_near_gt_mid and cond_mid_gt_far: + status = StatusCarroMapa.CaminhandoRua + + else: + # fallback: se ficar numa zona cinza, chama de Direcionando + status = StatusCarroMapa.Manobrando + + # monta probs "one-hot" + probs = {s: 0.0 for s in StatusCarroMapa} + probs[status] = 1.0 + + debug = { + "nav_near": nav_near, + "nav_mid": nav_mid, + "nav_far": nav_far, + "nav_global": nav_global, + "THR_NAV_ALTO": THR_NAV_ALTO, + "THR_NAV_BAIXO": THR_NAV_BAIXO, + "THR_NEAR_ALTO": THR_NEAR_ALTO, + } + + status_now = status + + # Histerese temporal: mantém teu esquema de histórico + self._status_hist.append((status_now, self._now())) + status_final = self._maioria_ultimos() + + return status_now, status_final, probs, debug + @staticmethod + def _now(): + # monotonic evita saltos de relógio + return time.monotonic() + def _bool_hysteresis(self, prev: bool, x: float, thr_on: float, thr_off: float) -> bool: + return (x >= (thr_off if prev else thr_on)) + + def _maioria_ultimos(self, janela_s: float | None = None) -> StatusCarroMapa: + """Maioria ponderada pelos últimos 'janela_s' segundos. + Se 'janela_s' None → usa self.janela_s.""" + J = self.janela_s if janela_s is None else float(janela_s) + t_now = self._now() + + # 1) limpa itens FORA da janela + while self._status_hist and (t_now - self._status_hist[0][1] > J): + self._status_hist.popleft() + + if not self._status_hist: + # fallback razoável + return StatusCarroMapa.Direcionando + + # 2) maioria simples (pode trocar por peso exponencial se quiser) + cont = {} + for st, _t in self._status_hist: + cont[st] = cont.get(st, 0) + 1 + + # regra de desempate: prioriza estado mais recente em caso de empate + top_freq = max(cont.values()) + empatados = [st for st, c in cont.items() if c == top_freq] + if len(empatados) == 1: + return empatados[0] + else: + # desempata olhando do fim pro início (mais recente primeiro) + for st, _t in reversed(self._status_hist): + if st in empatados: + return st + + + def largura_robo_px_por_distancia(self, d_m, largura_robo_m, largura_frame_px, fov_h_graus): + fov_h_rad = np.radians(fov_h_graus) + largura_real_visivel_m = 2.0 * d_m * np.tan(fov_h_rad / 2.0) + + if largura_real_visivel_m <= 1e-6: + return None + + frac = largura_robo_m / largura_real_visivel_m + frac = np.clip(frac, 0.0, 1.2) # evita exagero visual + + return int(frac * largura_frame_px) + + def display_segmentation_debug(self, frame, largura_robo_m, grid_ref): + try: + original = cv2.resize(frame, self.resolucao) + + seg_color = np.zeros_like(original) + for class_id in range(len(self.color_map)): + seg_color[self.predictions == class_id] = self.lut[class_id] + + overlay = cv2.addWeighted(original, 0.5, seg_color, 0.5, 0) + + centros_corredor = self.dados_visuais.get("centros_corredor") + + # 1. Linha do centro do corredor + largura do robô + if centros_corredor and len(centros_corredor) >= 2: + for ponto in centros_corredor: + x, y = ponto + if x is None or y is None: continue + cv2.circle(overlay, ponto, 4, (0, 255, 255), -1) + for i in range(len(centros_corredor) - 1): + cv2.line(overlay, centros_corredor[i], centros_corredor[i + 1], (0, 255, 255), 2) + H, W = overlay.shape[:2] + for idx, (x, y) in enumerate(centros_corredor): + if x is None or y is None: + continue + # mapear idx -> distância real + if idx >= len(grid_ref): + continue + d_m = grid_ref[len(grid_ref) - 1 - idx] + largura_px = self.largura_robo_px_por_distancia(d_m, largura_robo_m=largura_robo_m, largura_frame_px=W, fov_h_graus=69.0) + if largura_px is None: + continue + cv2.line(overlay, (x - largura_px // 2, y), (x + largura_px // 2, y), (255, 0, 255), 1) + + # 4. Texto de métricas + if self.dados_visuais: + erro_lateral_pct = self.dados_visuais["erro_lateral_pct"] + erro_angular = self.dados_visuais["erro_angular"] if self.dados_visuais["erro_angular"] else 0 + texto = [ + f"Erro angular: {erro_angular:.2f} graus", + f"Erro lateral: {erro_lateral_pct:.2f} %", + f"Status: {StatusCarroMapa(self.dados_visuais['status_corredor']).name}" + ] + for i, t in enumerate(texto): + cv2.putText(overlay, t, (10, 25 + i * 25), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 255), 2) + + # 5. Legenda das classes + legenda_inicio_y = 140 + for i, _ in enumerate(self.color_map): + nome = self.classes[i] + pos_y = legenda_inicio_y + i * 30 + cor_bgr = tuple(int(c) for c in self.lut[i]) + cv2.rectangle(overlay, (10, pos_y - 15), (30, pos_y + 5), cor_bgr, -1) + cv2.putText(overlay, nome, (40, pos_y + 2), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255,255,255), 1, cv2.LINE_AA) + + # Mostrar + cv2.imshow("Segmentacao - Overlay", overlay) + cv2.waitKey(1) + #cv2.destroyAllWindows() + except Exception as e: + print(f"Erro ao gerar display_segmentation_debug: {e}") diff --git a/Python/OAK/datasets/_2_rename_new_images.py b/Python/OAK/datasets/_2_rename_new_images.py new file mode 100644 index 000000000..3c811b0a6 --- /dev/null +++ b/Python/OAK/datasets/_2_rename_new_images.py @@ -0,0 +1,91 @@ +import argparse +from pathlib import Path +from datetime import datetime + + +def main(): + parser = argparse.ArgumentParser( + description="Renomeia imagens usando a data de modificação no formato ddMMyyyy_HHmmss.ext" + ) + parser.add_argument( + "--images_dir", + type=str, + required=True, + help="Pasta com as imagens (sem recursão). Ex: dataset/new_images", + ) + parser.add_argument( + "--force_jpeg", + action="store_true", + help="Se setado, força a extensão .jpeg em todos os arquivos.", + ) + parser.add_argument( + "--dry_run", + action="store_true", + help="Se setado, só mostra o que faria, sem renomear nada.", + ) + + args = parser.parse_args() + + images_dir = Path(args.images_dir) + if not images_dir.is_dir(): + raise SystemExit(f"Pasta não encontrada: {images_dir}") + + exts = {".jpg", ".jpeg", ".png", ".bmp", ".tif", ".tiff"} + + files = [ + p for p in sorted(images_dir.iterdir()) + if p.is_file() and p.suffix.lower() in exts + ] + + if not files: + print("[INFO] Nenhuma imagem encontrada na pasta.") + return + + print(f"[INFO] Encontrados {len(files)} arquivos de imagem em {images_dir}") + + # Pra evitar conflitos, vamos ir gerando nomes únicos + used_names = set() + + for src in files: + stat = src.stat() + mtime = stat.st_mtime + dt = datetime.fromtimestamp(mtime) + + base = dt.strftime("%d%m%Y_%H%M%S") + + if args.force_jpeg: + target_ext = ".jpeg" + else: + target_ext = src.suffix.lower() + + # Nome base desejado + new_name = f"{base}{target_ext}" + dst = images_dir / new_name + + # Se já existe (ou já usamos esse nome pra outro arquivo), adiciona sufixo _01, _02, ... + counter = 1 + while dst.exists() or dst.name in used_names: + new_name = f"{base}_{counter:02d}{target_ext}" + dst = images_dir / new_name + counter += 1 + + used_names.add(dst.name) + + if src == dst: + # já está com o nome correto + continue + + print(f"{src.name} -> {dst.name}") + + if not args.dry_run: + dst.parent.mkdir(parents=True, exist_ok=True) + src.rename(dst) + + if args.dry_run: + print("\n[INFO] DRY RUN: nada foi renomeado de verdade.") + else: + print("\n[OK] Renomeação concluída.") + + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/Python/OAK/datasets/_4_group_images_by_class_raw.py b/Python/OAK/datasets/_4_group_images_by_class_raw.py index 4a19f1bf7..03c15aa46 100644 --- a/Python/OAK/datasets/_4_group_images_by_class_raw.py +++ b/Python/OAK/datasets/_4_group_images_by_class_raw.py @@ -33,6 +33,7 @@ from utils import carregar_labelmap_completo, converter_mask_rgb_para_ids with open("config.json", "r", encoding="utf-8") as f: config = json.load(f) MODELO = config["camera"] +USE_MASKS2 = config["dual_head"] EXT_PREVIEWS = (".jpg", ".jpeg", ".png") EXT_MASKS = (".png", ".jpg", ".jpeg") @@ -221,7 +222,7 @@ def processar(originals_dir, labelmap_path, mover=False, if not os.path.isdir(previews_dir) or not os.path.isdir(raws_dir) or not os.path.isdir(masks_dir): raise RuntimeError("Estrutura inválida em originals/") - usar_masks2 = os.path.isdir(masks2_dir) + usar_masks2 = USE_MASKS2 and os.path.isdir(masks2_dir) cor_para_id, colormap_rgb, id_para_nome, ignore_rgb = carregar_labelmap_completo(labelmap_path) ignore_id = inferir_ignore_id(ignore_rgb, cor_para_id) diff --git a/Python/OAK/datasets/_5_augmentation_raw.py b/Python/OAK/datasets/_5_augmentation_raw.py index 4e199be4b..baa371de6 100644 --- a/Python/OAK/datasets/_5_augmentation_raw.py +++ b/Python/OAK/datasets/_5_augmentation_raw.py @@ -50,6 +50,7 @@ import numpy as np with open("config.json", "r", encoding="utf-8") as f: config = json.load(f) MODELO = config.get("camera", ".") +USE_MASKS2 = config.get("dual_head", False) # Pastas base DATASET_BASE = os.path.join(MODELO, "dataset") @@ -343,7 +344,7 @@ def process_group(group_name, copies): return 0 use_raw = os.path.isdir(raw_dir) - use_masks2 = os.path.isdir(msk2_dir) + use_masks2 = USE_MASKS2 and os.path.isdir(msk2_dir) imgs = [f for f in os.listdir(img_dir) if os.path.splitext(f.lower())[1] in IMG_EXTS] msk_map = map_by_base_priorizando_png(msk_dir, MSK_EXTS) @@ -399,7 +400,7 @@ def process_legacy(copies): imgs = [f for f in os.listdir(ORIG_OLD_IMG) if os.path.splitext(f.lower())[1] in IMG_EXTS] msk_map = map_by_base_priorizando_png(ORIG_OLD_MSK, MSK_EXTS) - use_masks2 = os.path.isdir(ORIG_OLD_MSK2) + use_masks2 = USE_MASKS2 and os.path.isdir(ORIG_OLD_MSK2) msk2_map = map_by_base_priorizando_png(ORIG_OLD_MSK2, MSK2_EXTS) if use_masks2 else {} img_out_dir, msk_out_dir, msk2_out_dir, _ = ensure_aug_dirs( diff --git a/Python/OAK/datasets/_6_normalize_raw.py b/Python/OAK/datasets/_6_normalize_raw.py index 31a3d57f6..eb914f526 100644 --- a/Python/OAK/datasets/_6_normalize_raw.py +++ b/Python/OAK/datasets/_6_normalize_raw.py @@ -33,6 +33,7 @@ with open("config.json", "r", encoding="utf-8") as f: config = json.load(f) MODELO = config["camera"] +USE_MASKS2 = config["dual_head"] RESOLUCAO = tuple(config["resolucao"]) # [W,H] pasta_base = os.path.join(MODELO, "dataset") labelmap_path = os.path.join(pasta_base, "labelmap.txt") @@ -170,7 +171,7 @@ def normalize_group_raw(fonte_root: str, fonte_nome: str, cor_para_id, ignore_id continue usar_raw = os.path.isdir(in_raw) - usar_msk2 = os.path.isdir(in_msk2) + usar_msk2 = USE_MASKS2 and os.path.isdir(in_msk2) out_prev = os.path.join(out_root, grupo, "previews") out_raw = os.path.join(out_root, grupo, "raws") if usar_raw else None diff --git a/Python/OAK/datasets/_7_split_raw.py b/Python/OAK/datasets/_7_split_raw.py index cf4218910..1da662353 100644 --- a/Python/OAK/datasets/_7_split_raw.py +++ b/Python/OAK/datasets/_7_split_raw.py @@ -29,6 +29,7 @@ import argparse with open("config.json", "r", encoding="utf-8") as f: config = json.load(f) MODELO = config.get("camera") +USE_MASKS2 = config.get("dual_head", False) RESOLUCAO = tuple(config.get("resolucao")) # Pastas (ajustadas para PREVIEWS/RAWS) @@ -270,7 +271,7 @@ def split_group(group_name, p_train, p_val, p_test, seed, mins, caps_map=None): src_msk2_dir = os.path.join(pasta_origem, group_name, "masks2") src_raw_dir = os.path.join(pasta_origem, group_name, "raws") - use_msk2 = os.path.isdir(src_msk2_dir) + use_msk2 = USE_MASKS2 and os.path.isdir(src_msk2_dir) use_raw = os.path.isdir(src_raw_dir) familias = build_family_index(src_prev_dir, src_msk_dir) diff --git a/Python/OAK/datasets/_8_train_segformer_b3.py b/Python/OAK/datasets/_8_train_segformer_b3.py index 94cdd18bf..65c0ade1b 100644 --- a/Python/OAK/datasets/_8_train_segformer_b3.py +++ b/Python/OAK/datasets/_8_train_segformer_b3.py @@ -1,4 +1,4 @@ -#python _8_train_segformer_b3.py --epochs 110 --batch 2 --lr 3e-5 --wd 0.01 --num_workers 4 --amp --amp_val --grad_accum 2 --class_weights auto --main_class navegavel --resume +#python _8_train_segformer_b3.py --epochs 180 --batch 2 --lr 3e-5 --wd 0.01 --num_workers 4 --amp --amp_val --grad_accum 2 --class_weights auto --main_class navegavel --resume # _8_train_segformer_b3.py (PATCH) import os diff --git a/Python/OAK/datasets/_9_test_segformer_b3.py b/Python/OAK/datasets/_9_test_segformer_b3.py index 68a848e7d..c67fc0b9d 100644 --- a/Python/OAK/datasets/_9_test_segformer_b3.py +++ b/Python/OAK/datasets/_9_test_segformer_b3.py @@ -14,6 +14,8 @@ Obs: Este script assume que seus ids de classe batem com o labelmap.txt (mask em IDs 0..K-1). """ +from collections import deque +from enum import IntEnum import json import os import time @@ -206,6 +208,7 @@ def main(): parser.add_argument("--camera", action="store_true", help="Usar câmera em vez de imagens") parser.add_argument("--groups", type=str, default=None, help="Filtrar grupos (ex: chao,erva_cana)") parser.add_argument("--split_folder", type=str, default="val", help="split padrão (se usar split/val/test)") + parser.add_argument("--test_folder", type=str, default=None, help="pasta para teste") args = parser.parse_args() device = torch.device("cuda" if torch.cuda.is_available() else "cpu") @@ -264,7 +267,7 @@ def main(): pipeline = dai.Pipeline() cam_rgb = pipeline.createColorCamera() cam_rgb.setResolution(dai.ColorCameraProperties.SensorResolution.THE_1080_P) - cam_rgb.setBoardSocket(dai.CameraBoardSocket.RGB) + cam_rgb.setBoardSocket(dai.CameraBoardSocket.CAM_A) cam_rgb.setColorOrder(dai.ColorCameraProperties.ColorOrder.RGB) cam_rgb.setInterleaved(False) cam_rgb.setFps(30) @@ -279,7 +282,8 @@ def main(): prev_time = time.time() while True: in_rgb = rgb_queue.get() - frame = in_rgb.getCvFrame() # RGB + frame_bgr = in_rgb.getCvFrame() # BGR + frame = cv2.cvtColor(frame_bgr, cv2.COLOR_BGR2RGB) H, W = frame.shape[:2] y_fim, y_inicio = compute_roi_indices(H, ROI_INICIO, ROI_TAMANHO) @@ -325,38 +329,76 @@ def main(): else: # === Modo imagens (agrupado + fallback) === - # Igual teu fastscnn: você pode usar dataset/512x288 diretamente - test_root = os.path.join(dataset_path, "512x288") + def collect_images_only(folder): + exts = (".jpg", ".jpeg", ".png") + paths = [ + os.path.join(folder, f) + for f in sorted(os.listdir(folder)) + if f.lower().endswith(exts) + ] + return paths + + test_root = args.test_folder if args.test_folder else "" - image_paths, mask_paths, groups_idx = collect_pairs_grouped(test_root, want_groups=args.groups) - if not image_paths: - # fallback clássico + image_paths = [] + mask_paths = [] + groups_idx = [] + + if test_root: + # tenta modo "imagens puras" + image_paths = collect_images_only(test_root) + if image_paths: + mask_paths = None + groups_idx = None + print(f"[TEST] Modo inferência pura: {len(image_paths)} imagens") + else: + # fallback: dataset estruturado + image_paths, mask_paths, groups_idx = collect_pairs_grouped( + test_root, want_groups=args.groups + ) + else: test_root = os.path.join(dataset_path, "split", args.split_folder) - image_paths, mask_paths, groups_idx = collect_pairs_grouped(test_root, want_groups=args.groups) - if not image_paths: - image_paths, mask_paths, groups_idx = collect_pairs_legacy(test_root) + image_paths, mask_paths, groups_idx = collect_pairs_grouped( + test_root, want_groups=args.groups + ) + if not image_paths: + image_paths, mask_paths, groups_idx = collect_pairs_legacy(test_root) - assert len(image_paths) == len(mask_paths) and len(image_paths) > 0, "Nenhuma imagem/máscara encontrada." + assert len(image_paths) > 0, "Nenhuma imagem encontrada." + if mask_paths is not None: + assert len(image_paths) == len(mask_paths), "Mismatch imagem/máscara" idx = 0 window_name = "Original | GroundTruth | Predito (SegFormer)" cv2.namedWindow(window_name, cv2.WINDOW_NORMAL) # permite redimensionar/maximizar while True: img_path = image_paths[idx] - mask_path = mask_paths[idx] grupo = groups_idx[idx] if groups_idx else "?" img_rgb = np.array(Image.open(img_path).convert("RGB")) - mask_gt = np.array(Image.open(mask_path).convert("L")) + + if mask_paths is not None: + mask_path = mask_paths[idx] + mask_gt = np.array(Image.open(mask_path).convert("L")) + else: + mask_gt = None H, W = img_rgb.shape[:2] y_fim, y_inicio = compute_roi_indices(H, ROI_INICIO, ROI_TAMANHO) img_roi = img_rgb[y_fim:y_inicio, 0:W] - mask_roi = mask_gt[y_fim:y_inicio, 0:W] + + if mask_gt is not None: + mask_roi = mask_gt[y_fim:y_inicio, 0:W] + else: + mask_roi = None img_resized = resize_keep_width(img_roi, RESOLUCAO[0], RESOLUCAO[1], cv2.INTER_AREA) - mask_resized = resize_keep_width(mask_roi, RESOLUCAO[0], RESOLUCAO[1], cv2.INTER_NEAREST) + + if mask_roi is not None: + mask_resized = resize_keep_width(mask_roi, RESOLUCAO[0], RESOLUCAO[1], cv2.INTER_NEAREST) + else: + mask_resized = None img_norm = img_resized.astype(np.float32) / 255.0 img_tensor = torch.from_numpy(img_norm).permute(2, 0, 1).unsqueeze(0).to(device) @@ -366,16 +408,23 @@ def main(): pred_ids = segformer_predict_ids(model, img_tensor) pred_rgb = converter_mask_ids_para_rgb(pred_ids, colormap_rgb, ignore_id) - mask_gt_rgb = converter_mask_ids_para_rgb(mask_resized, colormap_rgb, ignore_id) - #resultado = np.concatenate([img_resized, mask_gt_rgb, pred_rgb], axis=1) - # Overlay da predição sobre a imagem original + overlay_pred = cv2.addWeighted(img_resized, 0.6, pred_rgb, 0.4, 0.0) - resultado = np.concatenate([img_resized, mask_gt_rgb, overlay_pred], axis=1) + if mask_gt is not None: + mask_gt_rgb = converter_mask_ids_para_rgb(mask_resized, colormap_rgb, ignore_id) + resultado = np.concatenate([img_resized, mask_gt_rgb, overlay_pred], axis=1) + else: + resultado = np.concatenate([img_resized, overlay_pred], axis=1) legenda = desenhar_legenda_horizontal(colormap_rgb, classes) legenda_resized = cv2.resize(legenda, (resultado.shape[1], legenda.shape[0]), interpolation=cv2.INTER_NEAREST) resultado_completo = np.concatenate([resultado, legenda_resized], axis=0) + + mask_nav = (pred_ids == ClassesSegmentacao.NAVEGAVEL.value) + classificar_status_corredor(mask_nav) + + cv2.putText(resultado_completo, f"grupo: {grupo}", (10, 24), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2) # Converte pra BGR pra exibir vis_bgr = cv2.cvtColor(resultado_completo, cv2.COLOR_RGB2BGR) @@ -401,5 +450,160 @@ def main(): cv2.destroyAllWindows() + +class ClassesSegmentacao(IntEnum): + NAONAVEGAVEL = 0 + NAVEGAVEL = 1 + +class StatusCarroMapa(IntEnum): + Parado = 0 + EntrandoRua = 1, + CaminhandoRua = 2 + SaindoRua = 3, + Manobrando = 4 + Direcionando = 5 + RetornandoBase = 6 + +janela_s_padrao = 1.5 +_status_hist = deque(maxlen=1) + +def _now(): + # monotonic evita saltos de relógio + return time.monotonic() + +def _maioria_ultimos(janela_s: float | None = None) -> StatusCarroMapa: + """Maioria ponderada pelos últimos 'janela_s' segundos. + Se 'janela_s' for None → usa janela_s_padrao.""" + J = float(janela_s) if janela_s is not None else float(janela_s_padrao) + + t_now = _now() + + # 1) limpa itens FORA da janela + while _status_hist and (t_now - _status_hist[0][1] > J): + _status_hist.popleft() + + if not _status_hist: + # fallback razoável + return StatusCarroMapa.Direcionando + + # 2) maioria simples (pode trocar por peso exponencial se quiser) + cont: dict[StatusCarroMapa, int] = {} + for st, _t in _status_hist: + cont[st] = cont.get(st, 0) + 1 + + top_freq = max(cont.values()) + empatados = [st for st, c in cont.items() if c == top_freq] + + if len(empatados) == 1: + return empatados[0] + + # desempata olhando do fim pro início (mais recente primeiro) + for st, _t in reversed(_status_hist): + if st in empatados: + return st + +def classificar_status_corredor(mask_nav: np.ndarray): + """ + mask_nav: (H,W) com 1 = navegável, 0 = não-navegável + Retorna: + status_now : StatusCarroMapa + status_final : StatusCarroMapa (igual ao now, sem histerese por enquanto) + probs : dict[StatusCarroMapa, float] (aqui só 1.0 pro escolhido) + debug : métricas pra log + """ + H, W = mask_nav.shape + nav = (mask_nav > 0).astype(np.float32) + + def faixa_mean(y0, y1): + fatia = nav[y0:y1, :] + if fatia.size == 0: + return 0.0 + return float(fatia.mean()) + + # corta em 3 faixas: far (topo), mid (meio), near (embaixo) + y_far_top = 0 + y_far_bot = int(0.2 * H) + y_mid_top = y_far_bot + y_mid_bot = int(0.4 * H) + y_near_top = y_mid_bot + y_near_bot = H + + nav_far = faixa_mean(y_far_top, y_far_bot) + nav_mid = faixa_mean(y_mid_top, y_mid_bot) + nav_near = faixa_mean(y_near_top, y_near_bot) + nav_global = float(nav.mean()) if nav.size > 0 else 0.0 + + # limiares & delta + THR_NAV_ALTO = 0.95 # "quase tudo navegável" + THR_NAV_BAIXO = 0.30 # "quase nada navegável" + THR_NEAR_ALTO = 1.00 # near "100%" + DELTA = 0.05 # diferença mínima pra considerar > de verdade + + def maior_que(a, b): + return a > min(b - DELTA, 1.0) + + def maior_igual_que(a, b): + return a >= min(b - DELTA, 1.0) + + status = None + + # 1) PARADO: quase todo frame não navegável + if nav_global < THR_NAV_BAIXO: + status = StatusCarroMapa.Parado + + # 2) DIRECIONANDO: quase todo frame navegável + elif nav_global > THR_NAV_ALTO: + status = StatusCarroMapa.Direcionando + + else: + # 3) ENTRANDO RUA + cond_near_alto = nav_near >= THR_NEAR_ALTO + cond_near_gt_mid = maior_igual_que(nav_near, nav_mid) + cond_mid_gt_far = maior_que(nav_mid, nav_far) + + # 4) SAINDO RUA + cond_near_gt_mid2 = True or maior_que(nav_near, nav_mid) + cond_far_gt_mid = maior_que(nav_far, nav_mid) + + if cond_near_alto and cond_near_gt_mid and cond_mid_gt_far: + status = StatusCarroMapa.EntrandoRua + + elif cond_near_gt_mid2 and cond_far_gt_mid: + status = StatusCarroMapa.SaindoRua + + # 5) CAMINHANDO RUA (cone "normal" NEAR > MID > FAR) + elif cond_near_gt_mid and cond_mid_gt_far: + status = StatusCarroMapa.CaminhandoRua + + else: + # fallback: se ficar numa zona cinza, chama de Direcionando + status = StatusCarroMapa.Manobrando + + # monta probs "one-hot" + probs = {s: 0.0 for s in StatusCarroMapa} + probs[status] = 1.0 + + debug = { + "nav_near": nav_near, + "nav_mid": nav_mid, + "nav_far": nav_far, + "nav_global": nav_global, + "THR_NAV_ALTO": THR_NAV_ALTO, + "THR_NAV_BAIXO": THR_NAV_BAIXO, + "THR_NEAR_ALTO": THR_NEAR_ALTO, + } + + status_now = status + print(status_now.name) + # Histerese temporal: mantém teu esquema de histórico + _status_hist.append((status_now, _now())) + status_final = _maioria_ultimos() + + return status_now, status_final, probs, debug + + + + + if __name__ == "__main__": main() diff --git a/Python/OAK/datasets/config.json b/Python/OAK/datasets/config.json index 41f04133b..adb90c7f1 100644 --- a/Python/OAK/datasets/config.json +++ b/Python/OAK/datasets/config.json @@ -1,17 +1,17 @@ { - "camera": "gal5000", + "camera": "oak-d", "modelo": "segformer_b0", - "model_name": "ndvi_big", + "model_name": "nav", "dual_head": false, - "main_class_name": "erva", + "main_class_name": "navegavel", "es_classes": "", "model_to_use": "geral", "raw_size": [1296, 1028], - "resolucao": [1008, 800], + "resolucao": [1024, 576], "roi_inicio": 0.0, "roi_tamanho": 1.0, "shaves": 3, - "channels": 5, + "channels": 3, "use_ndvi": true, "backbone": "nvidia/segformer-b0-finetuned-ade-512-512" } \ No newline at end of file diff --git a/Python/OAK/datasets/config_oak.json b/Python/OAK/datasets/config_gal.json similarity index 67% rename from Python/OAK/datasets/config_oak.json rename to Python/OAK/datasets/config_gal.json index adb90c7f1..41f04133b 100644 --- a/Python/OAK/datasets/config_oak.json +++ b/Python/OAK/datasets/config_gal.json @@ -1,17 +1,17 @@ { - "camera": "oak-d", + "camera": "gal5000", "modelo": "segformer_b0", - "model_name": "nav", + "model_name": "ndvi_big", "dual_head": false, - "main_class_name": "navegavel", + "main_class_name": "erva", "es_classes": "", "model_to_use": "geral", "raw_size": [1296, 1028], - "resolucao": [1024, 576], + "resolucao": [1008, 800], "roi_inicio": 0.0, "roi_tamanho": 1.0, "shaves": 3, - "channels": 3, + "channels": 5, "use_ndvi": true, "backbone": "nvidia/segformer-b0-finetuned-ade-512-512" } \ No newline at end of file