diff --git a/config/perception/m48r3-additive-low-step-occupancy-v1.json b/config/perception/m48r3-additive-low-step-occupancy-v1.json index 7a5f550..7c5fd92 100644 --- a/config/perception/m48r3-additive-low-step-occupancy-v1.json +++ b/config/perception/m48r3-additive-low-step-occupancy-v1.json @@ -20,6 +20,9 @@ "voxel_size_m": 0.45, "neighbor_radius_cells": 1, "minimum_points": 5, + "sparse_persistence_minimum_points": 2, + "sparse_persistence_window_frames": 6, + "sparse_persistence_minimum_hits": 3, "minimum_voxels": 1, "local_radius_m": 10.0, "maximum_candidate_points_per_frame": 768, diff --git a/src/k1link/perception/m48_low_step_occupancy.py b/src/k1link/perception/m48_low_step_occupancy.py index f3e2315..52c190b 100644 --- a/src/k1link/perception/m48_low_step_occupancy.py +++ b/src/k1link/perception/m48_low_step_occupancy.py @@ -53,6 +53,9 @@ class LowStepComponentProfile: voxel_size_m: float neighbor_radius_cells: int minimum_points: int + sparse_persistence_minimum_points: int + sparse_persistence_window_frames: int + sparse_persistence_minimum_hits: int minimum_voxels: int local_radius_m: float maximum_candidate_points_per_frame: int @@ -65,6 +68,11 @@ class LowStepComponentProfile: or not 0.05 <= self.voxel_size_m <= 2.0 or self.neighbor_radius_cells != 1 or not 1 <= self.minimum_points <= 256 + or not 1 <= self.sparse_persistence_minimum_points < self.minimum_points + or not 2 <= self.sparse_persistence_window_frames <= 32 + or not 2 + <= self.sparse_persistence_minimum_hits + <= self.sparse_persistence_window_frames 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 @@ -156,6 +164,11 @@ class M48AdditiveLowStepGeometryProvider: self._peak_additive_observations = 0 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._last_sparse_sequence: int | None = None def associate( self, @@ -240,12 +253,18 @@ class M48AdditiveLowStepGeometryProvider: candidate, self.profile.component, ) - qualified = tuple( + strong = tuple( item for item in components if item[0].size >= self.profile.component.minimum_points and item[1] >= self.profile.component.minimum_voxels ) + persistent_sparse = self._persistent_sparse_components( + sequence=packet.envelope.sequence, + components=components, + points_map=frame.points_map, + ) + qualified = (*strong, *persistent_sparse) qualified = tuple( sorted( qualified, @@ -323,6 +342,61 @@ class M48AdditiveLowStepGeometryProvider: peak_voxels = max(peak_voxels, cells) return tuple(observations), candidate_count, voxel_count, peak_voxels + def _persistent_sparse_components( + self, + *, + sequence: int, + components: tuple[tuple[IntArray, int], ...], + points_map: npt.NDArray[np.float64], + ) -> tuple[tuple[IntArray, int], ...]: + """Promote only weak geometry repeated in a bounded causal window.""" + + component_profile = self.profile.component + current = tuple( + ( + item, + _component_cells( + points_map, + item[0], + voxel_size_m=component_profile.voxel_size_m, + ), + ) + for item in components + if item[0].size >= component_profile.sparse_persistence_minimum_points + 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 + ) + ): + self._sparse_history.clear() + first_allowed = sequence - component_profile.sparse_persistence_window_frames + 1 + while self._sparse_history and self._sparse_history[0][0] < first_allowed: + self._sparse_history.popleft() + promoted: list[tuple[IntArray, int]] = [] + for item, cells in current: + if item[0].size >= component_profile.minimum_points: + continue + hit_count = 1 + sum( + any( + _cells_touch(cells, previous) + for previous in previous_components + ) + for _, previous_components in self._sparse_history + ) + if hit_count >= component_profile.sparse_persistence_minimum_hits: + promoted.append(item) + self._sparse_history.append( + (sequence, tuple(cells for _, cells in current)) + ) + self._last_sparse_sequence = sequence + return tuple(promoted) + def snapshot(self) -> M48LowStepOccupancySnapshot: with self._lock: return M48LowStepOccupancySnapshot( @@ -401,6 +475,9 @@ def load_m48_low_step_occupancy_profile( "voxel_size_m", "neighbor_radius_cells", "minimum_points", + "sparse_persistence_minimum_points", + "sparse_persistence_window_frames", + "sparse_persistence_minimum_hits", "minimum_voxels", "local_radius_m", "maximum_candidate_points_per_frame", @@ -523,6 +600,18 @@ def load_m48_low_step_occupancy_profile( component, "neighbor_radius_cells" ), minimum_points=_positive_integer(component, "minimum_points"), + sparse_persistence_minimum_points=_positive_integer( + component, + "sparse_persistence_minimum_points", + ), + sparse_persistence_window_frames=_positive_integer( + component, + "sparse_persistence_window_frames", + ), + sparse_persistence_minimum_hits=_positive_integer( + component, + "sparse_persistence_minimum_hits", + ), minimum_voxels=_positive_integer(component, "minimum_voxels"), local_radius_m=_number(component, "local_radius_m"), maximum_candidate_points_per_frame=_positive_integer( @@ -591,6 +680,29 @@ def _voxel_components( return tuple(components) +def _component_cells( + points_map: npt.NDArray[np.float64], + source_indices: IntArray, + *, + voxel_size_m: float, +) -> frozenset[tuple[int, int, int]]: + rows = np.floor(points_map[source_indices] / voxel_size_m).astype(np.int64) + 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") diff --git a/tests/test_m48_low_step_occupancy.py b/tests/test_m48_low_step_occupancy.py index 0fde2f4..5b6cf3c 100644 --- a/tests/test_m48_low_step_occupancy.py +++ b/tests/test_m48_low_step_occupancy.py @@ -144,6 +144,28 @@ def test_wide_operator_region_cannot_bridge_two_spatial_components() -> None: assert snapshot.failed_frames == 0 +def test_sparse_component_requires_bounded_causal_persistence() -> None: + points = np.asarray( + ((0.00, 0.0, 5.00), (0.04, 0.0, 5.00)), + dtype=np.float64, + ) + store = _Store(_frame(points), np.ones(2, dtype=np.uint8)) + provider = M48AdditiveLowStepGeometryProvider( # type: ignore[arg-type] + store=store, + profile=load_m48_low_step_occupancy_profile(PROFILE_PATH), + ) + + first = provider.associate(_packet(0), ()) + second = provider.associate(_packet(1), ()) + third = provider.associate(_packet(2), ()) + + assert first == () + assert second == () + assert len(third) == 1 + assert third[0].source_point_ids == (0, 1) + assert "additive-low-step-current-component" in third[0].reason_codes + + def test_frame_1856_preserves_baseline_posts_and_splits_low_hemisphere_support() -> None: store = RecordedGeometryStore.from_repository(REPOSITORY_ROOT) provider = M48AdditiveLowStepGeometryProvider(