tuning de rede e ajustes no fluxo de detecção sonar
This commit is contained in:
Diego Freitas
parent
1f8b3027cc
commit
755d70011e
|
|
@ -54,6 +54,7 @@ namespace AgroBase.Models.Operadores
|
|||
{
|
||||
public StatusCarroMapa StatusCarro { get; set; }
|
||||
public double ErroLateral { get; set; }
|
||||
public double ErroAngular { get; set; }
|
||||
|
||||
public bool ObstaculoDetectado { get; set; } = false;
|
||||
public bool DeveParar { get; set; } = false;
|
||||
|
|
@ -67,7 +68,8 @@ namespace AgroBase.Models.Operadores
|
|||
var Sonar = Variaveis.OperacaoEmAndamento.Sensoriamento.OperadorVisual;
|
||||
ObstaculoDetectado = (Sonar?.Analises?.matriz_confianca?.block?.decision?.parar ?? false);
|
||||
StatusCarro = Sonar?.Analises?.segmentacao?.status_corredor ?? StatusCarroMapa.Indefinido;
|
||||
ErroLateral = Sonar?.Analises?.segmentacao?.erro_angular ?? 0;
|
||||
ErroLateral = Sonar?.Analises?.segmentacao?.erro_lateral_pct ?? 0;
|
||||
ErroAngular = Sonar?.Analises?.segmentacao?.erro_angular ?? 0;
|
||||
|
||||
if (!(Variaveis.OperacaoEmAndamento.Parametros?.Controle?.OakParadaPorObstaculo ?? false))
|
||||
{
|
||||
|
|
@ -102,6 +104,7 @@ namespace AgroBase.Models.Operadores
|
|||
{
|
||||
StatusCarro = StatusCarro,
|
||||
ErroLateral = ErroLateral,
|
||||
ErroAngular = ErroAngular,
|
||||
ObstaculoDetectado = ObstaculoDetectado,
|
||||
DeveParar = DeveParar,
|
||||
EnviarComandoParada = EnviarComandoParada,
|
||||
|
|
|
|||
|
|
@ -15,7 +15,7 @@ from shared.enums import StatusModulo, T_Code
|
|||
from health_worker.modulos.base import ModuloDiagnosticoBase
|
||||
|
||||
class ModuloIPBribge(ModuloDiagnosticoBase):
|
||||
def __init__(self, window_fast: int = 6, window_slow: int = 20):
|
||||
def __init__(self, window_fast: int = 8, window_slow: int = 24):
|
||||
self.t_code = T_Code.Ipb
|
||||
self.nome = "IP_Brigde"
|
||||
self.timeout = 5
|
||||
|
|
@ -61,8 +61,8 @@ class ModuloIPBribge(ModuloDiagnosticoBase):
|
|||
self._mqtt_backoff_max_s = 30.0
|
||||
self._mqtt_connected_ts = 0.0
|
||||
self.last_heartbeat_ts = 0.0
|
||||
self._heartbeat_timeout_s = 10.0
|
||||
self._heartbeat_grace_after_connect_s = 15.0
|
||||
self._heartbeat_timeout_s = 7.0
|
||||
self._heartbeat_grace_after_connect_s = 10.0
|
||||
self.sub = False
|
||||
self._mqtt_future = None
|
||||
self._mqtt_executor = ThreadPoolExecutor(max_workers=1)
|
||||
|
|
@ -80,9 +80,9 @@ class ModuloIPBribge(ModuloDiagnosticoBase):
|
|||
self._last_ping_loss_pct = 100.0
|
||||
|
||||
# parâmetros do ping
|
||||
self._ping_timeout_ms = 400 # recomendado: 300–500
|
||||
self._ping_count = 2 # recomendado: 1 (janela já suaviza)
|
||||
self._ping_min_interval = 0.6 # não precisa pingar a cada 100ms
|
||||
self._ping_timeout_ms = 500 # recomendado: 300–500
|
||||
self._ping_count = 1 # recomendado: 1 (janela já suaviza)
|
||||
self._ping_min_interval = 0.5 # não precisa pingar a cada 100ms
|
||||
self._hb_last_rx_monotonic = 0.0
|
||||
self._last_ping_sample_applied_ts = 0.0
|
||||
|
||||
|
|
@ -697,8 +697,8 @@ class ModuloIPBribge(ModuloDiagnosticoBase):
|
|||
# opcional: um pequeno piso e teto
|
||||
bw_max = max(0.35, min(bw_max, 10.0))
|
||||
|
||||
bw_safe = bw_max * 0.68
|
||||
bw_hard = bw_max * 0.82
|
||||
bw_safe = bw_max * 0.60
|
||||
bw_hard = bw_max * 0.78
|
||||
|
||||
# garantias mínimas
|
||||
bw_safe = max(0.20, bw_safe)
|
||||
|
|
@ -1124,11 +1124,11 @@ class ModuloIPBribge(ModuloDiagnosticoBase):
|
|||
self._cycles_recovered = min(self._cycles_recovered, 30)
|
||||
|
||||
prev_state = self.link_state
|
||||
if self._cycles_critical >= 8:
|
||||
if self._cycles_critical >= 10:
|
||||
self.link_state = "CRITICAL"
|
||||
elif self._cycles_degraded >= 12:
|
||||
elif self._cycles_degraded >= 14:
|
||||
self.link_state = "DEGRADED"
|
||||
elif self._cycles_recovered >= 20:
|
||||
elif self._cycles_recovered >= 24:
|
||||
self.link_state = "OK"
|
||||
|
||||
# entrou em DEGRADED agora
|
||||
|
|
|
|||
|
|
@ -82,7 +82,8 @@ class CameraManager:
|
|||
self._timestamp_analise = None
|
||||
|
||||
self.grid_ref_shape = (15, 10)
|
||||
self.gerar_grid_ref()
|
||||
self.grid_ref_base = self.gerar_grid_ref()
|
||||
self.grid_ref = self.grid_ref_base.copy()
|
||||
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)
|
||||
|
|
@ -120,18 +121,97 @@ class CameraManager:
|
|||
self.atualizar_saude_camera()
|
||||
|
||||
def gerar_grid_ref(self, n_frames=50):
|
||||
self.grid_ref = self._gerar_grid_referencia_geometrico()
|
||||
try:
|
||||
grid = self._gerar_grid_referencia_geometrico()
|
||||
|
||||
if grid is None or len(grid) != self.grid_ref_shape[1]:
|
||||
raise ValueError("grid_ref inválido ou com tamanho incorreto")
|
||||
|
||||
grid = np.asarray(grid, dtype=np.float32)
|
||||
|
||||
if not np.all(np.isfinite(grid)):
|
||||
raise ValueError("grid_ref contém NaN/Inf")
|
||||
|
||||
if np.any(grid <= 0):
|
||||
raise ValueError("grid_ref contém valores <= 0")
|
||||
|
||||
# monotonicidade esperada
|
||||
diffs = np.diff(grid)
|
||||
if not np.all(diffs > 0):
|
||||
self.mostrar_log(f"[WARN] grid_ref não monotônico: {grid.tolist()}")
|
||||
|
||||
self.grid_ref = grid
|
||||
|
||||
except Exception as e:
|
||||
self.mostrar_log(f"Erro ao gerar grid_ref: {e}")
|
||||
# fallback linear seguro
|
||||
self.grid_ref = np.linspace(0.5, 5.0, self.grid_ref_shape[1], dtype=np.float32)
|
||||
|
||||
def _gerar_grid_referencia_geometrico(self, angulo_inclinacao_graus=28.91, altura_camera_m=0.74):
|
||||
grid_w, grid_h = self.grid_ref_shape
|
||||
def dist_grid_calibrado(grid_h, i, fov, incl, altura):
|
||||
alpha_v = ((i + 0.5) / grid_h - 0.5) * np.radians(fov) # (i + 0.5) = centro da linha, (i + 0.0) = baixo da linha, (i + 1.0) = topo da linha
|
||||
gamma = np.radians(incl) + alpha_v
|
||||
d = (altura / np.tan(gamma)) #* 1000.0
|
||||
try:
|
||||
grid_w, grid_h = self.grid_ref_shape
|
||||
|
||||
def dist_grid_calibrado(grid_h, i, fov_deg, incl_deg, altura_m):
|
||||
alpha_v = ((i + 0.5) / grid_h - 0.5) * np.radians(abs(fov_deg))
|
||||
gamma = np.radians(incl_deg) + alpha_v
|
||||
|
||||
# evita tangente perto de zero
|
||||
gamma = max(gamma, np.radians(2.0))
|
||||
|
||||
d = altura_m / np.tan(gamma)
|
||||
|
||||
# saturação plausível
|
||||
d = float(np.clip(d, 0.2, 20.0))
|
||||
return d
|
||||
|
||||
d = np.array(
|
||||
[dist_grid_calibrado(grid_h, i, -43.28, angulo_inclinacao_graus, altura_camera_m) for i in range(grid_h)],
|
||||
dtype=np.float32
|
||||
)
|
||||
|
||||
d = d[::-1]
|
||||
return d
|
||||
d = np.array([dist_grid_calibrado(grid_h, i, -43.28, angulo_inclinacao_graus, altura_camera_m) for i in range(grid_h)], dtype=np.float32)
|
||||
d = d[::-1] # ordena de baixo->cima como você queria
|
||||
return d # <-- ndarray, não list
|
||||
|
||||
except Exception as e:
|
||||
self.mostrar_log(f"Erro em _gerar_grid_referencia_geometrico: {e}")
|
||||
return None
|
||||
|
||||
def _ajustar_grid_ref_por_pitch(self, pitch_graus, pitch_gain=1.0):
|
||||
"""
|
||||
Ajusta a grid_ref empírica base com base no pitch atual.
|
||||
Retorna uma nova grid_ref (1D, len = grid_h).
|
||||
"""
|
||||
try:
|
||||
if self.grid_ref_base is None:
|
||||
return self.grid_ref
|
||||
|
||||
grid_h = self.grid_ref_shape[1]
|
||||
|
||||
# pitch corrigido / limitado
|
||||
pitch_corr = float(np.clip(pitch_graus * pitch_gain, -10.0, 10.0))
|
||||
|
||||
# reutiliza a mesma lógica da calibração base
|
||||
grid_ref_fixed = self._gerar_grid_referencia_geometrico(
|
||||
angulo_inclinacao_graus=28.91 + pitch_corr,
|
||||
altura_camera_m=0.74
|
||||
)
|
||||
|
||||
if grid_ref_fixed is None:
|
||||
return self.grid_ref_base.copy()
|
||||
|
||||
grid_ref_fixed = np.asarray(grid_ref_fixed, dtype=np.float32)
|
||||
|
||||
if len(grid_ref_fixed) != grid_h:
|
||||
return self.grid_ref_base.copy()
|
||||
|
||||
if not np.all(np.isfinite(grid_ref_fixed)) or np.any(grid_ref_fixed <= 0):
|
||||
return self.grid_ref_base.copy()
|
||||
|
||||
return grid_ref_fixed
|
||||
|
||||
except Exception as e:
|
||||
self.mostrar_log(f"Erro ao ajustar grid_ref por pitch: {e}")
|
||||
return self.grid_ref_base.copy()
|
||||
|
||||
def atualizar_saude_camera(self):
|
||||
#self.mostrar_log("Atualizando saude da camera...")
|
||||
|
|
@ -346,6 +426,8 @@ class CameraManager:
|
|||
else:
|
||||
depth_frame_np = self._ultimo_depth_frame
|
||||
|
||||
imu_roll = ContextoGlobalRedis.get_modulo(T_Code.Imu).get("roll_seg", 0)
|
||||
self.grid_ref = self._ajustar_grid_ref_por_pitch(pitch_graus=imu_roll)
|
||||
self._analise_matriz_confianca(depth_frame_np)
|
||||
|
||||
|
||||
|
|
@ -356,7 +438,8 @@ class CameraManager:
|
|||
t0 = time.time()
|
||||
predictions, ts, res = self.get_segmentation_predictions()
|
||||
if ts == self._ts_segmentacao_anterior: return
|
||||
fps = 1.0 / (ts - self._ts_segmentacao_anterior)
|
||||
dt = max(1e-6, ts - self._ts_segmentacao_anterior) if self._ts_segmentacao_anterior else 0.0
|
||||
fps = (1.0 / dt) if dt > 0 else 0.0
|
||||
self._ts_segmentacao_anterior = ts
|
||||
if predictions is not None:
|
||||
analise_segmentacao, log = self.segmentacao_manager.segmentar(predictions)
|
||||
|
|
@ -391,7 +474,8 @@ class CameraManager:
|
|||
t0 = time.time()
|
||||
dets, ts, meta = self.get_detections()
|
||||
if ts == self._ts_deteccao_anterior: return
|
||||
fps = 1.0 / (ts - self._ts_deteccao_anterior)
|
||||
dt = max(1e-6, ts - self._ts_deteccao_anterior) if self._ts_deteccao_anterior else 0.0
|
||||
fps = (1.0 / dt) if dt > 0 else 0.0
|
||||
self._ts_deteccao_anterior = ts
|
||||
if dets is not None:
|
||||
t1 = time.time()
|
||||
|
|
@ -545,13 +629,31 @@ class CameraManager:
|
|||
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._ultimo_detections
|
||||
deteccoes = (self._ultima_analise_deteccao or {}).get("bboxes", [])
|
||||
det_params = {
|
||||
"w4": 0.18,
|
||||
"thr_det_soft": 0.45,
|
||||
"min_cell_coverage": 0.10,
|
||||
"min_det_conf": 0.45,
|
||||
"class_weights": {
|
||||
"person": 1.0,
|
||||
"dog": 0.7,
|
||||
"cat": 0.5,
|
||||
},
|
||||
"veto_labels": {"person"},
|
||||
"combine": "max",
|
||||
"conf_drop_alpha": 0.0,
|
||||
"only_veto_blocks_nav": True,
|
||||
"non_veto_cost_scale": 0.50,
|
||||
}
|
||||
|
||||
vel = get_velocidade_atual_ms()
|
||||
|
||||
t0 = time.time()
|
||||
grid_conf = self._construir_grid_confianca(depth_frame_np, segmentacao, self.grid_ref, self.grid_ref_shape, deteccoes=deteccoes)
|
||||
grid_conf = self._construir_grid_confianca(depth_frame_np, segmentacao, self.grid_ref, self.grid_ref_shape, deteccoes=deteccoes, det_params=det_params)
|
||||
#self.mostrar_log(grid_conf)
|
||||
if grid_conf is None:
|
||||
return
|
||||
t1 = time.time()
|
||||
grid_conf["ultima_chamada"] = self._ultima_analise_matriz_confianca.get("ultima_chamada", t0)
|
||||
self._calcular_performance(t0, t1, grid_conf)
|
||||
|
|
@ -573,43 +675,57 @@ class CameraManager:
|
|||
#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) ou None se não usar depth
|
||||
self,
|
||||
depth_mm, # np.ndarray (H,W) em mm ou None
|
||||
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)
|
||||
grid_ref, # np.ndarray (grid_h,) ou (grid_h, grid_w) em metros
|
||||
grid_shape=(15, 10), # (cols, rows)
|
||||
valid_mm=(300, 10000),
|
||||
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)
|
||||
conf_params=(0.30, 0.80), # t0,t1 para mapear % depth válido -> conf_depth
|
||||
w=(0.65, 0.25, 0.10), # pesos do custo: nao_navegavel, anom, (1-conf)
|
||||
anom_tau_min=0.22,
|
||||
anom_satur_m=0.50,
|
||||
# -------- 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)
|
||||
usar_depth: bool = True # NOVO: se False, ignora depth e anomalia
|
||||
deteccoes=None, # lista de dicts
|
||||
det_params=None,
|
||||
usar_depth: bool = True,
|
||||
anom_tau_up=0.22,
|
||||
anom_satur_up_m=0.50,
|
||||
anom_tau_down=0.18,
|
||||
anom_satur_down_m=0.40,
|
||||
anom_down_weight=0.70,
|
||||
):
|
||||
"""
|
||||
Retorna dict com arrays (grid_h, grid_w):
|
||||
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
|
||||
Constrói uma grid de confiança/risco por célula.
|
||||
|
||||
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.
|
||||
Filosofia:
|
||||
- segmentação é a base da navegabilidade
|
||||
- depth gera anomalia física
|
||||
- detecção fornece contexto crítico, sem derrubar confiança
|
||||
- confiança representa qualidade perceptiva, não obstáculo
|
||||
- custo é um mapa suave de risco, não uma decisão final
|
||||
|
||||
Retorna dict com arrays (grid_h, grid_w):
|
||||
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
|
||||
"""
|
||||
try:
|
||||
# ---------- defaults detecção ----------
|
||||
# --------------------------------------------------
|
||||
# 0) Defaults de detecção alinhados com nova lógica
|
||||
# --------------------------------------------------
|
||||
_det = {
|
||||
"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
|
||||
"w4": 0.18, # quanto a detecção pesa no custo (suave)
|
||||
"thr_det_soft": 0.45, # acima disso, reduz navegabilidade se for crítica
|
||||
"min_cell_coverage": 0.10,
|
||||
"min_det_conf": 0.35,
|
||||
"class_weights": {}, # ex: {'person':1.0,'car':0.9,'dog':0.6}
|
||||
"veto_labels": set(["person"]),
|
||||
"combine": "max", # "max" ou "sum_clamped"
|
||||
"conf_drop_alpha": 0.15, # 0 = não derruba; 0.15 = derruba 15% * det_score
|
||||
"min_det_conf": 0.45,
|
||||
"class_weights": {}, # ex: {'person':1.0,'dog':0.7}
|
||||
"veto_labels": {"person"}, # labels críticas
|
||||
"combine": "max", # "max" ou "sum_clamped"
|
||||
"conf_drop_alpha": 0.0, # NOVA FILOSOFIA: detecção não derruba confiança
|
||||
"only_veto_blocks_nav": True,
|
||||
"non_veto_cost_scale": 0.50 # labels não críticas pesam menos no custo
|
||||
}
|
||||
if det_params:
|
||||
_det.update(det_params)
|
||||
|
|
@ -617,30 +733,41 @@ class CameraManager:
|
|||
grid_w, grid_h = grid_shape
|
||||
H1, W1 = seg_ids.shape
|
||||
|
||||
# --- 1) Depth: opcional ---
|
||||
# --------------------------------------------------
|
||||
# 1) Pré-processamento do depth
|
||||
# --------------------------------------------------
|
||||
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 ---
|
||||
# --------------------------------------------------
|
||||
# 2) Bordas da grid
|
||||
# --------------------------------------------------
|
||||
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 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)
|
||||
# --------------------------------------------------
|
||||
# 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
|
||||
# mapas de detecção
|
||||
det_cov_max = np.zeros((grid_h, grid_w), np.float32)
|
||||
det_conf_max = np.zeros((grid_h, grid_w), np.float32)
|
||||
det_score = np.zeros((grid_h, grid_w), np.float32)
|
||||
det_top_label_id = -np.ones((grid_h, grid_w), np.int32)
|
||||
det_top_conf = np.zeros((grid_h, grid_w), np.float32)
|
||||
det_is_veto = np.zeros((grid_h, grid_w), np.float32) # debug útil
|
||||
|
||||
# --------------------------------------------------
|
||||
# 4) grid_ref 2D
|
||||
# --------------------------------------------------
|
||||
grid_ref = np.asarray(grid_ref, dtype=np.float32)
|
||||
|
||||
# --- 4) grid_ref 2D ---
|
||||
if grid_ref.ndim == 1:
|
||||
if grid_ref.shape[0] != grid_h:
|
||||
raise ValueError(f"grid_ref 1D deve ter len={grid_h}, veio {grid_ref.shape}")
|
||||
|
|
@ -650,27 +777,28 @@ 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) Loop por célula: seg + (depth se ativo) ---
|
||||
# --------------------------------------------------
|
||||
# 5) Loop por célula: segmentação + depth
|
||||
# --------------------------------------------------
|
||||
for j in range(grid_h):
|
||||
y0, y1 = int(y_edges[j]), int(y_edges[j+1])
|
||||
seg_row = seg_ids[y0:y1, :]
|
||||
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 = int(x_edges[i]), int(x_edges[i+1])
|
||||
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_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
|
||||
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 (apenas se usando)
|
||||
if depth_row is not None:
|
||||
depth_block = depth_row[:, x0:x1]
|
||||
vals = depth_block[~np.isnan(depth_block)]
|
||||
|
|
@ -679,28 +807,35 @@ class CameraManager:
|
|||
if valid >= max(int(min_valid_frac * n), 1):
|
||||
z_med[j, i] = np.nanmedian(vals) / 1000.0 # mm -> m
|
||||
|
||||
# --- 6) Confiança da célula ---
|
||||
# --------------------------------------------------
|
||||
# 6) Confiança da célula
|
||||
# --------------------------------------------------
|
||||
t0, t1 = conf_params
|
||||
conf_seg = np.maximum.reduce([pct_navegavel, pct_nao_navegavel])
|
||||
conf_seg = np.maximum(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
|
||||
conf_dep = np.clip((depth_valid_frac - t0) / max(1e-6, (t1 - t0)), 0.0, 1.0)
|
||||
conf_cell = 0.65 * conf_seg + 0.35 * 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) ---
|
||||
# --------------------------------------------------
|
||||
# 7) Rasterização das detecções
|
||||
# --------------------------------------------------
|
||||
if deteccoes:
|
||||
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"])
|
||||
label = str(det.get("label", "")).strip()
|
||||
label_id = int(det.get("label_id", -1))
|
||||
|
||||
is_veto = (label in _det["veto_labels"])
|
||||
w_class = float(_det["class_weights"].get(label, 1.0))
|
||||
if not is_veto:
|
||||
w_class *= float(_det["non_veto_cost_scale"])
|
||||
|
||||
# bbox em px
|
||||
if "bbox_px" in det and det["bbox_px"]:
|
||||
|
|
@ -711,6 +846,7 @@ class CameraManager:
|
|||
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
|
||||
|
||||
|
|
@ -720,98 +856,147 @@ class CameraManager:
|
|||
|
||||
# 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"))
|
||||
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"))
|
||||
j1 = min(grid_h - 1, np.searchsorted(y_edges, y1p, side="left"))
|
||||
|
||||
for j in range(j0, j1+1):
|
||||
y0c, y1c = int(y_edges[j]), int(y_edges[j+1])
|
||||
for i in range(i0, i1+1):
|
||||
x0c, x1c = int(x_edges[i]), int(x_edges[i+1])
|
||||
for j in range(j0, j1 + 1):
|
||||
y0c, y1c = int(y_edges[j]), int(y_edges[j + 1])
|
||||
|
||||
for i in range(i0, i1 + 1):
|
||||
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)
|
||||
|
||||
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))
|
||||
|
||||
inter = float((ix1 - ix0) * (iy1 - iy0))
|
||||
cell_area = float((x1c - x0c) * (y1c - y0c))
|
||||
if cell_area <= 0:
|
||||
continue
|
||||
|
||||
cov = inter / cell_area
|
||||
if cov < _det["min_cell_coverage"]:
|
||||
continue
|
||||
|
||||
base = conf if not veto else 1.0
|
||||
s = base * cov * w_class
|
||||
score_local = conf * cov * w_class
|
||||
|
||||
det_cov_max[j, i] = max(det_cov_max[j, i], cov)
|
||||
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)
|
||||
det_score[j, i] = np.clip(det_score[j, i] + score_local, 0.0, 1.0)
|
||||
else:
|
||||
det_score[j, i] = max(det_score[j, i], s)
|
||||
det_score[j, i] = max(det_score[j, i], score_local)
|
||||
|
||||
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))
|
||||
# guarda top label por confiança*cobertura, priorizando veto
|
||||
priority = (2.0 if is_veto else 1.0) * conf * cov
|
||||
if priority > det_top_conf[j, i]:
|
||||
det_top_conf[j, i] = priority
|
||||
det_top_label_id[j, i] = label_id
|
||||
det_is_veto[j, i] = 1.0 if is_veto else 0.0
|
||||
|
||||
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 de solo ---
|
||||
# --------------------------------------------------
|
||||
# 8) Anomalia física (positiva e negativa)
|
||||
# --------------------------------------------------
|
||||
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
|
||||
delta_signed = Z_ref - z_med
|
||||
delta_signed = np.where(np.isnan(z_med), 0.0, delta_signed)
|
||||
|
||||
# Algo mais perto do que deveria: obstáculo / saliência
|
||||
delta_up = np.maximum(delta_signed, 0.0)
|
||||
anom_up_raw = np.clip(delta_up / max(1e-6, anom_satur_up_m), 0.0, 1.0)
|
||||
anom_up = (
|
||||
anom_up_raw *
|
||||
(delta_up > anom_tau_up).astype(np.float32) *
|
||||
np.maximum(conf_dep, 0.25)
|
||||
)
|
||||
|
||||
# Algo mais longe do que deveria: buraco / vala / queda
|
||||
delta_down = np.maximum(-delta_signed, 0.0)
|
||||
anom_down_raw = np.clip(delta_down / max(1e-6, anom_satur_down_m), 0.0, 1.0)
|
||||
anom_down = (
|
||||
anom_down_raw *
|
||||
(delta_down > anom_tau_down).astype(np.float32) *
|
||||
np.maximum(conf_dep, 0.25)
|
||||
)
|
||||
|
||||
# Anomalia física final combinada
|
||||
anom = np.clip(np.maximum(anom_up, anom_down_weight * anom_down), 0.0, 1.0)
|
||||
|
||||
else:
|
||||
anom_up = np.zeros_like(pct_navegavel, dtype=np.float32)
|
||||
anom_down = np.zeros_like(pct_navegavel, dtype=np.float32)
|
||||
anom = np.zeros_like(pct_navegavel, dtype=np.float32)
|
||||
|
||||
# --- 8) Custo e navegabilidade ---
|
||||
# --------------------------------------------------
|
||||
# 9) Custo suave
|
||||
# --------------------------------------------------
|
||||
nao_navegavel = 1.0 - pct_navegavel
|
||||
w1, w2, w3 = w
|
||||
custo = w1 * nao_navegavel + w2 * anom + w3 * (1.0 - conf_cell)
|
||||
|
||||
if deteccoes:
|
||||
custo = np.clip(custo + _det["w4"] * det_score, 0.0, 1.0)
|
||||
custo_base = (
|
||||
w1 * nao_navegavel +
|
||||
w2 * anom +
|
||||
w3 * (1.0 - conf_cell)
|
||||
)
|
||||
|
||||
if deteccoes:
|
||||
navegavel = (
|
||||
(pct_navegavel >= 0.55) &
|
||||
(anom < 0.4) &
|
||||
(conf_cell >= 0.5) &
|
||||
(det_score < _det["thr_det_block"])
|
||||
)
|
||||
else:
|
||||
navegavel = (
|
||||
(pct_navegavel >= 0.55) &
|
||||
(anom < 0.4) &
|
||||
(conf_cell >= 0.5)
|
||||
)
|
||||
# Detecção pesa pouco no custo geral; veto é deixado para o fuser
|
||||
custo = np.clip(custo_base + _det["w4"] * det_score, 0.0, 1.0)
|
||||
|
||||
# --------------------------------------------------
|
||||
# 10) Navegabilidade por célula
|
||||
# --------------------------------------------------
|
||||
# Regra:
|
||||
# - segmentação é a base
|
||||
# - anomalia forte derruba
|
||||
# - confiança muito baixa derruba somente a navegabilidade local
|
||||
# - detecção crítica (veto) forte pode derrubar navegabilidade
|
||||
veto_soft_block = (det_is_veto > 0.5) & (det_score >= _det["thr_det_soft"])
|
||||
|
||||
navegavel = (
|
||||
(pct_navegavel >= 0.55) &
|
||||
(anom < 0.45) &
|
||||
(conf_cell >= 0.35) &
|
||||
(~veto_soft_block if _det["only_veto_blocks_nav"] else (det_score < _det["thr_det_soft"]))
|
||||
)
|
||||
|
||||
# --------------------------------------------------
|
||||
# 11) Retorno
|
||||
# --------------------------------------------------
|
||||
return {
|
||||
"pct_navegavel": pct_navegavel,
|
||||
"pct_nao_navegavel": pct_nao_navegavel,
|
||||
"pct_navegavel": np.clip(pct_navegavel, 0.0, 1.0),
|
||||
"pct_nao_navegavel": np.clip(pct_nao_navegavel, 0.0, 1.0),
|
||||
|
||||
"z_med": z_med,
|
||||
"z_ref": Z_ref,
|
||||
"depth_valid_frac": depth_valid_frac,
|
||||
"depth_valid_frac": np.clip(depth_valid_frac, 0.0, 1.0),
|
||||
|
||||
"conf": np.clip(conf_cell, 0.0, 1.0),
|
||||
"anom_up": np.clip(anom_up, 0.0, 1.0),
|
||||
"anom_down": np.clip(anom_down, 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),
|
||||
"det_cov_max": det_cov_max,
|
||||
"det_conf_max": det_conf_max,
|
||||
"det_score": det_score,
|
||||
|
||||
"det_cov_max": np.clip(det_cov_max, 0.0, 1.0),
|
||||
"det_conf_max": np.clip(det_conf_max, 0.0, 1.0),
|
||||
"det_score": np.clip(det_score, 0.0, 1.0),
|
||||
"det_top_label_id": det_top_label_id,
|
||||
"det_top_conf": det_top_conf,
|
||||
"det_top_conf": np.clip(det_top_conf, 0.0, 1.0),
|
||||
|
||||
# debug opcional
|
||||
"det_is_veto": det_is_veto.astype(np.float32),
|
||||
}
|
||||
|
||||
except Exception as e:
|
||||
self.mostrar_log(f"Erro ao construir grid de confianca: {e}")
|
||||
return None
|
||||
|
||||
|
||||
def debug_blockage_imshow(
|
||||
self,
|
||||
rgb_frame,
|
||||
|
|
|
|||
File diff suppressed because it is too large
Load Diff
Loading…
Reference in New Issue