feat(perception): add ground-aware cuboid refusion

2026-07-23 08:24:37 +03:00
parent b53d6d5a45
commit a0706fd5d8
8 changed files with 1116 additions and 114 deletions
@@ -13,7 +13,7 @@ import math
 import statistics
 from collections import defaultdict, deque
 from collections.abc import Mapping, Sequence
-from dataclasses import dataclass
+from dataclasses import dataclass, replace
 from pathlib import Path
 from typing import Any

@@ -97,6 +97,9 @@ class TrackFusion:
    status: str
    cuboid: Cuboid | None
    source_indices: npt.NDArray[np.int64]
+    pre_ground_clustered_points: int = 0
+    ground_rejected_points: int = 0
+    support_ground_z_map: float | None = None
    geometry: str = "none"
    observed_cuboid: Cuboid | None = None
    completion_fraction: float | None = None
@@ -466,6 +469,157 @@ def _completion_profile(profile: Mapping[str, Any]) -> dict[str, Any]:
    return dict(profile)


+def _object_support_ground_filter_profile(
+    profile: Mapping[str, Any],
+) -> dict[str, Any]:
+    groups = {"person", "bicycle", "motorcycle", "vehicle"}
+    minimum_height = profile.get("minimum_height_above_ground_m")
+    maximum_height = profile.get("maximum_height_above_ground_m")
+    if (
+        profile.get("mode") != "local-ground-relative-object-support-v1"
+        or not isinstance(minimum_height, Mapping)
+        or set(minimum_height) != groups
+        or not isinstance(maximum_height, Mapping)
+        or set(maximum_height) != groups
+    ):
+        raise RuntimeError("object-support ground filter profile is invalid")
+    scalar_checks = (
+        ("local_radius_m", 0.5, 10.0),
+        ("lower_percentile", 0.0, 30.0),
+        ("maximum_below_support_m", 0.1, 3.0),
+        ("maximum_above_support_m", 0.0, 0.5),
+        ("fallback_below_support_m", 0.0, 1.0),
+    )
+    for name, lower, upper in scalar_checks:
+        try:
+            value = float(profile[name])
+        except (KeyError, TypeError, ValueError) as exc:
+            raise RuntimeError(f"object-support ground filter field {name} is invalid") from exc
+        if not lower <= value <= upper:
+            raise RuntimeError(f"object-support ground filter field {name} is invalid")
+    for group in groups:
+        try:
+            lower = float(minimum_height[group])
+            upper = float(maximum_height[group])
+        except (KeyError, TypeError, ValueError) as exc:
+            raise RuntimeError(
+                f"object-support ground filter height for {group} is invalid"
+            ) from exc
+        if not 0.0 <= lower < upper <= 6.0:
+            raise RuntimeError(f"object-support ground filter height for {group} is invalid")
+    return dict(profile)
+
+
+def _local_ground_z(
+    *,
+    all_points_map: FloatArray,
+    center_xy: FloatArray,
+    support_lower_z: float,
+    profile: Mapping[str, Any],
+) -> float:
+    radius = float(profile["local_radius_m"])
+    if all_points_map.size:
+        distances = np.linalg.norm(all_points_map[:, :2] - center_xy, axis=1)
+        candidates = all_points_map[distances <= radius, 2]
+    else:
+        candidates = np.empty(0, dtype=np.float64)
+    estimate = (
+        float(np.percentile(candidates, float(profile["lower_percentile"])))
+        if candidates.size
+        else math.nan
+    )
+    if (
+        not math.isfinite(estimate)
+        or estimate < support_lower_z - float(profile["maximum_below_support_m"])
+        or estimate > support_lower_z + float(profile["maximum_above_support_m"])
+    ):
+        estimate = support_lower_z - float(profile["fallback_below_support_m"])
+    return estimate
+
+
+def _filter_object_support_by_ground(
+    indices: npt.NDArray[np.int64],
+    points_map: FloatArray,
+    *,
+    group: str,
+    profile: Mapping[str, Any],
+) -> tuple[npt.NDArray[np.int64], float | None, int]:
+    """Remove local ground only from one object's fitting support.
+
+    `points_map` is never modified or reduced. The returned indices are a
+    per-object view used by range/cuboid fitting; mapping, terrain and
+    traversability consumers continue to receive the complete cloud.
+    """
+
+    if indices.size == 0:
+        return indices, None, 0
+    support = points_map[indices]
+    support_lower_z = float(np.percentile(support[:, 2], 5.0))
+    ground_z = _local_ground_z(
+        all_points_map=points_map,
+        center_xy=np.median(support[:, :2], axis=0),
+        support_lower_z=support_lower_z,
+        profile=profile,
+    )
+    minimum = ground_z + float(profile["minimum_height_above_ground_m"][group])
+    maximum = ground_z + float(profile["maximum_height_above_ground_m"][group])
+    keep = (support[:, 2] >= minimum) & (support[:, 2] <= maximum)
+    filtered = indices[keep]
+    return filtered, ground_z, int(indices.size - filtered.size)
+
+
+def _support_overlap_fraction(
+    one: npt.NDArray[np.int64],
+    two: npt.NDArray[np.int64],
+) -> float:
+    if one.size == 0 or two.size == 0:
+        return 0.0
+    shared = np.intersect1d(one, two, assume_unique=False).size
+    return float(shared / min(one.size, two.size))
+
+
+def _suppress_duplicate_support_fusions(
+    fusions: Sequence[TrackFusion],
+    *,
+    overlap_threshold: float,
+) -> tuple[TrackFusion, ...]:
+    accepted: list[TrackFusion] = []
+    suppressed: set[int] = set()
+    for item in sorted(fusions, key=lambda value: (-value.score, value.track_id)):
+        if item.cuboid is None:
+            continue
+        if any(
+            other.association_group == item.association_group
+            and _support_overlap_fraction(other.source_indices, item.source_indices)
+            >= overlap_threshold
+            for other in accepted
+        ):
+            suppressed.add(item.track_id)
+        else:
+            accepted.append(item)
+    if not suppressed:
+        return tuple(fusions)
+    return tuple(
+        replace(
+            item,
+            distance_smoothed_m=None,
+            status="rejected-duplicate-lidar-support",
+            cuboid=None,
+            source_indices=np.empty(0, dtype=np.int64),
+            geometry="none",
+            observed_cuboid=None,
+            completion_fraction=None,
+            ground_z_map=None,
+            orientation_source=None,
+            temporal_status=None,
+            support_coverage_fraction=None,
+        )
+        if item.track_id in suppressed
+        else item
+        for item in fusions
+    )
+
+
 class CuboidCompletionTracker:
    """Complete visible LiDAR surfaces into provenance-marked, smoothed cuboids.

@@ -667,24 +821,12 @@ class CuboidCompletionTracker:
        center_xy: FloatArray,
        support_lower_z: float,
    ) -> float:
-        ground = self.profile["ground"]
-        radius = float(ground["local_radius_m"])
-        if all_points_map.size:
-            distances = np.linalg.norm(all_points_map[:, :2] - center_xy, axis=1)
-            candidates = all_points_map[distances <= radius, 2]
-        else:
-            candidates = np.empty(0, dtype=np.float64)
-        if candidates.size:
-            estimate = float(np.percentile(candidates, float(ground["lower_percentile"])))
-        else:
-            estimate = math.nan
-        if (
-            not math.isfinite(estimate)
-            or estimate < support_lower_z - float(ground["maximum_below_support_m"])
-            or estimate > support_lower_z + float(ground["maximum_above_support_m"])
-        ):
-            estimate = support_lower_z - float(ground["fallback_below_support_m"])
-        return estimate
+        return _local_ground_z(
+            all_points_map=all_points_map,
+            center_xy=center_xy,
+            support_lower_z=support_lower_z,
+            profile=self.profile["ground"],
+        )

    @staticmethod
    def _amodal_axis_center(lower: float, upper: float, size: float, sensor: float) -> float:
@@ -732,6 +874,17 @@ def fuse_tracks(
    session_seconds: float | None = None,
 ) -> tuple[TrackFusion, ...]:
    vehicle_labels = set(str(value) for value in association["vehicle_labels"])
+    ground_filter = association.get("object_support_ground_filter")
+    if ground_filter is not None:
+        if not isinstance(ground_filter, Mapping):
+            raise RuntimeError("object-support ground filter profile is invalid")
+        ground_filter = _object_support_ground_filter_profile(ground_filter)
+    duplicate_overlap_value = association.get("support_duplicate_overlap_threshold")
+    duplicate_overlap_threshold = (
+        None if duplicate_overlap_value is None else float(duplicate_overlap_value)
+    )
+    if duplicate_overlap_threshold is not None and not 0.0 < duplicate_overlap_threshold <= 1.0:
+        raise RuntimeError("support duplicate overlap threshold is invalid")
    accepted_tracks: list[dict[str, Any]] = []
    for track in sorted(tracks, key=lambda value: float(value["score"]), reverse=True):
        label = str(track["label"])
@@ -767,11 +920,21 @@ def fuse_tracks(
        allowed = np.asarray(association["semantic_ids"][group], dtype=np.uint8)
        compatible = candidates[np.isin(sampled_semantic[candidates], allowed)]
        clustered = _depth_cluster(compatible, depths, association)
-        selected = _spatial_cluster(
+        selected_before_ground = _spatial_cluster(
            source_indices[clustered],
            points_lidar,
            float(association["spatial_cluster_radius_m"][group]),
        )
+        selected = selected_before_ground
+        support_ground_z = None
+        ground_rejected_points = 0
+        if ground_filter is not None:
+            selected, support_ground_z, ground_rejected_points = _filter_object_support_by_ground(
+                selected_before_ground,
+                points_map,
+                group=group,
+                profile=ground_filter,
+            )
        ranges = np.linalg.norm(points_lidar[selected], axis=1)
        p10 = None if ranges.size == 0 else float(np.percentile(ranges, 10))
        median = None if ranges.size == 0 else float(np.median(ranges))
@@ -786,6 +949,12 @@ def fuse_tracks(
        support_coverage_fraction = None
        if compatible.size == 0:
            status = "rejected-no-semantic-lidar-support"
+        elif (
+            selected_before_ground.size >= minimum
+            and selected.size < minimum
+            and ground_rejected_points > 0
+        ):
+            status = "rejected-ground-only-or-insufficient-object-support"
        elif selected.size < minimum:
            status = f"rejected-fewer-than-{minimum}-clustered-points"
        else:
@@ -861,6 +1030,9 @@ def fuse_tracks(
                status=status,
                cuboid=cuboid,
                source_indices=selected,
+                pre_ground_clustered_points=int(selected_before_ground.size),
+                ground_rejected_points=ground_rejected_points,
+                support_ground_z_map=support_ground_z,
                geometry=geometry,
                observed_cuboid=observed_cuboid,
                completion_fraction=completion_fraction,
@@ -870,7 +1042,11 @@ def fuse_tracks(
                support_coverage_fraction=support_coverage_fraction,
            )
        )
-    return tuple(result)
+    if duplicate_overlap_threshold is None:
+        return tuple(result)
+    return _suppress_duplicate_support_fusions(
+        result, overlap_threshold=duplicate_overlap_threshold
+    )


 def fusion_document(item: TrackFusion) -> dict[str, Any]:
@@ -883,6 +1059,9 @@ def fusion_document(item: TrackFusion) -> dict[str, Any]:
        "candidate_projected_points": item.candidate_points,
        "semantic_compatible_points": item.semantic_points,
        "clustered_points": item.clustered_points,
+        "pre_ground_clustered_points": item.pre_ground_clustered_points,
+        "ground_rejected_points": item.ground_rejected_points,
+        "support_ground_z_map": item.support_ground_z_map,
        "distance_p10_m": item.distance_p10_m,
        "distance_median_m": item.distance_median_m,
        "distance_smoothed_m": item.distance_smoothed_m,