adicionado costmap_fuser
This commit is contained in:
Diego Freitas
parent
c3d4541964
commit
319c07af8c
|
|
@ -0,0 +1,241 @@
|
|||
import math
|
||||
import time
|
||||
import numpy as np
|
||||
|
||||
|
||||
class CostmapFuser:
|
||||
def __init__(
|
||||
self,
|
||||
grid_shape=(15, 10), # (cols, rows)
|
||||
K=3, # tamanho do buffer temporal
|
||||
M=2, # persistência M-de-N p/ navegável
|
||||
fuse_method="q0.7", # "q0.7" (quantil) ou "max"
|
||||
block_thr=0.7, # célula bloqueia se custo > thr
|
||||
central_cols=None, # (i0, i1) inclusivo; None = 3 colunas centrais
|
||||
y_range_m=(0.5, 5.0), # m: perto..longe (se não houver z_ref)
|
||||
near_is_bottom=True, # linha de baixo é mais perto?
|
||||
fov_h_rad=None
|
||||
):
|
||||
self.grid_w, self.grid_h = grid_shape
|
||||
self.K = int(K)
|
||||
self.M = int(M)
|
||||
self.block_thr = float(block_thr)
|
||||
self.fuse_method = fuse_method
|
||||
self.y_range_m = y_range_m
|
||||
self.near_is_bottom = near_is_bottom
|
||||
self.fov_h_rad = fov_h_rad
|
||||
|
||||
if central_cols is None:
|
||||
# 3 colunas centrais
|
||||
mid = self.grid_w // 2
|
||||
self.central_cols = (max(0, mid - 1), min(self.grid_w - 1, mid + 1))
|
||||
else:
|
||||
self.central_cols = central_cols
|
||||
|
||||
# ring-buffers
|
||||
self.buf_custo = []
|
||||
self.buf_conf = []
|
||||
self.buf_anom = []
|
||||
self.buf_nav = []
|
||||
self.buf_zref = [] # opcional
|
||||
self.buf_ts = []
|
||||
|
||||
self.seq = 0
|
||||
|
||||
def _stack(self, lst, fallback_val=0.0):
|
||||
"""Empilha listas de arrays (grid_h,grid_w). Se vazio, devolve um array fill."""
|
||||
if len(lst) == 0:
|
||||
return np.full((0, self.grid_h, self.grid_w), fallback_val, np.float32)
|
||||
return np.stack(lst, axis=0)
|
||||
|
||||
def _fuse_array(self, stack, method, q=0.7, avg=False):
|
||||
"""Fusão temporal de um stack (T, H, W)."""
|
||||
if stack.shape[0] == 0:
|
||||
return np.zeros((self.grid_h, self.grid_w), np.float32)
|
||||
if avg:
|
||||
return stack.mean(axis=0).astype(np.float32)
|
||||
if method == "max":
|
||||
return np.max(stack, axis=0).astype(np.float32)
|
||||
# quantil
|
||||
return np.quantile(stack, q, axis=0).astype(np.float32)
|
||||
|
||||
def _rows_near_to_far(self):
|
||||
"""Ordem das linhas do mais perto ao mais longe."""
|
||||
if self.near_is_bottom:
|
||||
# j = grid_h-1 (baixo) é mais perto
|
||||
return range(self.grid_h - 1, -1, -1)
|
||||
# j=0 (topo) é mais perto
|
||||
return range(0, self.grid_h, 1)
|
||||
|
||||
def _row_distances(self, z_ref_2d=None):
|
||||
"""Distância (m) por linha (H,), usando z_ref se disponível."""
|
||||
if z_ref_2d is not None and np.isfinite(z_ref_2d).any():
|
||||
# mediana por linha na janela central
|
||||
i0, i1 = self.central_cols
|
||||
z_line = np.nanmedian(z_ref_2d[:, i0:i1 + 1], axis=1) # (H,)
|
||||
if np.isfinite(z_line).any():
|
||||
return z_line
|
||||
# fallback linear: mapeia linhas para [y_min..y_max]
|
||||
y_min, y_max = self.y_range_m
|
||||
lin = np.linspace(y_min, y_max, self.grid_h).astype(np.float32)
|
||||
if self.near_is_bottom:
|
||||
# bottom (j=H-1) = y_min; top (j=0) = y_max
|
||||
return lin[::-1]
|
||||
return lin
|
||||
|
||||
def _compute_d_obs_min(self, custo_fused, nav_fused, z_ref_2d=None):
|
||||
"""Menor distância livre (m) na janela central; retorna None se livre."""
|
||||
i0, i1 = self.central_cols
|
||||
blocked = (custo_fused[:, i0:i1 + 1] > self.block_thr)
|
||||
if nav_fused is not None:
|
||||
blocked |= (nav_fused[:, i0:i1 + 1] == 0)
|
||||
|
||||
if not blocked.any():
|
||||
return None
|
||||
|
||||
row_dists = self._row_distances(z_ref_2d)
|
||||
d_min = None
|
||||
for j in self._rows_near_to_far():
|
||||
if blocked[j].any():
|
||||
d = float(row_dists[j])
|
||||
d_min = d if (d_min is None or d < d_min) else d_min
|
||||
return d_min
|
||||
|
||||
def _row_scale_x(self, row_dist_m):
|
||||
"""metros por célula em X para cada linha, dado FOV_H."""
|
||||
if self.fov_h_rad is None:
|
||||
return None
|
||||
# largura coberta naquela linha
|
||||
row_width = 2.0 * row_dist_m * math.tan(0.5 * float(self.fov_h_rad)) # (H,)
|
||||
# metros por coluna
|
||||
return (row_width / float(self.grid_w)).astype(np.float32) # (H,)
|
||||
|
||||
def update(self, grid_dict, ts=None):
|
||||
"""
|
||||
grid_dict deve conter (grid_h,grid_w): "custo","conf","anom","navegavel"
|
||||
opcional: "z_ref" (m) 2D
|
||||
Retorna snapshot pronto pra serializar e gravar no Redis.
|
||||
"""
|
||||
if ts is None:
|
||||
ts = time.perf_counter()
|
||||
custo = grid_dict["custo"].astype(np.float32)
|
||||
conf = grid_dict["conf"].astype(np.float32)
|
||||
anom = grid_dict["anom"].astype(np.float32)
|
||||
nav = grid_dict["navegavel"].astype(np.float32) # 0/1
|
||||
zref = grid_dict.get("z_ref", None)
|
||||
if zref is not None:
|
||||
zref = zref.astype(np.float32)
|
||||
|
||||
# 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)}"
|
||||
|
||||
# push no ring-buffer
|
||||
self.buf_custo.append(custo)
|
||||
self.buf_conf.append(conf)
|
||||
self.buf_anom.append(anom)
|
||||
self.buf_nav.append(nav)
|
||||
self.buf_ts.append(ts)
|
||||
self.buf_zref.append(zref)
|
||||
|
||||
# mantém no máximo K
|
||||
if len(self.buf_custo) > self.K:
|
||||
self.buf_custo.pop(0); self.buf_conf.pop(0); self.buf_anom.pop(0)
|
||||
self.buf_nav.pop(0); self.buf_ts.pop(0); self.buf_zref.pop(0)
|
||||
|
||||
# empilha
|
||||
S_custo = self._stack(self.buf_custo, 0.0)
|
||||
S_conf = self._stack(self.buf_conf, 0.0)
|
||||
S_anom = self._stack(self.buf_anom, 0.0)
|
||||
S_nav = self._stack(self.buf_nav, 0.0)
|
||||
|
||||
# fusão
|
||||
q = 0.7
|
||||
if self.fuse_method.startswith("q"):
|
||||
try:
|
||||
q = float(self.fuse_method[1:])
|
||||
except Exception:
|
||||
q = 0.7
|
||||
custo_f = self._fuse_array(S_custo, "q", q=q)
|
||||
anom_f = self._fuse_array(S_anom, "q", q=q)
|
||||
else:
|
||||
custo_f = self._fuse_array(S_custo, "max")
|
||||
anom_f = self._fuse_array(S_anom, "max")
|
||||
|
||||
conf_f = self._fuse_array(S_conf, method="q", q=0.5, avg=True) # média
|
||||
# navegável: M-de-N (soma >= M)
|
||||
if S_nav.shape[0] > 0:
|
||||
nav_f = (S_nav.sum(axis=0) >= self.M).astype(np.uint8)
|
||||
else:
|
||||
nav_f = np.zeros((self.grid_h, self.grid_w), np.uint8)
|
||||
|
||||
# z_ref fundido (opcional) só p/ d_obs_min
|
||||
zref_f = None
|
||||
if any(z is not None for z in self.buf_zref):
|
||||
# pega a última não-nula
|
||||
for z in reversed(self.buf_zref):
|
||||
if z is not None:
|
||||
zref_f = z
|
||||
break
|
||||
|
||||
d_obs_min = self._compute_d_obs_min(custo_f, nav_f, zref_f)
|
||||
|
||||
# distâncias por linha (m), usando z_ref se houver; senão, mapeamento linear y_range_m
|
||||
row_dist = self._row_distances(zref_f).astype(np.float32) # shape (grid_h,)
|
||||
row_dist_m = row_dist.astype(np.float32) / 1000.0
|
||||
|
||||
# escala X por linha (m/col)
|
||||
row_scale_x = self._row_scale_x(row_dist) # (H,) ou None
|
||||
|
||||
# incrementa seq
|
||||
self.seq += 1
|
||||
|
||||
# empacota para JSON (u8 para leveza)
|
||||
def to_u8_list(a):
|
||||
return np.clip((a * 255.0), 0, 255).astype(np.uint8).ravel().tolist()
|
||||
|
||||
snap = {
|
||||
"ts": float(ts),
|
||||
"seq": int(self.seq),
|
||||
"grid_w": int(self.grid_w),
|
||||
"grid_h": int(self.grid_h),
|
||||
"fuse": {
|
||||
"K": int(self.K),
|
||||
"M": int(self.M),
|
||||
"method": self.fuse_method,
|
||||
"block_thr": float(self.block_thr),
|
||||
"central_cols": [int(self.central_cols[0]), int(self.central_cols[1])],
|
||||
"near_is_bottom": bool(self.near_is_bottom),
|
||||
},
|
||||
"y_range_m": [float(self.y_range_m[0]), float(self.y_range_m[1])],
|
||||
"d_obs_min": (None if d_obs_min is None else float(d_obs_min)),
|
||||
"row_dist_m": row_dist_m.tolist(),
|
||||
"row_scale_x_m": row_scale_x.tolist(),
|
||||
"custo_u8": to_u8_list(custo_f),
|
||||
"conf_u8": to_u8_list(conf_f),
|
||||
"anom_u8": to_u8_list(anom_f),
|
||||
"nav_mask": nav_f.astype(np.uint8).ravel().tolist()
|
||||
}
|
||||
|
||||
# (opcional) incluir z_ref_u8 pra debug/visualização
|
||||
# if zref_f is not None:
|
||||
# zref_u8 = np.clip(zref_f / self.y_range_m[1] * 255.0, 0, 255).astype(np.uint8)
|
||||
# snap["zref_u8"] = zref_u8.ravel().tolist()
|
||||
|
||||
return snap
|
||||
|
||||
def unpack_snapshot(snap):
|
||||
H, W = snap["grid_h"], snap["grid_w"]
|
||||
custo_u8 = np.array(snap["custo_u8"], dtype=np.uint8).reshape(H, W)
|
||||
custo = custo_u8.astype(np.float32) / 255.0
|
||||
conf_u8 = np.array(snap["conf_u8"], dtype=np.uint8).reshape(H, W)
|
||||
conf = conf_u8.astype(np.float32) / 255.0
|
||||
anom_u8 = np.array(snap["anom_u8"], dtype=np.uint8).reshape(H, W)
|
||||
anom = anom_u8.astype(np.float32) / 255.0
|
||||
nav_u8 = np.array(snap["nav_mask"], dtype=np.uint8).reshape(H, W)
|
||||
nav = nav_u8.astype(bool)
|
||||
return custo, conf, anom, nav
|
||||
|
||||
def thr_u8(snap):
|
||||
thr = float(snap["fuse"]["block_thr"]) # ex.: 0.7
|
||||
return int(round(thr * 255.0)) # 0.7 -> 179
|
||||
Loading…
Reference in New Issue