From 319c07af8ca150154cb294708fac612974bd8621 Mon Sep 17 00:00:00 2001 From: Diego Freitas Date: Wed, 13 Aug 2025 14:48:52 -0300 Subject: [PATCH] adicionado costmap_fuser --- .../processamento/costmap_fuser.py | 241 ++++++++++++++++++ 1 file changed, 241 insertions(+) create mode 100644 AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/processamento/costmap_fuser.py diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/processamento/costmap_fuser.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/processamento/costmap_fuser.py new file mode 100644 index 000000000..229cf5acd --- /dev/null +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/processamento/costmap_fuser.py @@ -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