# SPDX-License-Identifier: Apache-2.0 """Host post-processing of METEOR's C++ runtime (tier4/METEOR @ dc193a81e995 ``deploy/cpp/decode.cpp`` + ``render.cpp``; research/meteor/SPEC.md section 5). numpy (OpenCV for the rotated-rectangle intersection when installed). Per frame, stateless (the temporal parts are in ``host.temporal``): - 3D boxes (``decode.cpp:27-57`` via ``render.cpp:383-395``): sigmoid(hm) > 0.15 at 3x3 peaks (no strictly greater neighbour), best 64 by score; x = 80 - (r + reg0) 0.4, y = 50 - (c + reg1) 0.4, l = e^reg2, w = e^reg3, yaw = atan2(reg4, reg5) (float32 as the C++); vehicle > 0.35, VRU > 0.15, x in [-(80 - 2), 80 - 2]; greedy same-class BEV NMS, suppressed if rotated IoU > 0.3 or intersection / smaller area > 0.6 (``render.cpp:283-319``). - Stationary flag (``render.cpp:265-276, 590-610``): stationary logit at the box cell > 0.0 when the head is healthy (std >= 0.05 and range >= 0.2 over the map), else the best traj mode's last waypoint |wp6| < 0.5 m. - Agent futures (``render.cpp:631-655``): the traj mode argmax at the box cell, waypoints = centre + (dx, dy)_t at t = 0.5 .. 3 s (the renderer draws vehicles only; every box carries its future here, ``kind`` tells them apart). - 2D boxes per camera and stride (``decode.cpp:59-108``): logits clipped to +-50, sigmoid > 0.30 at 3x3 peaks (on the clipped plane), best 48 per camera and scale; cx = (c + reg1) s, cy = (r + reg0) s, w = e^reg2 s, h = e^reg3 s; class 7 (road paint) hidden (``METEOR_2D_HIDE``). - Unknown obstacles (``render.cpp:657-729``): 2D class 0 boxes >= TH_UNK placed on the ground plane through the bottom-centre ray (range 1.5-60 m), else back-projected with the median ``depth_mean`` of a 5x5 window (1.5-50 m); |x| <= 60, |y| <= 25; greedy de-duplication within 1.5 m by score. - Plan (``render.cpp:432-452``): mode logits + 0.35 on the previous mode, argmax, straight preference (mode 0 unless the margin is >= 1.0); the selected path, softmax confidences of the (hysteresis-adjusted) logits, controls. - BEV seg road-edge thinning (``render.cpp:252-262``), traffic light argmax, risk = sigmoid, occupancy argmax, metric depth of a bin index (linear 1 + 1.25 b as trained, SPEC section 10 risk 2). Ties: ``std::sort`` of the C++ is not stable; this module sorts candidates stably in (class, row, col) order, so equal scores (saturated sigmoids) keep the scan order (documented policy, a set-based gate is insensitive to it). """ from __future__ import annotations import math from dataclasses import dataclass, field, replace from typing import Any, Dict, List, Optional, Sequence, Tuple import numpy as np from ..reference import config as C __all__ = ["HOST_POST_MODES", "host_post_mode", "set_host_post", "peak_mask_nd", "PostConfig", "Box3D", "Box2D", "sigmoid32", "peak_mask", "decode_boxes_3d", "filter_boxes_3d", "rotated_intersection", "bev_nms", "stationary_healthy", "annotate_boxes", "decode_boxes_2d", "ground_point", "unknown_obstacles", "select_mode", "plan_from_ego", "thin_road_edge", "decode_frame", "metric_depth", "tl_state", "occupancy_classes"] F32 = np.float32 @dataclass(frozen=True) class PostConfig: """Every host knob of METEOR's runtime with its default (RT-host; ``render.cpp:345-376``, ``param.yaml:36-40``).""" det3d_threshold: float = 0.15 # decode pre-threshold (render.cpp:388) det3d_topk: int = 64 vehicle_threshold: float = 0.35 # render.cpp:391 vru_threshold: float = 0.15 range_margin: float = 2.0 # x in [-(XR - 2), XF - 2] bev_nms_iou: float = 0.3 # METEOR_BEV_NMS_IOU bev_nms_containment: float = 0.6 # render.cpp:316 stationary_logit_threshold: float = 0.0 # METEOR_STAT_LOGIT_THRESH stationary_health_std: float = 0.05 stationary_health_range: float = 0.2 stationary_fallback_m: float = 0.5 det2d_threshold: float = 0.30 # METEOR_TH2D (C++ and param.yaml; Python orin_render 0.50) det2d_topk: int = 48 det2d_clip: float = 50.0 det2d_hide: Tuple[int, ...] = (7,) # METEOR_2D_HIDE (road_paint) unk2d: bool = True # METEOR_UNK2D unk2d_threshold: Optional[float] = None # METEOR_UNK2D_TH (default: det2d_threshold) ground_z: float = 0.0 # METEOR_GROUND_Z unk2d_ground_range: Tuple[float, float] = (1.5, 60.0) unk2d_depth_range: Tuple[float, float] = (1.5, 50.0) unk2d_max_x: float = 60.0 unk2d_max_y: float = 25.0 unk2d_dedupe_m: float = 1.5 mode_hysteresis: float = 0.35 # render.cpp:436 straight_margin: float = 1.0 # render.cpp:441 thin_road_edge: bool = True # METEOR_NO_THIN = 0 seg_fuse: bool = True # METEOR_SEG_FUSE (host.temporal) yaw_smoothing: bool = True # render.cpp:396-421 (host.temporal) risk_gain: float = 1.0 # METEOR_RISK_GAIN (display only) def with_params(self, **params: Any) -> "PostConfig": return replace(self, **{k: v for k, v in params.items() if v is not None}) @property def th_unk(self) -> float: return self.det2d_threshold if self.unk2d_threshold is None else self.unk2d_threshold @dataclass class Box3D: """One BEV box (ego frame, metres, radians CCW from +x); ``cell`` is the 400x250 detection-grid cell.""" cls: int score: float x: float y: float l: float w: float yaw: float cell: Tuple[int, int] stationary: bool = False future_mode: int = -1 future: Optional[np.ndarray] = None # [6, 2] absolute waypoints (ego frame) @property def label(self) -> str: return C.DET3D_CLASSES[self.cls] def to_dict(self) -> Dict[str, Any]: d = {"cls": self.cls, "label": self.label, "score": float(self.score), "x": float(self.x), "y": float(self.y), "l": float(self.l), "w": float(self.w), "yaw": float(self.yaw), "cell": [int(v) for v in self.cell], "stationary": bool(self.stationary), "future_mode": int(self.future_mode)} if self.future is not None: d["future"] = np.asarray(self.future, np.float64).round(4).tolist() return d @dataclass class Box2D: cls: int score: float cx: float cy: float w: float h: float stride: int @property def label(self) -> str: return C.DET2D_CLASSES[self.cls] def xyxy(self) -> List[float]: return [self.cx - self.w / 2, self.cy - self.h / 2, self.cx + self.w / 2, self.cy + self.h / 2] def to_dict(self) -> Dict[str, Any]: return {"cls": self.cls, "label": self.label, "score": float(self.score), "cx": float(self.cx), "cy": float(self.cy), "w": float(self.w), "h": float(self.h), "stride": int(self.stride)} def sigmoid32(x: Any) -> np.ndarray: """``1.0f / (1.0f + std::exp(-x))`` in float32 (decode.cpp ``sigmoidf``).""" x = np.asarray(x, F32) with np.errstate(over="ignore"): return (F32(1.0) / (F32(1.0) + np.exp(-x))).astype(F32) def peak_mask(plane: np.ndarray) -> np.ndarray: """True where no in-bounds 3x3 neighbour is strictly greater (``decode.cpp`` isPeak).""" p = np.pad(np.asarray(plane, F32), 1, constant_values=-np.inf) h, w = plane.shape m = np.full(plane.shape, -np.inf, F32) for dr in range(3): for dc in range(3): m = np.maximum(m, p[dr:dr + h, dc:dc + w]) return plane >= m # OPT_PLAN #9 (``METEOR_HOST_POST``): ``vec`` = the same decode vectorised over classes / cameras (peaks, sigmoid, # thresholds on whole [N, C, H, W] arrays; seg fusion on planar [9, H, W] state); ``stock`` = the port's per-plane loops. # Both give identical results (tests/test_host_postprocess.py, test_host_temporal.py; real-frame check in OPT_REPORT). # ``sparse`` (OPT round 4 R4.4) = ``vec`` with the peak decode evaluated only where it can pass: a cheap logit # pre-filter (``v > logit(thresh) - 0.01``: sigmoid is monotone and sigmoid(logit(t) - 0.01) < t - 0.002, far beyond # any float32 rounding), then the clip, ``sigmoid32``, the threshold and the 3x3 peak test (-inf outside the plane) on # the surviving elements only; the same elementwise float32 ops on the same values, in the same (n, c, r, q) order, # so the candidates are identical (tests/test_host_postprocess.py, real-frame dumps in OPT_REPORT R4.4). HOST_POST_MODES = ("stock", "vec", "sparse") HOST_POST_DEFAULT = "sparse" _HOST_POST = {"mode": None} def host_post_mode() -> str: import os mode = _HOST_POST["mode"] or os.environ.get("METEOR_HOST_POST", HOST_POST_DEFAULT).strip().lower() if mode not in HOST_POST_MODES: raise ValueError(f"METEOR_HOST_POST={mode!r}: expected one of {HOST_POST_MODES}") return mode def set_host_post(mode: Optional[str]) -> None: if mode is not None and mode not in HOST_POST_MODES: raise ValueError(f"host_post={mode!r}: expected one of {HOST_POST_MODES}") _HOST_POST["mode"] = mode def peak_mask_nd(planes: np.ndarray) -> np.ndarray: """:func:`peak_mask` of every [H, W] plane of a [..., H, W] float32 array at once (the same comparisons).""" v = np.asarray(planes, F32) pad = [(0, 0)] * (v.ndim - 2) + [(1, 1), (1, 1)] p = np.pad(v, pad, constant_values=-np.inf) h, w = v.shape[-2:] m = np.full(v.shape, -np.inf, F32) for dr in range(3): for dc in range(3): np.maximum(m, p[..., dr:dr + h, dc:dc + w], out=m) return v >= m def _candidates_nd(planes: np.ndarray, thresh: float, topk: int, clip: Optional[float] = None): """:func:`_candidates` of each leading index of a [N, C, H, W] array -> N lists (vectorised; same order).""" v = np.asarray(planes, F32) if clip is not None: v = np.clip(v, F32(-clip), F32(clip)) p = sigmoid32(v) mask = (p > F32(thresh)) & peak_mask_nd(v) res = [] for n in range(v.shape[0]): cc, rr, qq = np.nonzero(mask[n]) sc = p[n][cc, rr, qq] order = np.argsort(-sc, kind="stable")[:topk] res.append([(float(sc[i]), int(cc[i]), int(rr[i]), int(qq[i])) for i in order.tolist()]) return res def _candidates_sparse(planes: np.ndarray, thresh: float, topk: int, clip: Optional[float] = None): """:func:`_candidates_nd` evaluated only at the elements that pass a logit pre-filter (``HOST_POST=sparse``, R4.4; identical results: see :data:`HOST_POST_MODES`).""" v = np.asarray(planes, F32) t = float(thresh) lo = (math.log(t / (1.0 - t)) - 0.01) if 0.0 < t < 1.0 else None if lo is None or (clip is not None and lo <= -float(clip)): return _candidates_nd(planes, thresh, topk, clip) n_, c_, h, w = v.shape idx = np.flatnonzero(v.reshape(-1) > F32(lo)) # (n, c, r, q) order res: List[list] = [[] for _ in range(n_)] if idx.size == 0: return res flat = v.reshape(-1) x = flat[idx] if clip is not None: x = np.clip(x, F32(-clip), F32(clip)) p = sigmoid32(x) keep = p > F32(thresh) idx, x, p = idx[keep], x[keep], p[keep] q = idx % w r = (idx // w) % h ok = np.ones(idx.shape, bool) for dr in (-1, 0, 1): for dc in (-1, 0, 1): if dr == 0 and dc == 0: continue rr, qq = r + dr, q + dc inb = (rr >= 0) & (rr < h) & (qq >= 0) & (qq < w) nb = np.full(idx.shape, -np.inf, F32) nb[inb] = flat[idx[inb] + dr * w + dc] if clip is not None: nb = np.clip(nb, F32(-clip), F32(clip)) ok &= x >= nb idx, p = idx[ok], p[ok] plane = h * w nn = idx // (c_ * plane) cc = (idx // plane) % c_ r = (idx // w) % h q = idx % w bounds = np.searchsorted(nn, np.arange(n_ + 1)) for n in range(n_): a, b = int(bounds[n]), int(bounds[n + 1]) sc = p[a:b] order = np.argsort(-sc, kind="stable")[:topk] + a res[n] = [(float(p[i]), int(cc[i]), int(r[i]), int(q[i])) for i in order.tolist()] return res def _candidates(planes: np.ndarray, thresh: float, topk: int, clip: Optional[float] = None): """(score, cls, row, col) of the thresholded peaks of [C, H, W] logits, best ``topk`` (stable by scan order).""" out = [] for c in range(planes.shape[0]): v = np.asarray(planes[c], F32) if clip is not None: v = np.clip(v, F32(-clip), F32(clip)) p = sigmoid32(v) rr, cc = np.nonzero((p > F32(thresh)) & peak_mask(v)) for r, q in zip(rr.tolist(), cc.tolist()): out.append((float(p[r, q]), c, r, q)) out.sort(key=lambda z: -z[0]) return out[:topk] def decode_boxes_3d(hm: np.ndarray, reg: np.ndarray, *, thresh: float = 0.15, topk: int = 64) -> List[Box3D]: """``decode_boxes`` (decode.cpp:27-57) on hm [2, 400, 250] logits and reg [6, 400, 250]; float32 box math.""" hm = np.asarray(hm, F32).reshape(-1, C.DET_H, C.DET_W) reg = np.asarray(reg, F32).reshape(6, C.DET_H, C.DET_W) res = F32(C.DET_RES) boxes = [] mode = host_post_mode() cand = (_candidates_sparse if mode == "sparse" else _candidates_nd)(hm[None], thresh, topk)[0] \ if mode != "stock" else _candidates(hm, thresh, topk) for sc, c, r, q in cand: R = reg[:, r, q] x = F32(80.0) - (F32(r) + R[0]) * res y = F32(50.0) - (F32(q) + R[1]) * res boxes.append(Box3D(c, sc, float(x), float(y), float(np.exp(R[2])), float(np.exp(R[3])), float(np.arctan2(R[4], R[5])), (r, q))) return boxes def filter_boxes_3d(boxes: Sequence[Box3D], cfg: PostConfig = PostConfig()) -> List[Box3D]: """Class thresholds (vehicle > 0.35, VRU > 0.15) and the grid-range clip (``render.cpp:388-394``).""" xr = C.BEV_H * C.BEV_RES - C.BEV_XF out = [] for b in boxes: th = cfg.vehicle_threshold if b.cls == 0 else cfg.vru_threshold if F32(b.score) > F32(th) and -(xr - cfg.range_margin) <= b.x <= C.BEV_XF - cfg.range_margin: out.append(b) return out def _rect(b: Box3D): return ((float(b.x), float(b.y)), (float(b.l), float(b.w)), float(F32(b.yaw * 180.0 / math.pi))) def rotated_intersection(a: Box3D, b: Box3D) -> float: """Area of the intersection of two rotated BEV rectangles: ``cv::rotatedRectangleIntersection`` + ``convexHull`` + ``contourArea`` as ``render.cpp:283-307`` (OpenCV when installed, else exact convex clipping).""" try: import cv2 except ImportError: # pragma: no cover - the image and both dev envs have OpenCV cv2 = None if cv2 is not None: r, pts = cv2.rotatedRectangleIntersection(_rect(a), _rect(b)) if r == cv2.INTERSECT_NONE or pts is None or len(pts) < 3: return 0.0 return float(cv2.contourArea(cv2.convexHull(pts))) from ..ttaw.nms import bev_box_corners, clip_convex, polygon_area pa = bev_box_corners(np.array([a.x]), np.array([a.y]), np.array([a.l]), np.array([a.w]), np.array([a.yaw]))[0] pb = bev_box_corners(np.array([b.x]), np.array([b.y]), np.array([b.l]), np.array([b.w]), np.array([b.yaw]))[0] poly = clip_convex(pa, pb) return float(polygon_area(poly)) if len(poly) >= 3 else 0.0 def bev_nms(boxes: Sequence[Box3D], iou_threshold: float = 0.3, containment: float = 0.6) -> List[Box3D]: """``bevBoxNms`` (render.cpp:309-319): score order, greedy, same class only; a box is dropped if its rotated IoU with a kept box exceeds ``iou_threshold`` or intersection / smaller area exceeds ``containment``.""" order = sorted(boxes, key=lambda b: -b.score) kept: List[Box3D] = [] for d in order: ok = True for k in kept: if k.cls != d.cls: continue inter = rotated_intersection(d, k) if inter <= 0.0: continue ad, ak = float(d.l) * float(d.w), float(k.l) * float(k.w) if inter / max(ad + ak - inter, 1e-6) > iou_threshold or inter / max(min(ad, ak), 1e-6) > containment: ok = False break if ok: kept.append(d) return kept def stationary_healthy(stat: np.ndarray, cfg: PostConfig = PostConfig()) -> bool: """``stationaryHealthy`` (render.cpp:265-276): std >= 0.05 and max - min >= 0.2 over the finite values (double).""" s = np.asarray(stat, np.float64).ravel() s = s[np.isfinite(s)] if s.size == 0: return False mean = s.sum() / s.size var = (s * s).sum() / s.size - mean * mean return bool(math.sqrt(max(var, 0.0)) >= cfg.stationary_health_std and (s.max() - s.min()) >= cfg.stationary_health_range) def _cell(b: Box3D) -> Tuple[int, int]: return int((C.BEV_XF - b.x) / C.DET_RES), int((C.BEV_YH - b.y) / C.DET_RES) def annotate_boxes(boxes: Sequence[Box3D], stationary: Optional[np.ndarray], traj: Optional[np.ndarray], cfg: PostConfig = PostConfig()) -> bool: """Stationary flag and agent future of every box, in place; returns whether the stationary head is healthy.""" stat = None if stationary is None else np.asarray(stationary, F32).reshape(C.DET_H, C.DET_W) tr = None if traj is None else np.asarray(traj, F32).reshape(C.TRAJ_C, C.DET_H, C.DET_W) ok = stat is not None and stationary_healthy(stat, cfg) for b in boxes: r0, c0 = _cell(b) inside = 0 <= r0 < C.DET_H and 0 <= c0 < C.DET_W fut = None if tr is not None and inside: v = tr[:, r0, c0] kb = int(np.argmax(v[36:39])) # first max (strict > scan) fut = v[kb * 12:kb * 12 + 12].reshape(6, 2) b.future_mode = kb b.future = fut.astype(np.float64) + np.array([b.x, b.y]) if ok and inside: b.stationary = bool(stat[r0, c0] > F32(cfg.stationary_logit_threshold)) elif fut is not None: b.stationary = bool(math.hypot(float(fut[5, 0]), float(fut[5, 1])) < cfg.stationary_fallback_m) else: b.stationary = False return ok def decode_boxes_2d(outputs: Dict[str, np.ndarray], cfg: PostConfig = PostConfig(), *, keep_hidden: bool = False) -> List[List[Box2D]]: """``decode_boxes2d_ms`` (decode.cpp:59-108) over the three strides -> 8 lists of boxes (pixels at 768x432); hidden classes (``det2d_hide``) are removed unless ``keep_hidden``.""" res: List[List[Box2D]] = [[] for _ in range(C.N_CAMS)] for si, s in enumerate(C.DET2D_STRIDES): hm = np.asarray(outputs[f"hm2d_s{si}"], F32).reshape(C.N_CAMS, len(C.DET2D_CLASSES), -1) h, w = C.FEAT_H * 4 // s, C.FEAT_W * 4 // s hm = hm.reshape(C.N_CAMS, -1, h, w) reg = np.asarray(outputs[f"reg2d_s{si}"], F32).reshape(C.N_CAMS, 4, h, w) sf = F32(s) if host_post_mode() != "stock": fn = _candidates_sparse if host_post_mode() == "sparse" else _candidates_nd cand = fn(hm, cfg.det2d_threshold, cfg.det2d_topk, clip=cfg.det2d_clip) else: cand = [_candidates(hm[n], cfg.det2d_threshold, cfg.det2d_topk, clip=cfg.det2d_clip) for n in range(C.N_CAMS)] for n in range(C.N_CAMS): for sc, c, r, q in cand[n]: if c in cfg.det2d_hide and not keep_hidden: continue R = reg[n, :, r, q] res[n].append(Box2D(c, sc, float((F32(q) + R[1]) * sf), float((F32(r) + R[0]) * sf), float(np.exp(R[2]) * sf), float(np.exp(R[3]) * sf), s)) return res def ground_point(u: float, v: float, Kc: np.ndarray, T: np.ndarray, ground_z: float = 0.0) -> Optional[Tuple[float, float]]: """``groundPoint`` (render.cpp:321-336): where the ray through pixel (u, v) meets the plane z = ground_z (ego frame); ``T`` = T_cam_ego (ego -> camera). None if the ray points up or behind.""" Kc = np.asarray(Kc, np.float64).reshape(3, 3) T = np.asarray(T, np.float64).reshape(4, 4) dc = np.array([(u - Kc[0, 2]) / Kc[0, 0], (v - Kc[1, 2]) / Kc[1, 1], 1.0]) R, t = T[:3, :3], T[:3, 3] c = -(R.T @ t) d = R.T @ dc if d[2] >= -1e-6: return None lam = (ground_z - c[2]) / d[2] if lam <= 0: return None return float(c[0] + lam * d[0]), float(c[1] + lam * d[1]) def unknown_obstacles(boxes2d: Sequence[Sequence[Box2D]], depth_mean: Optional[np.ndarray], K: np.ndarray, T_cam_ego: np.ndarray, cfg: PostConfig = PostConfig()) -> List[Dict[str, float]]: """2D 'obs' boxes lifted to the BEV (``render.cpp:657-729``) -> [{x, y, score, range}] after de-duplication.""" K = np.asarray(K, np.float64).reshape(C.N_CAMS, 3, 3) T = np.asarray(T_cam_ego, np.float64).reshape(C.N_CAMS, 4, 4) dm = None if depth_mean is None else np.asarray(depth_mean, np.float64).reshape(C.N_CAMS, -1, C.FEAT_W // 2) cand = [] for i in range(min(C.N_CAMS, len(boxes2d))): for b in boxes2d[i]: if b.cls != 0 or b.score < cfg.th_unk: continue gp = ground_point(b.cx, b.cy + 0.5 * b.h, K[i], T[i], cfg.ground_z) lo, hi = cfg.unk2d_ground_range if gp is not None and lo < math.hypot(*gp) < hi: x, y, d = gp[0], gp[1], math.hypot(*gp) else: if dm is None: continue dmh, dmw = dm.shape[1], dm.shape[2] u = min(max(int(b.cx / C.IMG_W * dmw), 0), dmw - 1) v = min(max(int((b.cy + 0.35 * b.h) / C.IMG_H * dmh), 0), dmh - 1) win = np.sort(dm[i, max(0, v - 2):min(dmh, v + 3), max(0, u - 2):min(dmw, u + 3)].ravel()) d = float(win[win.size // 2]) lo, hi = cfg.unk2d_depth_range if not (lo < d < hi): continue pc = np.array([(b.cx - K[i, 0, 2]) / K[i, 0, 0] * d, (b.cy - K[i, 1, 2]) / K[i, 1, 1] * d, d]) pe = T[i, :3, :3].T @ (pc - T[i, :3, 3]) x, y = float(pe[0]), float(pe[1]) if abs(x) > cfg.unk2d_max_x or abs(y) > cfg.unk2d_max_y: continue cand.append({"x": x, "y": y, "score": float(b.score), "range": float(d), "camera": C.CAMERAS[i]}) cand.sort(key=lambda p: -p["score"]) kept: List[Dict[str, float]] = [] for p in cand: if all((p["x"] - k["x"]) ** 2 + (p["y"] - k["y"]) ** 2 > cfg.unk2d_dedupe_m ** 2 for k in kept): kept.append(p) return kept def select_mode(ego: np.ndarray, prev_mode: Optional[int] = None, cfg: PostConfig = PostConfig()) -> Tuple[int, np.ndarray]: """``render.cpp:432-445``: float32 mode logits + hysteresis on the previous mode, first argmax, straight preference. Returns (mode, the adjusted logits).""" lg = np.asarray(ego, F32).reshape(-1)[36:39].copy() if prev_mode is not None and 0 <= prev_mode < C.EGO_MODES: lg[prev_mode] = lg[prev_mode] + F32(cfg.mode_hysteresis) k = 0 for i in range(1, C.EGO_MODES): if lg[i] > lg[k]: k = i if k != 0 and (lg[k] - lg[0]) < F32(cfg.straight_margin): k = 0 return k, lg def plan_from_ego(ego: np.ndarray, prev_mode: Optional[int] = None, cfg: PostConfig = PostConfig()) -> Dict[str, Any]: """The plan of ``render.cpp:432-452``: selected mode and path, the three paths, confidences (softmax of the adjusted logits, double), controls (steer rad, accel m/s^2, brake logit and probability).""" e = np.asarray(ego, F32).reshape(-1) k, lg = select_mode(e, prev_mode, cfg) mx = float(lg.max()) pr = np.exp(lg.astype(np.float64) - mx) pr /= pr.sum() paths = e[:36].reshape(C.EGO_MODES, C.EGO_STEPS, 2) brake = float(e[41]) return {"mode": int(k), "path": paths[k].astype(np.float64), "paths": paths.astype(np.float64), "mode_logits": e[36:39].astype(np.float64), "mode_probs": pr, "steer": float(e[39]), "accel": float(e[40]), "brake_logit": brake, "brake_prob": 1.0 / (1.0 + math.exp(-brake)), "dt": C.EGO_DT} def thin_road_edge(lane: np.ndarray) -> np.ndarray: """``thin_road_edge`` (render.cpp:252-262): keep only road-edge (6) pixels next to drivable (1, 3, 4, 5) pixels that are not drivable themselves; every other edge pixel becomes background. Returns a new map.""" lane = np.asarray(lane, np.uint8) edge = lane == 6 if not edge.any(): return lane.copy() drv = np.isin(lane, (1, 3, 4, 5)) p = np.pad(drv, 1) h, w = lane.shape dil = np.zeros_like(drv) for dr in range(3): for dc in range(3): dil |= p[dr:dr + h, dc:dc + w] out = lane.copy() out[edge] = 0 out[edge & dil & ~drv] = 6 return out def metric_depth(bins: np.ndarray, kind: str = "linear") -> np.ndarray: """Depth in metres of a bin index map: ``linear`` 1 + 1.25 b (the trained bins, SPEC 10 risk 2) or ``log`` (the exported depth_mean centres).""" return C.depth_bin_centres(kind)[np.asarray(bins, np.int64)] def tl_state(tl: np.ndarray) -> Dict[str, Any]: t = np.asarray(tl, np.float64).reshape(-1) p = np.exp(t - t.max()) p /= p.sum() k = int(np.argmax(t)) return {"state": C.TL_CLASSES[k], "state_id": k, "probs": p} def occupancy_classes(occ: np.ndarray) -> np.ndarray: """[1, 10, 16, 200, 200] logits -> uint8 [16, 200, 200] class per voxel (first max).""" return np.argmax(np.asarray(occ).reshape(len(C.OCC_CLASSES), C.OCC_Z_BINS, 200, 200), axis=0).astype(np.uint8) @dataclass class FrameDecode: """The stateless decode of one frame's outputs.""" boxes3d: List[Box3D] stationary_healthy: bool boxes2d: List[List[Box2D]] unknown: List[Dict[str, float]] plan: Dict[str, Any] traffic_light: Dict[str, Any] lane: np.ndarray extras: Dict[str, Any] = field(default_factory=dict) def decode_frame(outputs: Dict[str, np.ndarray], K: Optional[np.ndarray] = None, T_cam_ego: Optional[np.ndarray] = None, cfg: PostConfig = PostConfig(), *, prev_mode: Optional[int] = None) -> FrameDecode: """Every stateless decode of ``render.cpp`` on the 19 outputs (numpy, ONNX shapes). Seg fusion, yaw smoothing and the mode history are ``host.temporal``'s.""" boxes = decode_boxes_3d(outputs["hm"], outputs["reg"], thresh=cfg.det3d_threshold, topk=cfg.det3d_topk) boxes = bev_nms(filter_boxes_3d(boxes, cfg), cfg.bev_nms_iou, cfg.bev_nms_containment) healthy = annotate_boxes(boxes, outputs.get("stationary"), outputs.get("traj"), cfg) b2d = decode_boxes_2d(outputs, cfg) unk: List[Dict[str, float]] = [] if cfg.unk2d and K is not None and T_cam_ego is not None: unk = unknown_obstacles(b2d, outputs.get("depth_mean"), K, T_cam_ego, cfg) lane = np.asarray(outputs["lane"], np.uint8).reshape(C.BEV_H, C.BEV_W) return FrameDecode(boxes, healthy, b2d, unk, plan_from_ego(outputs["ego"], prev_mode, cfg), tl_state(outputs["tl"]), lane)