| |
| """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 |
| det3d_topk: int = 64 |
| vehicle_threshold: float = 0.35 |
| vru_threshold: float = 0.15 |
| range_margin: float = 2.0 |
| bev_nms_iou: float = 0.3 |
| bev_nms_containment: float = 0.6 |
| stationary_logit_threshold: float = 0.0 |
| stationary_health_std: float = 0.05 |
| stationary_health_range: float = 0.2 |
| stationary_fallback_m: float = 0.5 |
| det2d_threshold: float = 0.30 |
| det2d_topk: int = 48 |
| det2d_clip: float = 50.0 |
| det2d_hide: Tuple[int, ...] = (7,) |
| unk2d: bool = True |
| unk2d_threshold: Optional[float] = None |
| ground_z: float = 0.0 |
| 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 |
| straight_margin: float = 1.0 |
| thin_road_edge: bool = True |
| seg_fuse: bool = True |
| yaw_smoothing: bool = True |
| risk_gain: float = 1.0 |
|
|
| 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 |
|
|
| @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 |
|
|
|
|
| |
| |
| |
| |
| |
| |
| |
| |
| 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)) |
| 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: |
| 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])) |
| 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) |
|
|
