| |
| """Host post-processing of TransFusion-L with Autoware's semantics (research/transfusion/SPEC.md section 5): the three |
| network outputs (``cls_score0`` (1, 5, K), ``bbox_pred0`` (1, 8, K), ``dir_cls_pred0`` (1, 2, K): from the device |
| readback or the CPU reference) -> Autoware ``DetectedObjects``-equivalent detections. |
| |
| Order of operations (``AW/lib/postprocess/postprocess_kernel.cu:51-143``, ``circle_nms_kernel.cu``, |
| ``AW/src/lidar_transfusion_node.cpp:163-178``): |
| |
| 1. :func:`decode_transfusion` (``generateBoxes3D_kernel``, float32): ``label`` = first class with the largest score |
| (strict ``>`` scan), ``score`` = that score (no sigmoid: it is in the graph); ``score = 0`` unless |
| ``sqrt(sin^2 + cos^2) >= yaw_norm_thresholds[label]``; ``x = bbox[0] * 4 * 0.24 + (-92.16)`` (the multiplier is |
| ``num_point_values = 4``), ``y`` likewise, ``z = bbox[2]`` (box centre), ``length / width / height = |
| exp(bbox[3:6])``, ``yaw = atan2(sin, cos)``; the velocity rows are ignored; |
| 2. keep ``score > score_threshold`` (strict), sort by descending score (``thrust::sort``, not stable on the GPU: here |
| stable, ties keep proposal order) and circle NMS 0.5 m, class-agnostic (``ttaw.nms.circle_nms``); |
| 3. ``box3DToDetectedObject`` (``AW/lib/ros_utils.cpp:29-61``): ``getSemanticType`` labels (CAR=1, TRUCK=2, BUS=3, |
| BICYCLE=6, PEDESTRIAN=7), ``existence_probability = score``, the **network yaw unchanged** (unlike CenterPoint's |
| ``-yaw - pi/2``), dimensions (length, width, height), ``orientation_availability`` SIGN_UNKNOWN for car-like labels; |
| 4. IoU-BEV NMS (``perception_utils::IouBevNms::apply(objects)``, ``sort=false``: every earlier object suppresses, |
| pedestrian-vs-other pairs skipped, 10 m gate, IoU > 0.1; ``ttaw.nms.iou_bev_nms``); |
| 5. the area class remapper (``AW/lib/detection_class_remapper.cpp:54-72``: CAR -> TRUCK for 12.1-36 m^2, CAR / |
| TRUCK / BUS -> TRAILER for 36-999.999 m^2; ``ttaw.nms.ClassRemapper``). |
| |
| :data:`RUNTIME_PARAMS` are the per-request knobs (RT-host class, PLAN.md 0.4); their defaults are Autoware's |
| (``transfusion.param.yaml``), including the node's sanitization (``score_threshold <= 0`` -> 0.2, |
| ``circle_nms_dist_threshold <= 0`` -> 0.5). The yaw-norm thresholds stay the yaml's (one per class). |
| """ |
| from __future__ import annotations |
|
|
| import math |
| from typing import Any, Dict, Mapping, Optional, Sequence, Tuple |
|
|
| import numpy as np |
|
|
| from ..reference.config import NODE_PARAMS, NUM_POINT_VALUES, TransFusionConfig |
| from ..ttaw.decode import (AUTOWARE_LABELS, LABEL_IDS, SIGN_UNKNOWN, UNAVAILABLE, DecodedBoxes, DetectedObjects, |
| is_car_like, sort_by_score) |
| from ..ttaw.nms import circle_nms, iou_bev_nms |
| from ..ttaw.outputs import Detections3D |
|
|
| __all__ = ["RUNTIME_PARAMS", "LABELS", "TransFusionPostprocessor", "decode_transfusion", "semantic_label", |
| "to_detected_objects", "OUTPUT_NAMES"] |
|
|
| LABELS = AUTOWARE_LABELS |
| OUTPUT_NAMES = ("cls_score0", "bbox_pred0", "dir_cls_pred0") |
|
|
| |
| RUNTIME_PARAMS: Dict[str, Tuple[type, Any, Any, Any]] = { |
| |
| "score_threshold": (float, 0.0, 1.0, NODE_PARAMS["score_threshold"]), |
| |
| "circle_nms_dist_threshold": (float, 0.0, 10.0, NODE_PARAMS["circle_nms_dist_threshold"]), |
| "iou_nms_threshold": (float, 0.0, 1.0, NODE_PARAMS["iou_nms_threshold"]), |
| "iou_nms_search_distance_2d": (float, 0.0, 200.0, NODE_PARAMS["iou_nms_search_distance_2d"]), |
| "remap_classes": (bool, None, None, True), |
| |
| "max_detections": (int, 1, None, None), |
| } |
|
|
| |
| _SEMANTIC = {"CAR": "CAR", "TRUCK": "TRUCK", "BUS": "BUS", "TRAILER": "TRAILER", "MOTORCYCLE": "MOTORCYCLE", |
| "BICYCLE": "BICYCLE", "PEDESTRIAN": "PEDESTRIAN"} |
|
|
|
|
| def semantic_label(class_name: str) -> int: |
| """``getSemanticType``: class name -> ObjectClassification label; anything else -> UNKNOWN.""" |
| return LABEL_IDS[_SEMANTIC.get(class_name, "UNKNOWN")] |
|
|
|
|
| def _output(raw: Mapping[str, Any], name: str, rows: int) -> np.ndarray: |
| a = np.asarray(raw[name], dtype=np.float32) |
| if a.ndim == 3: |
| if a.shape[0] != 1: |
| raise ValueError(f"{name}: batch {a.shape[0]} != 1") |
| a = a[0] |
| if a.ndim != 2 or a.shape[0] < rows: |
| raise ValueError(f"{name}: expected ({rows}, K), got {a.shape}") |
| return a |
|
|
|
|
| def decode_transfusion(raw: Mapping[str, Any], cfg: TransFusionConfig) -> DecodedBoxes: |
| """``generateBoxes3D_kernel`` for every proposal (no threshold yet) -> ``ttaw.decode.DecodedBoxes`` in proposal |
| order (``cell`` holds the proposal index; ``vel_x`` / ``vel_y`` the unused velocity rows).""" |
| cls = _output(raw, "cls_score0", len(cfg.class_names))[: len(cfg.class_names)] |
| box = _output(raw, "bbox_pred0", 6) |
| rot = _output(raw, "dir_cls_pred0", 2) |
| K = cls.shape[1] |
| if box.shape[1] != K or rot.shape[1] != K: |
| raise ValueError(f"proposal counts differ: {cls.shape} / {box.shape} / {rot.shape}") |
| nan = np.isnan(cls) |
| scan = np.where(nan, np.float32(-np.inf), cls) if nan.any() else cls |
| label = np.argmax(scan, axis=0).astype(np.int32) |
| score = np.take_along_axis(cls, label[None].astype(np.int64), 0)[0].astype(np.float32) |
| yaw_norm = np.sqrt(rot[0] * rot[0] + rot[1] * rot[1]).astype(np.float32) |
| score = np.where(yaw_norm >= cfg.yaw_norm_thresholds[label], score, np.float32(0.0)).astype(np.float32) |
| f32 = np.float32 |
| k = f32(NUM_POINT_VALUES) |
| x = ((box[0] * k) * f32(cfg.voxel_size[0]) + f32(cfg.point_cloud_range[0])).astype(np.float32) |
| y = ((box[1] * k) * f32(cfg.voxel_size[1]) + f32(cfg.point_cloud_range[1])).astype(np.float32) |
| vel = box[6:8] if box.shape[0] >= 8 else np.zeros((2, K), np.float32) |
| with np.errstate(over="ignore"): |
| return DecodedBoxes(cell=np.arange(K, dtype=np.int64), label=label, score=score, x=x, y=y, |
| z=box[2].astype(np.float32), length=np.exp(box[3]).astype(np.float32), |
| width=np.exp(box[4]).astype(np.float32), height=np.exp(box[5]).astype(np.float32), |
| yaw=np.arctan2(rot[0], rot[1]).astype(np.float32), vel_x=vel[0].copy(), |
| vel_y=vel[1].copy()) |
|
|
|
|
| def to_detected_objects(boxes: DecodedBoxes, class_names: Sequence[str]) -> DetectedObjects: |
| """``box3DToDetectedObject`` of lidar_transfusion for every row: no yaw conversion, no twist.""" |
| table = np.array([semantic_label(n) for n in class_names], dtype=np.uint8) |
| lab = np.asarray(boxes.label, dtype=np.int64) |
| valid = (lab >= 0) & (lab < len(class_names)) |
| label = np.where(valid, table[np.clip(lab, 0, len(class_names) - 1)], LABEL_IDS["UNKNOWN"]).astype(np.uint8) |
| orient = np.where(is_car_like(label), SIGN_UNKNOWN, UNAVAILABLE).astype(np.uint8) |
| return DetectedObjects(label=label, existence_probability=np.asarray(boxes.score, np.float32), |
| x=boxes.x.copy(), y=boxes.y.copy(), z=boxes.z.copy(), yaw=boxes.yaw.copy(), |
| length=boxes.length.copy(), width=boxes.width.copy(), height=boxes.height.copy(), |
| orientation_availability=orient, source_index=np.arange(len(boxes), dtype=np.int64), |
| label_before_remap=label.copy(), extra={"proposal": np.asarray(boxes.cell, np.int64)}) |
|
|
|
|
| class TransFusionPostprocessor: |
| """Stateless; ``params`` are validated ``RUNTIME_PARAMS`` values (missing keys take the defaults).""" |
|
|
| def __init__(self, config: TransFusionConfig, *, nms: str = "numpy"): |
| if nms not in ("numpy", "numba"): |
| raise ValueError("nms must be 'numpy' or 'numba'") |
| self.config = config |
| self.remapper = config.remapper |
| |
| |
| self.nms = nms |
|
|
| def objects(self, raw: Mapping[str, Any], params: Optional[Mapping[str, Any]] = None |
| ) -> Tuple[DetectedObjects, Dict[str, int]]: |
| p = {k: v[3] for k, v in RUNTIME_PARAMS.items()} |
| p.update(params or {}) |
| cfg = self.config |
| decoded = decode_transfusion(raw, cfg) |
| thr = np.float32(cfg.sanitized_score_threshold(p["score_threshold"])) |
| boxes = sort_by_score(decoded.take(np.nonzero(decoded.score > thr)[0])) |
| cnms, inms = circle_nms, iou_bev_nms |
| if self.nms == "numba": |
| from . import fast |
|
|
| cnms, inms = fast.circle_nms, fast.iou_bev_nms |
| keep = cnms(boxes.x, boxes.y, cfg.sanitized_circle_nms(p["circle_nms_dist_threshold"])) |
| objs = to_detected_objects(boxes.take(keep), cfg.class_names) |
| n_circle = len(objs) |
| search, iou = float(p["iou_nms_search_distance_2d"]), float(p["iou_nms_threshold"]) |
| if not (math.isfinite(search) and search >= 0.0 and math.isfinite(iou) and 0.0 <= iou <= 1.0): |
| raise ValueError("IouBevNms parameters out of range") |
| objs = objs.take(inms(objs.x, objs.y, objs.length, objs.width, objs.yaw, objs.label, |
| search_distance_2d=search, iou_threshold=iou)) |
| if p["remap_classes"]: |
| objs.label = self.remapper.apply(objs.label, objs.length, objs.width) |
| counts = {"proposals": int(len(decoded)), "after_threshold": int(len(boxes)), "after_circle_nms": n_circle, |
| "after_iou_nms": int(len(objs))} |
| if p["max_detections"] is not None and len(objs) > int(p["max_detections"]): |
| objs = objs.take(np.arange(int(p["max_detections"]))) |
| return objs, counts |
|
|
| def to_output(self, objs: DetectedObjects, *, model: str, frame_id: str = "base_link", |
| meta: Optional[Dict[str, Any]] = None) -> Detections3D: |
| """``ttaw.outputs.Detections3D`` (the ``/predict`` body): boxes (x, y, z, length, width, height, yaw), |
| ``existence_probability`` as the score, ObjectClassification label ids; ``meta`` gets the per-object |
| orientation availability, the pre-remap labels and the proposal index (aligned with the score-sorted |
| detections).""" |
| order = np.argsort(-objs.existence_probability, kind="stable") |
| objs = objs.take(order) |
| m = dict(meta or {}) |
| m["orientation_availability"] = objs.orientation_availability.tolist() |
| m["label_before_remap"] = [LABELS[int(v)] for v in objs.label_before_remap] |
| m["proposal"] = objs.extra["proposal"].tolist() |
| return Detections3D(objs.boxes_xyzlwh_yaw(), objs.existence_probability, objs.label.astype(np.int32), |
| labels=LABELS, model=model, frame_id=frame_id, meta=m) |
|
|
| def empty(self, *, model: str, frame_id: str = "base_link", meta: Optional[Dict[str, Any]] = None) -> Detections3D: |
| """The output of a frame Autoware skips (``autoware_compat``): no objects.""" |
| m = dict(meta or {}) |
| m.update({"orientation_availability": [], "label_before_remap": [], "proposal": []}) |
| return Detections3D(np.zeros((0, 7), np.float32), np.zeros(0, np.float32), np.zeros(0, np.int32), |
| labels=LABELS, model=model, frame_id=frame_id, meta=m) |
|
|
| def __call__(self, raw: Mapping[str, Any], params: Optional[Mapping[str, Any]] = None, *, model: str, |
| frame_id: str = "base_link", meta: Optional[Dict[str, Any]] = None) -> Detections3D: |
| objs, counts = self.objects(raw, params) |
| return self.to_output(objs, model=model, frame_id=frame_id, meta={**(meta or {}), "counts": counts}) |
|
|
