From 14961841671bd7a2b395139cdfd580b914b8c472 Mon Sep 17 00:00:00 2001 From: DCCONSTRUCTIONS Date: Wed, 26 Aug 2026 13:59:26 +0300 Subject: [PATCH] perf(perception): require exact critical-range persistence --- .../m48r3-additive-low-step-occupancy-v1.json | 3 +- ...ve-low-step-reference-graph-shadow-v1.json | 2 +- .../Invoke-M48NNativeReferenceGraph.ps1 | 4 +- .../perception/m48_low_step_occupancy.py | 131 +++++++----------- tests/test_m48_low_step_occupancy.py | 111 +++++++++++---- 5 files changed, 137 insertions(+), 114 deletions(-) diff --git a/config/perception/m48r3-additive-low-step-occupancy-v1.json b/config/perception/m48r3-additive-low-step-occupancy-v1.json index 7c5fd92..2c820d3 100644 --- a/config/perception/m48r3-additive-low-step-occupancy-v1.json +++ b/config/perception/m48r3-additive-low-step-occupancy-v1.json @@ -22,7 +22,8 @@ "minimum_points": 5, "sparse_persistence_minimum_points": 2, "sparse_persistence_window_frames": 6, - "sparse_persistence_minimum_hits": 3, + "sparse_persistence_minimum_hits": 6, + "sparse_persistence_maximum_range_m": 8.0, "minimum_voxels": 1, "local_radius_m": 10.0, "maximum_candidate_points_per_frame": 768, diff --git a/config/perception/m48r3-native-low-step-reference-graph-shadow-v1.json b/config/perception/m48r3-native-low-step-reference-graph-shadow-v1.json index 397ec08..130d394 100644 --- a/config/perception/m48r3-native-low-step-reference-graph-shadow-v1.json +++ b/config/perception/m48r3-native-low-step-reference-graph-shadow-v1.json @@ -22,7 +22,7 @@ "provider_id": "ravnoves00-additive-low-step-geometry/v1", "version": "0.1.0", "revision": "m48r3-ravnoves00-additive-low-step/v1", - "sha256": "9ecc71a1a769d79ac62c93bcd84883ffbe8afd3d6db07c67ba2a5a504d6d841f" + "sha256": "00fc197ee7200e2f0447b6bec7cc2f21b3c2970d7dd9414cc41e6787d712fee5" }, { "role": "temporal", diff --git a/experiments/perception/worker/Invoke-M48NNativeReferenceGraph.ps1 b/experiments/perception/worker/Invoke-M48NNativeReferenceGraph.ps1 index fbe0a40..3dec7ab 100644 --- a/experiments/perception/worker/Invoke-M48NNativeReferenceGraph.ps1 +++ b/experiments/perception/worker/Invoke-M48NNativeReferenceGraph.ps1 @@ -113,10 +113,10 @@ $expectedConfigs = [ordered]@{ if ($AdditiveLowStep) { $expectedConfigs.Remove("m48n-rf-detr-native-reference-graph-shadow-v0.json") $expectedConfigs["m48r3-native-low-step-reference-graph-shadow-v1.json"] = ( - "c4fef7534a6c3b4305a1c068d68eb1e9be115e1310cddbda529269c7820c40ef" + "e2e307347265076908c4ec1b7da75028998490dfa74ba64f12939e233406d9d0" ) $expectedConfigs["m48r3-additive-low-step-occupancy-v1.json"] = ( - "9ecc71a1a769d79ac62c93bcd84883ffbe8afd3d6db07c67ba2a5a504d6d841f" + "00fc197ee7200e2f0447b6bec7cc2f21b3c2970d7dd9414cc41e6787d712fee5" ) } foreach ($entry in $expectedConfigs.GetEnumerator()) { diff --git a/src/k1link/perception/m48_low_step_occupancy.py b/src/k1link/perception/m48_low_step_occupancy.py index 52c190b..7b77e6e 100644 --- a/src/k1link/perception/m48_low_step_occupancy.py +++ b/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") diff --git a/tests/test_m48_low_step_occupancy.py b/tests/test_m48_low_step_occupancy.py index 5b6cf3c..93fca10 100644 --- a/tests/test_m48_low_step_occupancy.py +++ b/tests/test_m48_low_step_occupancy.py @@ -23,10 +23,7 @@ from k1link.perception.providers import SourcePacket from k1link.perception.recorded_source import RecordedFrameReference REPOSITORY_ROOT = Path(__file__).resolve().parents[1] -PROFILE_PATH = ( - REPOSITORY_ROOT - / "config/perception/m48r3-additive-low-step-occupancy-v1.json" -) +PROFILE_PATH = REPOSITORY_ROOT / "config/perception/m48r3-additive-low-step-occupancy-v1.json" R2_CASES_PATH = ( REPOSITORY_ROOT / ".runtime/compute-experiments/m48/static-occupancy-qualification-results" @@ -126,9 +123,7 @@ def test_wide_operator_region_cannot_bridge_two_spatial_components() -> None: observations = provider.associate(_packet(), ()) additive = tuple( - item - for item in observations - if "additive-low-step-current-component" in item.reason_codes + item for item in observations if "additive-low-step-current-component" in item.reason_codes ) assert len(additive) == 2 @@ -155,15 +150,29 @@ def test_sparse_component_requires_bounded_causal_persistence() -> None: profile=load_m48_low_step_occupancy_profile(PROFILE_PATH), ) - first = provider.associate(_packet(0), ()) - second = provider.associate(_packet(1), ()) - third = provider.associate(_packet(2), ()) + first_five = tuple(provider.associate(_packet(sequence), ()) for sequence in range(5)) + sixth = provider.associate(_packet(5), ()) - 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 + assert first_five == ((), (), (), (), ()) + assert len(sixth) == 1 + assert sixth[0].source_point_ids == (0, 1) + assert "additive-low-step-current-component" in sixth[0].reason_codes + + +def test_persistent_sparse_component_is_bounded_to_critical_range() -> None: + points = np.asarray( + ((0.00, 0.0, 8.20), (0.04, 0.0, 8.20)), + 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), + ) + + observations = tuple(provider.associate(_packet(sequence), ()) for sequence in range(6)) + + assert observations == ((), (), (), (), (), ()) def test_frame_1856_preserves_baseline_posts_and_splits_low_hemisphere_support() -> None: @@ -185,8 +194,7 @@ def test_frame_1856_preserves_baseline_posts_and_splits_low_hemisphere_support() profile=frame.projection, ) source_rows = { - int(source_index): row - for row, source_index in enumerate(projected.source_indices) + int(source_index): row for row, source_index in enumerate(projected.source_indices) } cases = [ json.loads(line) @@ -198,14 +206,62 @@ def test_frame_1856_preserves_baseline_posts_and_splits_low_hemisphere_support() hemispheres = by_anchor["anchor-924a4623077fe5df18816b47"] assert posts["accepted_graph"]["component_count"] >= 2 - assert _component_hits( - observations, - hemispheres["extent_xyxy"], - projected.pixels_xy, - source_rows, - width=frame.projection.width, - height=frame.projection.height, - ) >= 2 + assert ( + _component_hits( + observations, + hemispheres["extent_xyxy"], + projected.pixels_xy, + source_rows, + width=frame.projection.width, + height=frame.projection.height, + ) + >= 2 + ) + + +def test_exact_six_frame_persistence_recovers_critical_near_anchor() -> None: + store = RecordedGeometryStore.from_repository(REPOSITORY_ROOT) + provider = M48AdditiveLowStepGeometryProvider( + store=store, + profile=load_m48_low_step_occupancy_profile(PROFILE_PATH), + ) + observations: tuple[ObstacleObservation, ...] = () + for source_sequence in range(1084, 1093): + observations = provider.associate(_packet(source_sequence), ()) + + evidence_frame = store.frame_for_index(1092) + target_frame = store.frame_for_index(1093) + assert evidence_frame is not None + assert target_frame is not None + projected = project_map_points_kb4( + evidence_frame.points_map, + position_map_xyz=target_frame.sensor_position_map, + orientation_map_from_lidar_xyzw=target_frame.sensor_orientation_xyzw, + profile=target_frame.projection, + ) + source_rows = { + int(source_index): row for row, source_index in enumerate(projected.source_indices) + } + critical = next( + json.loads(line) + for line in R2_CASES_PATH.read_text("utf-8").splitlines() + if "anchor-0df056d9c565b74a25d3cca3" in line + ) + additive = tuple( + item for item in observations if "additive-low-step-current-component" in item.reason_codes + ) + + assert ( + _component_hits( + additive, + critical["extent_xyxy"], + projected.pixels_xy, + source_rows, + width=target_frame.projection.width, + height=target_frame.projection.height, + ) + >= 1 + ) def _component_hits( @@ -229,9 +285,6 @@ def _component_hits( if not indices: continue rows = [source_rows[index] for index in indices if index in source_rows] - if any( - x1 <= pixels[row, 0] <= x2 and y1 <= pixels[row, 1] <= y2 - for row in rows - ): + if any(x1 <= pixels[row, 0] <= x2 and y1 <= pixels[row, 1] <= y2 for row in rows): count += 1 return count