adicionado logica de deteccoes na matriz de confianca
This commit is contained in:
Diego Freitas
parent
6853d3f264
commit
796b3a9701
Binary file not shown.
Binary file not shown.
|
|
@ -1,7 +1,6 @@
|
|||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Drawing;
|
||||
using System.Linq;
|
||||
using static AgroBase.Models.Enums;
|
||||
|
||||
namespace AgroBase.Models.Operadores
|
||||
|
|
|
|||
Binary file not shown.
Binary file not shown.
|
|
@ -298,6 +298,8 @@ class CameraManager:
|
|||
|
||||
def _realizar_analises(self):
|
||||
self._analise_segmentacao()
|
||||
|
||||
self._analise_deteccao()
|
||||
|
||||
if self._depth_frame_necessario:
|
||||
depth_frame_np, depth_timestamp, depth_res = self.get_depth_frame()
|
||||
|
|
@ -308,7 +310,6 @@ class CameraManager:
|
|||
distancia_max_m = parametros_camera["distancia_maxima"] / 1000.0
|
||||
self._analise_matriz_confianca(depth_frame_np, distancia_max_m, fov_h)
|
||||
|
||||
self._analise_deteccao()
|
||||
|
||||
def _realizar_analises_async(self):
|
||||
executor = self._pool
|
||||
|
|
@ -399,13 +400,14 @@ class CameraManager:
|
|||
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
|
||||
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"))
|
||||
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)
|
||||
|
|
@ -423,7 +425,9 @@ class CameraManager:
|
|||
#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)
|
||||
|
||||
#vis, metrics = self.debug_blockage_imshow(self._ultimo_rgb_frame, snapshot, velocidade_media=vel)
|
||||
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:
|
||||
|
|
@ -849,49 +853,80 @@ class CameraManager:
|
|||
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) com IDs de classe por pixel
|
||||
seg_ids_512x288, # np.ndarray (288,512)
|
||||
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), # faixa válida do depth (ajuste conforme tua OAK)
|
||||
range_m=(0.5, 5.0), # janela útil à frente (só p/ debug/checar)
|
||||
min_valid_frac=0.30, # % mínimo de pixels válidos p/ aceitar mediana
|
||||
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, # tolerância mínima de “aproximação” (m)
|
||||
anom_satur_m=0.50 # saturação da anomalia (m)
|
||||
anom_tau_min=0.22,
|
||||
anom_satur_m=0.50,
|
||||
# -------- NOVO: 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)
|
||||
):
|
||||
"""
|
||||
Retorna dict com arrays (grid_h, grid_w):
|
||||
pct_rua, pct_cana, pct_obs, z_med (m), z_ref (m),
|
||||
depth_valid_frac, conf, anom, custo, navegavel (0/1)
|
||||
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
|
||||
"""
|
||||
if class_ids is None:
|
||||
# AJUSTE para os IDs reais do teu labelmap!
|
||||
class_ids = {'rua': 0, 'cana': 1, 'obs': 2}
|
||||
|
||||
grid_w, grid_h = grid_shape # (15, 10)
|
||||
# ---------- 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)
|
||||
"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)
|
||||
"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
|
||||
}
|
||||
if det_params:
|
||||
_det.update(det_params)
|
||||
|
||||
grid_w, grid_h = grid_shape
|
||||
H0, W0 = depth_mm.shape
|
||||
|
||||
# --- 1) Reduz o depth para 512x288 preservando rótulos (sem blur de escala) ---
|
||||
# --- 1) Resize depth para 512x288 ---
|
||||
d_small = cv2.resize(depth_mm, (512, 288), interpolation=cv2.INTER_NEAREST).astype(np.float32)
|
||||
# marca inválidos
|
||||
d_small[(d_small < valid_mm[0]) | (d_small > valid_mm[1])] = np.nan
|
||||
|
||||
# --- 2) Bordas das células da grid 15x10 sobre a imagem 512x288 ---
|
||||
x_edges = np.linspace(0, 512, grid_w + 1, dtype=int) # 16 bordas
|
||||
y_edges = np.linspace(0, 288, grid_h + 1, dtype=int) # 11 bordas
|
||||
# --- 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)
|
||||
|
||||
# --- 3) Saídas (grid_h, grid_w) = (10, 15) ---
|
||||
# --- 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)
|
||||
|
||||
# --- 4) Garante grid_ref 2D em metros ---
|
||||
# --- 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
|
||||
|
||||
# --- 4) grid_ref 2D ---
|
||||
if grid_ref.ndim == 1:
|
||||
# grid_ref é por LINHA (grid_h,)
|
||||
if grid_ref.shape[0] != grid_h:
|
||||
raise ValueError(f"grid_ref 1D deve ter len={grid_h}, veio {grid_ref.shape}")
|
||||
Z_ref = np.repeat(grid_ref[:, None], grid_w, axis=1)
|
||||
|
|
@ -900,13 +935,16 @@ 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}")
|
||||
|
||||
# --- 5) Agregação por célula (150 células: tranquilo em tempo real) ---
|
||||
# pré-slices por linha
|
||||
for j in range(grid_h):
|
||||
y0, y1 = y_edges[j], y_edges[j+1]
|
||||
seg_row = seg_ids_512x288[y0:y1, :] # (rows, 512)
|
||||
depth_row = d_small[y0:y1, :] # (rows, 512)
|
||||
seg_row = seg_ids_512x288[y0:y1, :]
|
||||
depth_row = d_small[y0:y1, :]
|
||||
row_h = max(1, y1 - y0)
|
||||
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]
|
||||
|
||||
|
|
@ -922,46 +960,132 @@ class CameraManager:
|
|||
pct_cana[j, i] = n_cana / n
|
||||
pct_obs[j, i] = n_obs / n
|
||||
|
||||
# depth: mediana em metros + fração válida
|
||||
# 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 # mm -> m
|
||||
z_med[j, i] = np.nanmedian(vals) / 1000.0
|
||||
|
||||
# --- 6) Confiança por célula ---
|
||||
# --- 6) Confiança ---
|
||||
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
|
||||
|
||||
# --- 7) Anomalia (obstáculo = mais perto que o esperado) ---
|
||||
# ΔZ > 0 → medido está mais perto que a referência
|
||||
delta = Z_ref - z_med # m
|
||||
# z_med NaN → delta = 0 (sem evidência)
|
||||
# --- 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"]:
|
||||
continue
|
||||
|
||||
label = str(det.get("label", ""))
|
||||
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)
|
||||
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))
|
||||
if x1p <= x0p or y1p <= y0p:
|
||||
continue
|
||||
|
||||
bbox_area = float((x1p - x0p) * (y1p - y0p))
|
||||
if bbox_area <= 1.0:
|
||||
continue
|
||||
|
||||
# descobre células que sobrepõem a bbox
|
||||
# índices i (colunas) e j (linhas) 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]
|
||||
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)
|
||||
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))
|
||||
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"
|
||||
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)
|
||||
# porta de tolerância mínima (tau) + confiança do depth
|
||||
anom = anom_raw * (delta > anom_tau_min).astype(np.float32) * conf_dep
|
||||
|
||||
# --- 8) Custo e navegabilidade ---
|
||||
nao_rua = 1.0 - pct_rua
|
||||
w1, w2, w3 = w
|
||||
custo = np.clip(w1 * nao_rua + w2 * anom + w3 * (1.0 - conf_cell), 0.0, 1.0)
|
||||
custo = w1 * nao_rua + w2 * anom + w3 * (1.0 - conf_cell)
|
||||
|
||||
navegavel = (pct_rua >= 0.55) & (anom < 0.4) & (conf_cell >= 0.5)
|
||||
# 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"])
|
||||
else:
|
||||
navegavel = (pct_rua >= 0.55) & (anom < 0.4) & (conf_cell >= 0.5)
|
||||
|
||||
return {
|
||||
"pct_rua": pct_rua,
|
||||
"pct_cana": pct_cana,
|
||||
"pct_obs": pct_obs,
|
||||
"z_med": z_med, # metros
|
||||
"z_ref": Z_ref, # metros
|
||||
"z_med": z_med,
|
||||
"z_ref": Z_ref,
|
||||
"depth_valid_frac": depth_valid_frac,
|
||||
"conf": conf_cell, # 0..1
|
||||
"anom": anom, # 0..1
|
||||
"custo": custo, # 0..1
|
||||
"navegavel": navegavel.astype(np.uint8) # 0/1
|
||||
"conf": np.clip(custo*0 + conf_cell, 0.0, 1.0), # garante 0..1
|
||||
"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,
|
||||
"det_top_label_id": det_top_label_id,
|
||||
"det_top_conf": det_top_conf,
|
||||
}
|
||||
|
||||
def _put_text_centered(self, img, text, cx, cy, font_scale=0.4, thickness=1, color=(255,255,255), outline=True):
|
||||
|
|
|
|||
|
|
@ -72,7 +72,7 @@ def load_seg_config(force_reload=False):
|
|||
# "kernel_morf": 3
|
||||
# }
|
||||
_CONFIG_CACHE = {
|
||||
"debug_visual": False,
|
||||
"debug_visual": True,
|
||||
"ia_roi_begin": 0.0,
|
||||
"ia_roi_size": 1.0,
|
||||
"ia_resolution": [512,288],
|
||||
|
|
|
|||
|
|
@ -107,7 +107,12 @@ class CostmapFuser:
|
|||
margem_parada=0.25, # m
|
||||
N_on=2, # frames p/ entrar
|
||||
N_off=5, # frames p/ sair
|
||||
blackout_imediato=True
|
||||
blackout_imediato=True,
|
||||
|
||||
det_score_f=None, # (H,W) 0..1
|
||||
det_label_id=None, # (H,W) int, -1 = none
|
||||
det_strength=None, # (H,W) 0..1 ~ conf*cov dominante
|
||||
thr_det_consider=0.25
|
||||
):
|
||||
H, W = custo_f.shape
|
||||
c0, c1 = central_cols
|
||||
|
|
@ -122,6 +127,15 @@ class CostmapFuser:
|
|||
mask_conf = (conf_f < thr_conf_low)
|
||||
unsafe = mask_anom | mask_cost | mask_conf
|
||||
|
||||
has_det = None
|
||||
labelmap_det = None
|
||||
if (det_score_f is not None):
|
||||
has_det = (det_score_f >= float(thr_det_consider))
|
||||
from visual_worker.config import load_det_config
|
||||
labelmap_det = load_det_config().get("classes")
|
||||
if labelmap_det is not None:
|
||||
labelmap_det = {i: name for i, name in enumerate(labelmap_det)}
|
||||
|
||||
# 2) Largura em colunas por linha
|
||||
width_need_m = robot_width_m + margin_m
|
||||
cols_need = np.empty(H, dtype=int)
|
||||
|
|
@ -145,6 +159,12 @@ class CostmapFuser:
|
|||
coverage_central = np.zeros(H, np.float32)
|
||||
exists_unsafe_central = np.zeros(H, np.bool_) # <- NOVO: existe ao menos 1 px inseguro na janela
|
||||
j_block = None
|
||||
|
||||
det_exists_central = np.zeros(H, np.bool_)
|
||||
det_min_z_central = np.full(H, np.nan, np.float32)
|
||||
det_best_label = -np.ones(H, np.int32)
|
||||
det_best_strength = np.zeros(H, np.float32)
|
||||
|
||||
it = (range(H-1, -1, -1) if near_is_bottom else range(H))
|
||||
for j in it:
|
||||
a, b = central_window(j, cols_need[j])
|
||||
|
|
@ -183,6 +203,26 @@ class CostmapFuser:
|
|||
if b <= a:
|
||||
continue
|
||||
|
||||
if has_det is not None:
|
||||
det_win = has_det[j, a:b]
|
||||
if np.any(det_win):
|
||||
det_exists_central[j] = True
|
||||
# força dominante (strength) e rótulo dominante nessa janela
|
||||
if (det_strength is not None) and (det_label_id is not None):
|
||||
str_win = det_strength[j, a:b]
|
||||
lbl_win = det_label_id[j, a:b]
|
||||
# pega o pixel com MAIOR força na janela
|
||||
k = np.argmax(str_win)
|
||||
det_best_strength[j] = float(str_win.ravel()[k])
|
||||
det_best_label[j] = int(lbl_win.ravel()[k])
|
||||
# distância usando zmed somente onde há detecção
|
||||
if zmed_f is not None:
|
||||
z_win = zmed_f[j, a:b]
|
||||
z_sel = z_win[det_win]
|
||||
z_sel = z_sel[np.isfinite(z_sel) & (z_sel > 0)]
|
||||
if z_sel.size:
|
||||
det_min_z_central[j] = float(np.min(z_sel))
|
||||
|
||||
# máscara de insegurança na janela
|
||||
bad = unsafe[j, a:b]
|
||||
|
||||
|
|
@ -238,18 +278,82 @@ class CostmapFuser:
|
|||
|
||||
d_used = _min_non_none(d_obs_true_min_m_z, d_block_line_m_z)
|
||||
|
||||
extra_det_txt = ""
|
||||
if has_det is not None:
|
||||
# prioriza a linha de bloqueio; se não houver, a primeira linha com detecção
|
||||
j_det = None
|
||||
if (j_block is not None) and det_exists_central[j_block]:
|
||||
j_det = j_block
|
||||
else:
|
||||
for jj in it2: # perto -> longe (ou longe->perto dependendo de near_is_bottom)
|
||||
if det_exists_central[jj]:
|
||||
j_det = jj
|
||||
break
|
||||
if j_det is not None:
|
||||
d_det = det_min_z_central[j_det]
|
||||
# pega janela central nessa linha
|
||||
a, b = central_window(j_det, cols_need[j_det])
|
||||
labels = {}
|
||||
if det_label_id is not None and det_strength is not None:
|
||||
lbls = det_label_id[j_det, a:b].ravel()
|
||||
strs = det_strength[j_det, a:b].ravel()
|
||||
for lid, s in zip(lbls, strs):
|
||||
if lid < 0:
|
||||
continue
|
||||
if s < thr_det_consider:
|
||||
continue
|
||||
# guarda o maior score visto pra essa classe
|
||||
if lid not in labels or s > labels[lid]:
|
||||
labels[lid] = s
|
||||
# traduz para nomes
|
||||
if labels:
|
||||
parts = []
|
||||
for lid, s in labels.items():
|
||||
if labelmap_det is not None and lid < len(labelmap_det):
|
||||
nm = labelmap_det[lid]
|
||||
else:
|
||||
nm = f"label#{lid}"
|
||||
parts.append(f"{nm}({s:.2f})")
|
||||
lbl_txt = ", ".join(parts)
|
||||
else:
|
||||
lbl_txt = "objeto"
|
||||
d_det_txt = "-" if (d_det is None or not np.isfinite(d_det)) else f"{d_det:.2f} m"
|
||||
extra_det_txt = f" | deteccoes: {lbl_txt} a {d_det_txt}"
|
||||
|
||||
reason_detail = (
|
||||
f"Janela central bloqueada (p>={rho_block_central:.2f}). "
|
||||
f"d*={_fmt_m(d_used)} [linha={_fmt_m(d_block_line_m_z)}; pixel={_fmt_m(d_obs_true_min_m_z)}]; "
|
||||
f"cobertura_central_max={central_cov_max:.2f}."
|
||||
f"cobertura_central_max={central_cov_max:.2f}{extra_det_txt}."
|
||||
)
|
||||
|
||||
elif (global_cov >= rho_block_global) and (conf_mean < 0.45):
|
||||
blocked_raw = True
|
||||
reason = "blackout"
|
||||
extra = ""
|
||||
if has_det is not None and np.any(det_exists_central):
|
||||
# lista até 2 rótulos distintos mais fortes (opcional)
|
||||
labs = []
|
||||
if (det_best_label is not None) and (labelmap_det is not None):
|
||||
# pega top-2 por força
|
||||
idxs = np.argsort(-det_best_strength) # desc força
|
||||
seen = set()
|
||||
for k in idxs:
|
||||
lid = int(det_best_label[k])
|
||||
if lid < 0:
|
||||
continue
|
||||
if lid in seen:
|
||||
continue
|
||||
seen.add(lid)
|
||||
labs.append(labelmap_det.get(lid, f"label#{lid}"))
|
||||
if len(labs) >= 2:
|
||||
break
|
||||
if labs:
|
||||
extra = f" (deteccoes vistas: {', '.join(labs)})"
|
||||
else:
|
||||
extra = " (deteccoes presentes)"
|
||||
reason_detail = (
|
||||
f"Percepcao degradada: cobertura_global={global_cov:.2f}≥{rho_block_global:.2f} "
|
||||
f"e confianca_media={conf_mean:.2f}<0.45."
|
||||
f"e confianca_media={conf_mean:.2f}<0.45{extra}."
|
||||
)
|
||||
|
||||
elif (central_cov_max > 0.45) and (left > 0.7 or right > 0.7):
|
||||
|
|
@ -257,9 +361,12 @@ class CostmapFuser:
|
|||
reason = "narrow"
|
||||
lado = "direita" if right > left else "esquerda"
|
||||
lado_frac = max(left, right)
|
||||
extra = ""
|
||||
if has_det is not None and np.any(det_exists_central):
|
||||
extra = " (detecções na faixa central)"
|
||||
reason_detail = (
|
||||
f"Corredor estreito: lateral {lado} muito fechada (frac={lado_frac:.2f}), "
|
||||
f"central_max={central_cov_max:.2f}."
|
||||
f"central_max={central_cov_max:.2f}{extra}."
|
||||
)
|
||||
|
||||
# 8) Persistência + decisão de parada
|
||||
|
|
@ -382,6 +489,10 @@ class CostmapFuser:
|
|||
if zmed is not None:
|
||||
zmed = zmed.astype(np.float32)
|
||||
|
||||
det_score = grid_dict.get("det_score", None)
|
||||
det_lbl = grid_dict.get("det_top_label_id", None)
|
||||
det_sdom = grid_dict.get("det_top_conf", None) # nosso conf*cov dominante
|
||||
|
||||
# valida shape (H,W) = (grid_h,grid_w)
|
||||
H, W = custo.shape
|
||||
assert (H, W) == (self.grid_h, self.grid_w), f"grid {H,W} != {(self.grid_h,self.grid_w)}"
|
||||
|
|
@ -457,7 +568,12 @@ class CostmapFuser:
|
|||
use_persistence=True,
|
||||
velocidade_mps=velocidade_ms,
|
||||
a_max_freio=0.8, margem_parada=0.60,
|
||||
N_on=2, N_off=5, blackout_imediato=True
|
||||
N_on=2, N_off=5, blackout_imediato=True,
|
||||
|
||||
det_score_f = det_score,
|
||||
det_label_id = det_lbl,
|
||||
det_strength = det_sdom,
|
||||
thr_det_consider = 0.25 # só considera detecção acima desse score
|
||||
)
|
||||
|
||||
# incrementa seq
|
||||
|
|
|
|||
Loading…
Reference in New Issue