diff --git a/AgroBase/AgroBase/bin/x64/Debug/Parametros/version_files.vsf b/AgroBase/AgroBase/bin/x64/Debug/Parametros/version_files.vsf index e10e6ffdf..e78345430 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Parametros/version_files.vsf +++ b/AgroBase/AgroBase/bin/x64/Debug/Parametros/version_files.vsf @@ -19,6 +19,15 @@ }, { "id": 3, + "Arquivo": "model", + "Diretorio": "C:\\AgroBaseModels\\Ervas\\", + "Extensao": ".json", + "Versao": "1_1", + "TipoArquivo": 1, + "ArquivoDownload": "models/weed_detector_normstats-1_1.json" + }, + { + "id": 4, "Arquivo": "modelseg", "Diretorio": "C:\\AgroBaseModels\\Ruas\\", "Extensao": ".pt", @@ -27,7 +36,7 @@ "ArquivoDownload": "models/street_detector_model_seg-1_3.pt", }, { - "id": 4, + "id": 5, "Arquivo": "modelseg", "Diretorio": "C:\\AgroBaseModels\\Ruas\\", "Extensao": ".txt", @@ -36,7 +45,16 @@ "ArquivoDownload": "models/street_detector_labelmap_seg-1_3.txt" }, { - "id": 5, + "id": 6, + "Arquivo": "modelseg", + "Diretorio": "C:\\AgroBaseModels\\Ruas\\", + "Extensao": ".json", + "Versao": "1_1", + "TipoArquivo": 0, + "ArquivoDownload": "models/street_detector_normstats-1_1.json" + }, + { + "id": 7, "Arquivo": "modeldet", "Diretorio": "C:\\AgroBaseModels\\Ruas\\", "Extensao": ".blob", @@ -45,7 +63,7 @@ "ArquivoDownload": "models/street_detector_model_det-1_0.blob", }, { - "id": 6, + "id": 8, "Arquivo": "parametersAtu", "Diretorio": "Parametros/", "Extensao": ".par", @@ -54,7 +72,7 @@ "ArquivoDownload": "parameters/parametersAtu-1_1.par", }, { - "id": 7, + "id": 9, "Arquivo": "parametersMvd", "Diretorio": "Parametros/", "Extensao": ".par", @@ -63,7 +81,7 @@ "ArquivoDownload": "parameters/parametersMvd-2_0.par" }, { - "id": 8, + "id": 10, "Arquivo": "parametersSen", "Diretorio": "Parametros/", "Extensao": ".par", @@ -72,7 +90,7 @@ "ArquivoDownload": "parameters/parametersSen-1_3.par" }, { - "id": 9, + "id": 11, "Arquivo": "pinoutAtu", "Diretorio": "Parametros/", "Extensao": ".pin", @@ -81,7 +99,7 @@ "ArquivoDownload": "parameters/pinoutAtu-1_0.pin" }, { - "id": 10, + "id": 12, "Arquivo": "pinoutSen", "Diretorio": "Parametros/", "Extensao": ".pin", @@ -90,7 +108,7 @@ "ArquivoDownload": "parameters/pinoutSen-1_2.pin" }, { - "id": 11, + "id": 13, "Arquivo": "weed_detector_oak", "Diretorio": "Python\\Scripts\\", "Extensao": ".py", @@ -99,7 +117,7 @@ "ArquivoDownload": "weed_detector_oak-1_0.py" }, { - "id": 12, + "id": 14, "Arquivo": "map_load", "Diretorio": "Python\\Scripts\\", "Extensao": ".py", @@ -108,7 +126,7 @@ "ArquivoDownload": "scripts/map_load-1_0.py" }, { - "id": 13, + "id": 15, "Arquivo": "map_follow", "Diretorio": "Python\\Scripts\\", "Extensao": ".py", @@ -117,7 +135,7 @@ "ArquivoDownload": "scripts/map_follow-1_0.py" }, { - "id": 14, + "id": 16, "Arquivo": "gps_viewer", "Diretorio": "Python\\Scripts\\", "Extensao": ".py", @@ -126,7 +144,7 @@ "ArquivoDownload": "scripts/gps_viewer-1_0.py" }, { - "id": 15, + "id": 17, "Arquivo": "modelo_3d", "Diretorio": "Python\\Output\\", "Extensao": ".obj", @@ -135,21 +153,12 @@ "ArquivoDownload": "modelo_3d-1_0.obj" }, { - "id": 16, + "id": 18, "Arquivo": "modelo_3d", "Diretorio": "Python\\Output\\", "Extensao": ".mtl", "Versao": "1_0", "TipoArquivo": 14, "ArquivoDownload": "modelo_3d-1_0.mtl" - }, - { - "id": 17, - "Arquivo": "model", - "Diretorio": "C:\\AgroBaseModels\\Ervas\\", - "Extensao": ".json", - "Versao": "1_1", - "TipoArquivo": 1, - "ArquivoDownload": "models/weed_detector_normstats-1_1.json" - }, + } ] \ No newline at end of file diff --git a/Python/OAK/datasets/_6_normalize.py b/Python/OAK/datasets/_6_normalize.py index 348f64123..2a2845045 100644 --- a/Python/OAK/datasets/_6_normalize.py +++ b/Python/OAK/datasets/_6_normalize.py @@ -22,16 +22,20 @@ import os import json import cv2 from typing import Dict, List, Tuple + +import numpy as np from utils import carregar_labelmap_completo, converter_mask_rgb_para_ids # ⚙️ Configurações with open("config.json", "r", encoding="utf-8") as f: config = json.load(f) MODELO = config["camera"] +MODEL_NAME = config["model_name"] USE_MASKS2 = config["dual_head"] RESOLUCAO = tuple(config["resolucao"]) # [W, H] ou [width, height] pasta_base = os.path.join(MODELO, "dataset") labelmap_path = os.path.join(pasta_base, "labelmap.txt") +save_path = os.path.join(MODELO, "backup", config["modelo"], MODEL_NAME) # Dimensões alvo (pode expandir para múltiplas se quiser) RESOLUCOES = { @@ -46,6 +50,11 @@ IMG_EXTS = (".jpg", ".jpeg", ".png") MSK_EXTS = (".png", ".jpg", ".jpeg") # preferir .png MSK2_EXTS = (".png", ".jpg", ".jpeg") # idem +# === Acumuladores globais para mean/std dos canais RAW4 === +GLOBAL_SUM = None # soma por canal +GLOBAL_SUMSQ = None # soma dos quadrados por canal +GLOBAL_PIXELS = 0 # n de pixels por canal (H*W por imagem) + def infer_ignore_id(ignore_rgb, default_id=255): """ Tenta inferir o ID de ignore a partir do valor retornado por carregar_labelmap_completo. @@ -119,8 +128,7 @@ def map_masks2_by_base(msk2_dir: str) -> Dict[str, str]: by_base[base] = cand return by_base -def normalize_pair(caminho_rgb: str, caminho_mask: str, cor_para_id, ignore_id: int, - out_img_dir: str, out_msk_dir: str, dim: Tuple[int,int], prefix: str = ""): +def normalize_pair(caminho_rgb: str, caminho_mask: str, cor_para_id, ignore_id: int, out_img_dir: str, out_msk_dir: str, dim: Tuple[int,int], prefix: str = ""): """Redimensiona e grava a imagem e a máscara (se houver).""" img_rgb = cv2.imread(caminho_rgb) if img_rgb is None: @@ -141,6 +149,18 @@ def normalize_pair(caminho_rgb: str, caminho_mask: str, cor_para_id, ignore_id: # Redimensiona imagem img_resized = cv2.resize(img_rgb, dim, interpolation=cv2.INTER_AREA) + + global GLOBAL_SUM, GLOBAL_SUMSQ, GLOBAL_PIXELS + img_float = img_resized.astype(np.float32) / 255.0 # 0-1 + h, w, c = img_float.shape + flat = img_float.reshape(-1, c).astype(np.float64) + if (GLOBAL_SUM is None): + GLOBAL_SUM = np.zeros(c, dtype=np.float64) + GLOBAL_SUMSQ = np.zeros(c, dtype=np.float64) + GLOBAL_SUM += flat.sum(axis=0) + GLOBAL_SUMSQ += (flat ** 2).sum(axis=0) + GLOBAL_PIXELS += h * w + garantir_dir(out_img_dir) cv2.imwrite(os.path.join(out_img_dir, nome_saida_img), img_resized) @@ -156,6 +176,7 @@ def normalize_pair(caminho_rgb: str, caminho_mask: str, cor_para_id, ignore_id: garantir_dir(out_msk_dir) cv2.imwrite(os.path.join(out_msk_dir, nome_saida_msk), mask_resized) + return True def normalize_pair_mask2(caminho_rgb: str, caminho_mask2: str, @@ -316,6 +337,40 @@ def main(args): print(f"\n✅ Concluído! Total normalizados: {total_geral}") + # === calcula mean/std globais e salva em JSON === + global GLOBAL_SUM, GLOBAL_SUMSQ, GLOBAL_PIXELS + if GLOBAL_SUM is not None and GLOBAL_PIXELS > 0: + # média e variância por canal + mean = (GLOBAL_SUM / GLOBAL_PIXELS) + var = (GLOBAL_SUMSQ / GLOBAL_PIXELS) - mean**2 + std = np.sqrt(np.maximum(var, 1e-6)) + + # Converte para list pra salvar em JSON + mean_list = mean.tolist() + std_list = std.tolist() + + # Se quiser, você pode nomear os canais explicitamente + # dependendo da convenção do raw4: + channel_names = ["R", "G", "IR", "B"] + + stats = { + "channels": channel_names[:len(mean_list)], + "mean": mean_list, + "std": std_list, + "pixels_per_channel": int(GLOBAL_PIXELS), + } + + garantir_dir(save_path) + stats_path = os.path.join(save_path, "norm_stats.json") + with open(stats_path, "w", encoding="utf-8") as f: + json.dump(stats, f, indent=2, ensure_ascii=False) + + print(f"📁 Stats salvos em: {stats_path}") + print(f" mean: {mean_list}") + print(f" std : {std_list}") + else: + print("⚠️ Nenhum RAW processado, não há stats para salvar.") + if __name__ == "__main__": ap = argparse.ArgumentParser(description="Augmentação por grupos (images/masks)") ap.add_argument("--groups-except", type=str, default="", help="Lista de grupos para nao usar, separados por vírgula (ex: chao,erva_cana).") diff --git a/Python/OAK/datasets/_8_train_segformer_b3.py b/Python/OAK/datasets/_8_train_segformer_b3.py index 65c0ade1b..533b47e66 100644 --- a/Python/OAK/datasets/_8_train_segformer_b3.py +++ b/Python/OAK/datasets/_8_train_segformer_b3.py @@ -87,10 +87,12 @@ def compute_pixel_acc_from_cm(cm: torch.Tensor, eps: float = 1e-6) -> float: return acc -IMAGENET_MEAN = torch.tensor([0.485, 0.456, 0.406]).view(3, 1, 1) -IMAGENET_STD = torch.tensor([0.229, 0.224, 0.225]).view(3, 1, 1) +IMAGENET_MEAN = [0.485, 0.456, 0.406] +IMAGENET_STD = [0.229, 0.224, 0.225] +NORM_MEAN = torch.tensor(IMAGENET_MEAN).view(3, 1, 1) +NORM_STD = torch.tensor(IMAGENET_STD).view(3, 1, 1).clamp_min(1e-6) def normalize_img(img: torch.Tensor) -> torch.Tensor: - return (img - IMAGENET_MEAN.to(img.device)) / IMAGENET_STD.to(img.device) + return (img - NORM_MEAN.to(img.device)) / NORM_STD.to(img.device) def default_collate(batch): @@ -276,6 +278,7 @@ def main(): parser.add_argument("--main_class", type=str, default=None) parser.add_argument("--es_classes", type=str, default="") parser.add_argument("--seed", type=int, default=42) + parser.add_argument("--norm_stats", type=str, default=None, help="Caminho para JSON com mean/std por canal (ex: norm_stats.json).") args = parser.parse_args() set_seed(args.seed) @@ -397,6 +400,51 @@ def main(): scaler = GradScaler(enabled=args.amp and device.type == "cuda") + + + # ========================== + # Normalizador (fixo ou dinâmico) + # ========================== + global NORM_MEAN, NORM_STD + norm_stats = None + + # Caminho padrão: dentro do dataset, nome do arquivo de stats + # (ajusta aqui pro nome que você realmente usou: norm_stats.json, por ex.) + norm_stats_path = os.path.join(save_path, "norm_stats.json") + if args.norm_stats is not None: + norm_stats_path = args.norm_stats + + if norm_stats_path is not None and os.path.exists(norm_stats_path): + with open(norm_stats_path, "r", encoding="utf-8") as f: + norm_stats = json.load(f) + + stats_channels = norm_stats.get("channels", []) + stats_mean = norm_stats.get("mean", []) + stats_std = norm_stats.get("std", []) + + print(f"[NORM] usando stats fixos de: {norm_stats_path}") + print(f"[NORM] channels={stats_channels}") + print(f"[NORM] mean={stats_mean}") + print(f"[NORM] std ={stats_std}") + + # Garante que temos pelo menos R,G,B + idx_by_name = {name: i for i, name in enumerate(stats_channels)} + required = ["R", "G", "B"] + if not all(ch in idx_by_name for ch in required): + print("[NORM] AVISO: norm_stats não contém todos os canais R,G,B. Mantendo normalize imagenet.") + else: + NORM_MEAN = torch.tensor(stats_mean, dtype=torch.float32, device=device).view(3, 1, 1) + NORM_STD = torch.tensor(stats_std, dtype=torch.float32, device=device).view(3, 1, 1).clamp_min(1e-6) + + print("[NORM] Normalização fixa por canal ativada para [R,G,B].") + else: + if norm_stats_path: + print(f"[NORM] Caminho de norm_stats não encontrado: {norm_stats_path}. Usando normalize imagenet.") + else: + print("[NORM] norm_stats não informado. Usando normalize imagenet.") + + + start_epoch = 1 best_miou = -1.0 best_main_iou = -1.0 diff --git a/Python/OAK/datasets/_9_test_segformer_b3.py b/Python/OAK/datasets/_9_test_segformer_b3.py index c67fc0b9d..74addcc7b 100644 --- a/Python/OAK/datasets/_9_test_segformer_b3.py +++ b/Python/OAK/datasets/_9_test_segformer_b3.py @@ -19,6 +19,7 @@ from enum import IntEnum import json import os import time +from typing import Dict, Tuple import cv2 import glob import argparse @@ -309,7 +310,11 @@ def main(): now = time.time() fps = 1.0 / max(1e-6, (now - prev_time)) prev_time = now - cv2.putText(overlay, f"FPS: {fps:.1f}", (10, 30), + + mask_nav = (pred_ids == ClassesSegmentacao.NAVEGAVEL.value) + status_now, status_final, debug = classificar_status_corredor(mask_nav) + + cv2.putText(overlay, f"FPS: {fps:.1f} - {status_now.name}", (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 255, 0), 2) legenda = desenhar_legenda_vertical(colormap_rgb, classes) @@ -455,30 +460,37 @@ class ClassesSegmentacao(IntEnum): NAONAVEGAVEL = 0 NAVEGAVEL = 1 + class StatusCarroMapa(IntEnum): Parado = 0 - EntrandoRua = 1, + EntrandoRua = 1 CaminhandoRua = 2 - SaindoRua = 3, + SaindoRua = 3 Manobrando = 4 Direcionando = 5 - RetornandoBase = 6 + RetornandoBase = 6 # não usamos aqui, mas mantido + Indefinido = 7 + + +# ===== HISTER ESE TEMPORAL ===== janela_s_padrao = 1.5 -_status_hist = deque(maxlen=1) +# deixa espaço pra um histórico decente, a janela em segundos faz o recorte real +_status_hist = deque(maxlen=60) -def _now(): - # monotonic evita saltos de relógio + +def _now() -> float: + """Tempo monotônico (não sofre com ajustes de relógio do SO).""" 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.""" + """Maioria dos statuses dentro da última janela de tempo (em segundos).""" J = float(janela_s) if janela_s is not None else float(janela_s_padrao) t_now = _now() - # 1) limpa itens FORA da janela + # limpa itens FORA da janela while _status_hist and (t_now - _status_hist[0][1] > J): _status_hist.popleft() @@ -486,8 +498,8 @@ def _maioria_ultimos(janela_s: float | None = None) -> StatusCarroMapa: # fallback razoável return StatusCarroMapa.Direcionando - # 2) maioria simples (pode trocar por peso exponencial se quiser) - cont: dict[StatusCarroMapa, int] = {} + # maioria simples + cont: Dict[StatusCarroMapa, int] = {} for st, _t in _status_hist: cont[st] = cont.get(st, 0) + 1 @@ -502,105 +514,299 @@ def _maioria_ultimos(janela_s: float | None = None) -> StatusCarroMapa: 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 + status_now : StatusCarroMapa (instantâneo deste frame) + status_final : StatusCarroMapa (com histerese) + probs : dict[StatusCarroMapa, float] (one-hot) + debug : métricas pra log/diagnóstico """ + if mask_nav is None or mask_nav.size == 0: + status_now = StatusCarroMapa.Manobrando + _status_hist.append((status_now, _now())) + status_final = _maioria_ultimos() + return status_now, status_final, { + "nav_near": 0.0, + "nav_mid": 0.0, + "nav_far": 0.0, + "nav_global": 0.0, + } + H, W = mask_nav.shape nav = (mask_nav > 0).astype(np.float32) - def faixa_mean(y0, y1): + def faixa_mean(y0: int, y1: int) -> float: 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) + # 3 faixas verticais: far (topo), mid (meio), near (embaixo) y_far_top = 0 - y_far_bot = int(0.2 * H) + y_far_bot = int(0.25 * H) y_mid_top = y_far_bot - y_mid_bot = int(0.4 * H) + y_mid_bot = int(0.5 * 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 + 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()) - # 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 + # diferenças entre faixas (pra medir quão "desbalanceado" está) + d_nm = abs(nav_near - nav_mid) + d_mf = abs(nav_mid - nav_far) + d_nf = abs(nav_near - nav_far) + max_delta = max(d_nm, d_mf, d_nf) - 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) + # ---- Blobs 2D na região distante (parede esquerda x direita) ---- + y_blob_top = 0 + y_blob_bot = int(0.35 * H) # um pouco mais profundo que o nav_far - status = None + faixa_obs = (mask_nav[y_blob_top:y_blob_bot, :] == 0).astype(np.uint8) # 1 = obstáculo - # 1) PARADO: quase todo frame não navegável - if nav_global < THR_NAV_BAIXO: + H_blob, W_blob = faixa_obs.shape + num_blobs_far = 0 + corridor_nav_far = 0.0 + corridor_width_frac = 0.0 + + if H_blob > 0 and W_blob > 0 and faixa_obs.max() > 0: + # connectedComponents espera 0/255 + faixa_obs_bin = (faixa_obs * 255).astype(np.uint8) + + num_labels, labels = cv2.connectedComponents(faixa_obs_bin) + + # ignora blobs muito pequenos (ruído) + MIN_AREA = 0.005 * H_blob * W_blob # 0.5% da área da faixa + blobs = [] + + for label in range(1, num_labels): # 0 é o fundo + ys, xs = np.where(labels == label) + area = len(xs) + if area < MIN_AREA: + continue + + x_min, x_max = xs.min(), xs.max() + y_min, y_max = ys.min(), ys.max() + blobs.append({ + "area": area, + "bbox": (x_min, y_min, x_max, y_max), + "x_center": float(xs.mean()), + }) + + num_blobs_far = len(blobs) + + if num_blobs_far >= 2: + # ordena da parede mais à esquerda pra mais à direita + blobs_sorted = sorted(blobs, key=lambda b: b["x_center"]) + left_blob = blobs_sorted[0] + right_blob = blobs_sorted[-1] + + # corredor é a região entre x_max da esquerda e x_min da direita + x_left = left_blob["bbox"][2] + 1 # x_max (inclusivo) -> +1 pra slice + x_right = right_blob["bbox"][0] # x_min + + if x_right > x_left: + corridor_width = x_right - x_left + corridor_width_frac = corridor_width / float(W) + + faixa_nav_corridor = (mask_nav[y_blob_top:y_blob_bot, x_left:x_right] > 0).astype(np.float32) + if faixa_nav_corridor.size > 0: + corridor_nav_far = float(faixa_nav_corridor.mean()) + + # ===== Limiares (podemos tunar depois) ===== + THR_BLOCKED = 0.30 # global bem baixo -> quase sem caminho + THR_OPEN = 0.90 # global bem alto -> mundo aberto + THR_DELTA_COR = 0.15 # diferença "significativa" entre faixas + THR_COR_FAR_LARGO = 0.65 # far ainda relativamente alto em corredor largo + THR_OPEN_MUITO_LIMPO = 0.90 # quase tudo navegável + THR_DELTA_OBST_PEQ = 0.20 # desbalance vertical máximo para considerar "campo aberto com obstáculo pequeno" + MIN_CORRIDOR_WIDTH_FRAC = 0.10 # corredor tem que ter pelo menos ~10% da largura + MAX_CORRIDOR_WIDTH_FRAC = 0.70 # evita chamar de corredor quando é um "campo" gigante + + # 1) Parado: quase não há caminho à frente (meio+frente mortos) + cond_parado = ( + nav_global <= THR_BLOCKED + and nav_mid < 0.20 + and nav_far < 0.10 + ) + + # 2) Direcionando: mundo aberto, sem corredor marcado + # 2.1 base: tudo alto e muito homogêneo + cond_direcionando_base = ( + nav_global >= THR_OPEN + and max_delta < 0.10 + ) + + # 2.2 modo "campo aberto com obstáculo pequeno": + # quase tudo navegável, e até o far é bem alto + cond_direcionando_obst_peq = ( + nav_global >= THR_OPEN_MUITO_LIMPO and # >= 0.90 + nav_near >= 0.95 and + nav_mid >= 0.80 and # um pouco mais permissivo + nav_far >= 0.50 and # aceita far um pouco mais fechado + num_blobs_far <= 1 # no máximo UMA parede grande + ) + + # 2.3 modo "campo aberto com parede na frente": + # chão bem navegável perto, sem corredor definido, e FAR quase todo bloqueado + cond_direcionando_frente_fe_chada = ( + nav_near >= 0.80 and # perto bem aberto + nav_mid >= 0.30 and # meio ainda razoável + nav_far <= 0.10 and # topo praticamente bloqueado (parede) + nav_global >= 0.50 and # ainda tem bastante área navegável no frame + num_blobs_far <= 1 # no máximo uma "parede", nada de corredor + ) + + # 2.3 modo "campo aberto com borda lateral": + # cena razoavelmente aberta, uma parede forte de um lado, mas sem corredor fechado + cond_direcionando_borda_lateral = ( + nav_global >= 0.60 and # já tem boa área navegável + nav_near >= 0.70 and + nav_mid >= 0.50 and + nav_far >= 0.40 and # far não está "morrendo", só mais sujo + nav_far <= 0.80 and # não é mundão 100% limpo + num_blobs_far == 1 # exatamente UMA parede grande + ) + + cond_direcionando_aberto = ( + nav_global >= 0.75 and + nav_near >= 0.70 and + nav_mid >= 0.70 and + nav_far >= 0.70 and + max_delta <= 0.12 and + num_blobs_far <= 1 + ) + + cond_direcionando = ( + cond_direcionando_base + or cond_direcionando_obst_peq + or cond_direcionando_frente_fe_chada + or cond_direcionando_borda_lateral + or cond_direcionando_aberto + ) + + # 3) CaminhandoRua: dentro do corredor "clássico" + cond_caminhando_base = ( + nav_near >= 0.55 and + nav_mid >= 0.25 and + nav_far <= 0.50 and + (nav_near - nav_far) >= 0.20 and + num_blobs_far >= 2 # precisa de DUAS paredes + ) + + # 3.1 CaminhandoRua em corredor mais largo, com parede só de um lado + # enquadra bem os casos: + # nav_global ~0.66–0.77, near ~0.75–0.88, mid ~0.57–0.70, far ~0.56–0.63 + cond_caminhando_largo = ( + nav_global >= 0.60 and + nav_near >= 0.75 and + nav_mid >= 0.50 and + nav_far >= 0.50 and + nav_far <= THR_COR_FAR_LARGO and + (nav_near - nav_far) >= THR_DELTA_COR and + num_blobs_far >= 2 # corredor largo, mas ainda corredor + ) + + cond_caminhando_multi_corredores = ( + nav_global >= 0.50 and # tem chão suficiente + nav_mid >= 0.55 and # meio bem limpo + nav_near >= 0.50 and # perto também ok + num_blobs_far >= 2 and # pelo menos duas "paredes" + corridor_width_frac >= MIN_CORRIDOR_WIDTH_FRAC and + corridor_width_frac <= MAX_CORRIDOR_WIDTH_FRAC and + corridor_nav_far >= 0.55 # corredor entre paredes bem navegável + ) + + cond_caminhando = ( + cond_caminhando_base + or cond_caminhando_largo + or cond_caminhando_multi_corredores + ) + + cond_entrando = ( + nav_global >= 0.75 and + nav_near >= 0.90 and + nav_mid >= 0.60 and + nav_far >= 0.40 and + nav_far <= 0.85 and + (nav_near - nav_far) >= 0.10 and # far mais fechado que near + num_blobs_far >= 2 and # duas paredes detectadas + corridor_width_frac >= MIN_CORRIDOR_WIDTH_FRAC and + corridor_nav_far >= 0.60 # corredor entre as paredes bem navegável + ) + + # 5) SaindoRua + cond_saindo_1 = ( + nav_near >= 0.50 and + nav_mid >= 0.25 and + nav_far >= 0.55 and + (nav_far - nav_mid) >= 0.10 and + nav_far >= nav_near - 0.15 + ) + + cond_saindo_2 = ( + nav_global >= 0.75 and + nav_near >= 0.70 and + nav_mid >= 0.60 and + nav_far >= 0.75 and + nav_far >= nav_mid + ) + + cond_saindo = cond_saindo_1 or cond_saindo_2 + + # ===== Decisão (ordem importa!) ===== + if cond_parado: status = StatusCarroMapa.Parado - - # 2) DIRECIONANDO: quase todo frame navegável - elif nav_global > THR_NAV_ALTO: + elif cond_direcionando: status = StatusCarroMapa.Direcionando - + elif cond_saindo: + status = StatusCarroMapa.SaindoRua + elif cond_entrando: + status = StatusCarroMapa.EntrandoRua + elif cond_caminhando: + status = StatusCarroMapa.CaminhandoRua 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 + status = StatusCarroMapa.Indefinido debug = { + "nav_global": nav_global, "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, + "d_nm": d_nm, + "d_mf": d_mf, + "d_nf": d_nf, + "max_delta": max_delta, + "num_blobs_far": num_blobs_far, + "corridor_nav_far": corridor_nav_far, + "corridor_width_frac": corridor_width_frac, + "cond_parado": cond_parado, + "cond_direcionando_base": cond_direcionando_base, + "cond_direcionando_obst_peq": cond_direcionando_obst_peq, + "cond_caminhando_base": cond_caminhando_base, + "cond_caminhando_largo": cond_caminhando_largo, + "cond_entrando": cond_entrando, + "cond_saindo": cond_saindo, + "THR_BLOCKED": THR_BLOCKED, + "THR_OPEN": THR_OPEN, + "THR_DELTA_COR": THR_DELTA_COR, + "THR_COR_FAR_LARGO": THR_COR_FAR_LARGO, + "THR_OPEN_MUITO_LIMPO": THR_OPEN_MUITO_LIMPO, } status_now = status - print(status_now.name) - # Histerese temporal: mantém teu esquema de histórico + print(status_now.name, debug) + _status_hist.append((status_now, _now())) status_final = _maioria_ultimos() - return status_now, status_final, probs, debug - + return status_now, status_final, debug