perf(perception): require exact critical-range persistence

src/k1link/perception/m48_low_step_occupancy.py

@@ -36,9 +36,7 @@ from .geometry import (
 from .geometry_math import POINT_OCCUPIED
 from .providers import SourcePacket
 
-M48_LOW_STEP_PROFILE_SCHEMA: Final = (
-    "missioncore.m48-additive-low-step-occupancy-profile/v1"
-)
+M48_LOW_STEP_PROFILE_SCHEMA: Final = "missioncore.m48-additive-low-step-occupancy-profile/v1"
 M48_LOW_STEP_PROVIDER_ID: Final = "ravnoves00-additive-low-step-geometry/v1"
 
 IntArray = npt.NDArray[np.int64]
@@ -56,6 +54,7 @@ class LowStepComponentProfile:
     sparse_persistence_minimum_points: int
     sparse_persistence_window_frames: int
     sparse_persistence_minimum_hits: int
+    sparse_persistence_maximum_range_m: float
     minimum_voxels: int
     local_radius_m: float
     maximum_candidate_points_per_frame: int
@@ -73,6 +72,8 @@ class LowStepComponentProfile:
             or not 2
             <= self.sparse_persistence_minimum_hits
             <= self.sparse_persistence_window_frames
+            or not math.isfinite(self.sparse_persistence_maximum_range_m)
+            or not 1.0 <= self.sparse_persistence_maximum_range_m <= self.local_radius_m
             or not 1 <= self.minimum_voxels <= 128
             or not math.isfinite(self.local_radius_m)
             or not 1.0 <= self.local_radius_m <= 100.0
@@ -165,9 +166,9 @@ class M48AdditiveLowStepGeometryProvider:
         self._peak_component_voxels = 0
         self._additive_core_duration_ns = 0
         self._sparse_lock = Lock()
-        self._sparse_history: deque[
-            tuple[int, tuple[frozenset[tuple[int, int, int]], ...]]
-        ] = deque()
+        self._sparse_history: deque[tuple[int, tuple[frozenset[tuple[int, int, int]], ...]]] = (
+            deque()
+        )
         self._last_sparse_sequence: int | None = None
 
     def associate(
@@ -188,9 +189,7 @@ class M48AdditiveLowStepGeometryProvider:
         except Exception:
             with self._lock:
                 self._failed_frames += 1
-                self._additive_core_duration_ns += max(
-                    0, time.perf_counter_ns() - started
-                )
+                self._additive_core_duration_ns += max(0, time.perf_counter_ns() - started)
             raise
         with self._lock:
             self._completed_frames += 1
@@ -198,18 +197,10 @@ class M48AdditiveLowStepGeometryProvider:
             self._candidate_points += candidate_points
             self._additive_observation_count += len(additive)
             self._additive_voxels += voxel_count
-            self._peak_candidate_points = max(
-                self._peak_candidate_points, candidate_points
-            )
-            self._peak_additive_observations = max(
-                self._peak_additive_observations, len(additive)
-            )
-            self._peak_component_voxels = max(
-                self._peak_component_voxels, peak_component_voxels
-            )
-            self._additive_core_duration_ns += max(
-                0, time.perf_counter_ns() - started
-            )
+            self._peak_candidate_points = max(self._peak_candidate_points, candidate_points)
+            self._peak_additive_observations = max(self._peak_additive_observations, len(additive))
+            self._peak_component_voxels = max(self._peak_component_voxels, peak_component_voxels)
+            self._additive_core_duration_ns += max(0, time.perf_counter_ns() - started)
         return tuple(result)
 
     def _build_additive_observations(
@@ -226,9 +217,9 @@ class M48AdditiveLowStepGeometryProvider:
         claimed = {
             point_id for observation in baseline for point_id in observation.source_point_ids
         }
-        candidate = np.flatnonzero(
-            (step > 0) & (frame.point_class != POINT_OCCUPIED)
-        ).astype(np.int64)
+        candidate = np.flatnonzero((step > 0) & (frame.point_class != POINT_OCCUPIED)).astype(
+            np.int64
+        )
         if claimed and candidate.size:
             candidate = candidate[
                 np.fromiter(
@@ -263,6 +254,7 @@ class M48AdditiveLowStepGeometryProvider:
             sequence=packet.envelope.sequence,
             components=components,
             points_map=frame.points_map,
+            sensor_position_map=frame.sensor_position_map,
         )
         qualified = (*strong, *persistent_sparse)
         qualified = tuple(
@@ -272,8 +264,7 @@ class M48AdditiveLowStepGeometryProvider:
                     float(
                         np.min(
                             np.linalg.norm(
-                                frame.points_map[item[0]]
-                                - frame.sensor_position_map,
+                                frame.points_map[item[0]] - frame.sensor_position_map,
                                 axis=1,
                             )
                         )
@@ -298,17 +289,11 @@ class M48AdditiveLowStepGeometryProvider:
             points = frame.points_map[indices]
             centroid = np.median(points, axis=0)
             covariance = points.var(axis=0)
-            nearest = float(
-                np.min(np.linalg.norm(points - frame.sensor_position_map, axis=1))
-            )
+            nearest = float(np.min(np.linalg.norm(points - frame.sensor_position_map, axis=1)))
             observations.append(
                 ObstacleObservation(
-                    observation_id=(
-                        f"{packet.envelope.frame_id}:low-step:{component_index}"
-                    ),
-                    occupancy_key=(
-                        f"{packet.envelope.frame_id}:low-step:{component_index}"
-                    ),
+                    observation_id=(f"{packet.envelope.frame_id}:low-step:{component_index}"),
+                    occupancy_key=(f"{packet.envelope.frame_id}:low-step:{component_index}"),
                     source_id=packet.envelope.source_id,
                     frame_id=packet.envelope.frame_id,
                     evidence_time_ns=packet.envelope.timestamps.source_ns,
@@ -348,6 +333,7 @@ class M48AdditiveLowStepGeometryProvider:
         sequence: int,
         components: tuple[tuple[IntArray, int], ...],
         points_map: npt.NDArray[np.float64],
+        sensor_position_map: npt.NDArray[np.float64],
     ) -> tuple[tuple[IntArray, int], ...]:
         """Promote only weak geometry repeated in a bounded causal window."""
 
@@ -366,13 +352,10 @@ class M48AdditiveLowStepGeometryProvider:
             and item[1] >= component_profile.minimum_voxels
         )
         with self._sparse_lock:
-            if (
-                self._last_sparse_sequence is not None
-                and (
-                    sequence <= self._last_sparse_sequence
-                    or sequence - self._last_sparse_sequence
-                    > component_profile.sparse_persistence_window_frames
-                )
+            if self._last_sparse_sequence is not None and (
+                sequence <= self._last_sparse_sequence
+                or sequence - self._last_sparse_sequence
+                > component_profile.sparse_persistence_window_frames
             ):
                 self._sparse_history.clear()
             first_allowed = sequence - component_profile.sparse_persistence_window_frames + 1
@@ -383,17 +366,23 @@ class M48AdditiveLowStepGeometryProvider:
                 if item[0].size >= component_profile.minimum_points:
                     continue
                 hit_count = 1 + sum(
-                    any(
-                        _cells_touch(cells, previous)
-                        for previous in previous_components
-                    )
+                    any(not cells.isdisjoint(previous) for previous in previous_components)
                     for _, previous_components in self._sparse_history
                 )
-                if hit_count >= component_profile.sparse_persistence_minimum_hits:
+                if (
+                    hit_count >= component_profile.sparse_persistence_minimum_hits
+                    and float(
+                        np.min(
+                            np.linalg.norm(
+                                points_map[item[0]] - sensor_position_map,
+                                axis=1,
+                            )
+                        )
+                    )
+                    <= component_profile.sparse_persistence_maximum_range_m
+                ):
                     promoted.append(item)
-            self._sparse_history.append(
-                (sequence, tuple(cells for _, cells in current))
-            )
+            self._sparse_history.append((sequence, tuple(cells for _, cells in current)))
             self._last_sparse_sequence = sequence
         return tuple(promoted)
 
@@ -409,9 +398,7 @@ class M48AdditiveLowStepGeometryProvider:
                 additive_observation_count=self._additive_observation_count,
                 additive_voxel_count=self._additive_voxels,
                 peak_candidate_points_per_frame=self._peak_candidate_points,
-                peak_additive_observations_per_frame=(
-                    self._peak_additive_observations
-                ),
+                peak_additive_observations_per_frame=(self._peak_additive_observations),
                 peak_voxels_per_component=self._peak_component_voxels,
                 additive_core_duration_ns=self._additive_core_duration_ns,
             )
@@ -478,6 +465,7 @@ def load_m48_low_step_occupancy_profile(
             "sparse_persistence_minimum_points",
             "sparse_persistence_window_frames",
             "sparse_persistence_minimum_hits",
+            "sparse_persistence_maximum_range_m",
             "minimum_voxels",
             "local_radius_m",
             "maximum_candidate_points_per_frame",
@@ -537,9 +525,7 @@ def load_m48_low_step_occupancy_profile(
         if _number(acceptance, key) < 0.0:
             raise M48LowStepOccupancyError("low-step acceptance bounds are invalid")
     if (
-        not _string(source, "m48r2_result_id").startswith(
-            "m48-static-occupancy-qualification-"
-        )
+        not _string(source, "m48r2_result_id").startswith("m48-static-occupancy-qualification-")
         or len(_string(source, "m48r2_result_id"))
         != len("m48-static-occupancy-qualification-") + 64
     ):
@@ -577,9 +563,7 @@ def load_m48_low_step_occupancy_profile(
             LowStepSeparationExpectation(
                 anchor_id=_string(item, "anchor_id"),
                 sequence=_positive_integer(item, "sequence"),
-                expected_minimum_components=_positive_integer(
-                    item, "expected_minimum_components"
-                ),
+                expected_minimum_components=_positive_integer(item, "expected_minimum_components"),
                 interpretation=_string(item, "interpretation"),
             )
         )
@@ -596,9 +580,7 @@ def load_m48_low_step_occupancy_profile(
         base_geometry_profile_sha256=_digest(base, "sha256"),
         component=LowStepComponentProfile(
             voxel_size_m=_number(component, "voxel_size_m"),
-            neighbor_radius_cells=_positive_integer(
-                component, "neighbor_radius_cells"
-            ),
+            neighbor_radius_cells=_positive_integer(component, "neighbor_radius_cells"),
             minimum_points=_positive_integer(component, "minimum_points"),
             sparse_persistence_minimum_points=_positive_integer(
                 component,
@@ -612,14 +594,16 @@ def load_m48_low_step_occupancy_profile(
                 component,
                 "sparse_persistence_minimum_hits",
             ),
+            sparse_persistence_maximum_range_m=_number(
+                component,
+                "sparse_persistence_maximum_range_m",
+            ),
             minimum_voxels=_positive_integer(component, "minimum_voxels"),
             local_radius_m=_number(component, "local_radius_m"),
             maximum_candidate_points_per_frame=_positive_integer(
                 component, "maximum_candidate_points_per_frame"
             ),
-            maximum_cells_per_component=_positive_integer(
-                component, "maximum_cells_per_component"
-            ),
+            maximum_cells_per_component=_positive_integer(component, "maximum_cells_per_component"),
             maximum_components_per_frame=_positive_integer(
                 component, "maximum_components_per_frame"
             ),
@@ -636,9 +620,7 @@ def _voxel_components(
 ) -> tuple[tuple[IntArray, int], ...]:
     if source_indices.size == 0:
         return ()
-    cells = np.floor(
-        points_map[source_indices] / profile.voxel_size_m
-    ).astype(np.int64)
+    cells = np.floor(points_map[source_indices] / profile.voxel_size_m).astype(np.int64)
     cell_points: dict[tuple[int, int, int], list[int]] = {}
     for local_index, row in enumerate(cells):
         key = (int(row[0]), int(row[1]), int(row[2]))
@@ -690,19 +672,6 @@ def _component_cells(
     return frozenset((int(row[0]), int(row[1]), int(row[2])) for row in rows)
 
 
-def _cells_touch(
-    current: frozenset[tuple[int, int, int]],
-    previous: frozenset[tuple[int, int, int]],
-) -> bool:
-    return any(
-        abs(left[0] - right[0]) <= 1
-        and abs(left[1] - right[1]) <= 1
-        and abs(left[2] - right[2]) <= 1
-        for left in current
-        for right in previous
-    )
-
-
 def _object(value: object, label: str) -> dict[str, object]:
     if not isinstance(value, dict) or not all(isinstance(key, str) for key in value):
         raise M48LowStepOccupancyError(f"{label} must be an object")