From 7aa3ce55c04c0bec2017d505061391ff1ba6aa75 Mon Sep 17 00:00:00 2001 From: DCCONSTRUCTIONS Date: Wed, 5 Aug 2026 21:12:47 +0300 Subject: [PATCH] fix(perception): reconstruct rolling occupancy from K1 increments --- ...ission-core-worker-shadow-v1.template.json | 2 +- config/perception/m4-replay-threat-v3.json | 67 +++ .../perception/m4-rolling-local-map-v1.json | 38 ++ scripts/build_m4_worker_shadow_artifact.py | 4 +- src/k1link/perception/contracts.py | 28 +- src/k1link/perception/rolling_map.py | 443 ++++++++++++++++++ src/k1link/perception/temporal_replay.py | 236 ++++++++-- src/k1link/perception/threat.py | 175 ++++--- src/k1link/perception/threat_replay.py | 351 +++++++++++++- src/k1link/web/m4_threat_replay_api.py | 12 +- tests/test_m4_threat_replay_result.py | 59 ++- tests/test_m4_worker_shadow_artifact.py | 4 + tests/test_perception_architecture.py | 2 + tests/test_replay_threat_provider.py | 32 +- tests/test_rolling_local_map.py | 137 ++++++ tests/test_temporal_replay_result.py | 40 ++ 16 files changed, 1489 insertions(+), 141 deletions(-) create mode 100644 config/perception/m4-replay-threat-v3.json create mode 100644 config/perception/m4-rolling-local-map-v1.json create mode 100644 src/k1link/perception/rolling_map.py create mode 100644 tests/test_rolling_local_map.py diff --git a/config/deployment/mission-core-worker-shadow-v1.template.json b/config/deployment/mission-core-worker-shadow-v1.template.json index 2fd1b60..ee89529 100644 --- a/config/deployment/mission-core-worker-shadow-v1.template.json +++ b/config/deployment/mission-core-worker-shadow-v1.template.json @@ -139,7 +139,7 @@ }, "wheel": { "name": "nodedc_mission_core-0.1.0-py3-none-any.whl", - "sha256": "19d8caf9a522747c461fb3ca30aafe54169959d8bd8e671fa6fc8c0ac107875d" + "sha256": "__WHEEL_SHA256__" } }, "rollback": { diff --git a/config/perception/m4-replay-threat-v3.json b/config/perception/m4-replay-threat-v3.json new file mode 100644 index 0000000..11c1845 --- /dev/null +++ b/config/perception/m4-replay-threat-v3.json @@ -0,0 +1,67 @@ +{ + "schema_version": "missioncore.replay-threat-profile/v3", + "profile_id": "m4-ravnoves00-virtual-corridor/v3", + "provider_id": "dual-evidence-replay-threat/v3", + "source": { + "source_id": "RAVNOVES00", + "session_id": "20260720T065719Z_viewer_live", + "temporal_result_id": "m4-temporal-replay-b8611526dfcd2b9be9049560d751bbd23a9ad54b7dda8e9dc48a17374d46266e", + "temporal_frames_sha256": "08a03669d3487ecf7bd62b047ae5b8d1ff586a27e7435e5f235907a4d709dd25", + "geometry_result_id": "m4-geometry-replay-8daf3109e3cf30b960b4b376032ff3b5ec58ca42a1e5899b841cf29fbcf14ad8", + "geometry_frames_sha256": "b4db5d0ebaba4d6268a1006707dc313c229f3dbdfd73b1d863cad5d853be8ac4", + "detector_result_id": "m4-detector-replay-11f83f2e0b81758ac2a5a5fc54e9d293b501678df5f6ef97b5c6069ba08605c5", + "detector_frames_sha256": "9bf5ae17938cd57c112278781b38d54a7187cf2dabc7bb0332acdb1efad721f5", + "source_pack_id": "e10-lidar-pack-576c994a6c814e2592dd6240ace3902a5db94843312c759a73ba0c9166157d2b", + "source_pack_sha256": "0685d24219d8236caf8b7f1685e93f6d6b59e7fd015a768d88a92bbe8b154944" + }, + "calibration": { + "calibration_id": "camera-1-kb4-05f3ad9b", + "content_identity_sha256": "05f3ad9b38b3a4fc95388a8ec83da83c745e217709e51787b3d5aad0969f6fa9", + "usage": "projection-and-forward-axis-binding" + }, + "body_frame": { + "schema_version": "missioncore.replay-body-frame-profile/v1", + "origin": "local-surface-vertical-projection", + "up": "vendor-slam-map-gravity-axis", + "forward": "smoothed-slam-trajectory-validated-by-camera-axis", + "trajectory_half_window_frames": 20, + "minimum_trajectory_displacement_m": 0.2, + "maximum_camera_route_misalignment_deg": 25.0, + "maximum_sensor_height_deviation_m": 0.45, + "maximum_surface_slope_deg": 10.0 + }, + "virtual_rig": { + "profile_id": "virtual-base-footprint-1000x600/v3", + "body_length_m": 1.0, + "body_width_m": 0.6, + "lidar_reference": "virtual-body-center", + "nominal_sensor_height_m": 1.25, + "physical_mount_claimed": false + }, + "corridor": { + "profile_id": "ravnoves00-forward-corridor-8m/v3", + "forward_length_m": 8.0, + "rear_margin_m": 0.5, + "lateral_clearance_m": 0.2, + "prediction_horizon_seconds": 5.0, + "occupied_voxel_size_m": 0.45, + "minimum_motion_span_seconds": 0.2 + }, + "policy": { + "camera_only_decision": "unknown", + "held_or_stale_decision": "unknown", + "semantic_class_used": false, + "detector_identity_used": false, + "absence_of_points_means_free": false, + "geometry_only_is_eligible": true, + "retained_map_intersection_decision": "threat" + }, + "authority": { + "mode": "replay-simulated", + "physical_live": false, + "physical_collision_accepted": false, + "commands_enabled": false, + "actuation_allowed": false, + "navigation_or_safety_accepted": false + } +} diff --git a/config/perception/m4-rolling-local-map-v1.json b/config/perception/m4-rolling-local-map-v1.json new file mode 100644 index 0000000..ffc533e --- /dev/null +++ b/config/perception/m4-rolling-local-map-v1.json @@ -0,0 +1,38 @@ +{ + "schema_version": "missioncore.rolling-local-map-profile/v1", + "profile_id": "ravnoves00-rolling-local-obstacle-map/v1", + "provider_id": "rolling-local-obstacle-map/v1", + "source": { + "source_id": "RAVNOVES00", + "session_id": "20260720T065719Z_viewer_live", + "representation_id": "registered-map-increment-v1" + }, + "bounds": { + "coordinate_frame": "map", + "voxel_size_m": 0.45, + "retention_seconds": 3.0, + "local_radius_m": 12.0, + "maximum_cells": 65536, + "maximum_cells_per_component": 4096, + "maximum_components": 1024, + "neighbor_radius_cells": 1 + }, + "policy": { + "input_is_complete_scan": false, + "input_is_registered_map_increment": true, + "absence_of_republication_means_free": false, + "clearing_from_missing_points": false, + "retained_occupancy_can_assert_threat": true, + "retained_motion_claimed": false, + "local_radius_eviction": true, + "time_bound_eviction": true, + "capacity_eviction_allowed": false + }, + "authority": { + "ground_truth": false, + "physical_live": false, + "commands_enabled": false, + "actuation_allowed": false, + "navigation_or_safety_accepted": false + } +} diff --git a/scripts/build_m4_worker_shadow_artifact.py b/scripts/build_m4_worker_shadow_artifact.py index d3e0d75..e88e0a8 100644 --- a/scripts/build_m4_worker_shadow_artifact.py +++ b/scripts/build_m4_worker_shadow_artifact.py @@ -25,7 +25,7 @@ WHEEL_NAME = "nodedc_mission_core-0.1.0-py3-none-any.whl" RUNNER_NAME = RUNNER.name PATCH_ID = re.compile(r"^[A-Za-z0-9._-]{1,96}$") EXPECTED_BASELINE_SHA256 = "ea10359339e6cce31b5780a2710299771cab7cc0c1c2a2b56a1621f786b31fa8" -EXPECTED_WHEEL_SHA256 = "fad22ce1b3ed926e0208ed95c767c01d61dd83a3a4af47607aa84a2d377e5bce" +EXPECTED_WHEEL_SHA256 = "215411719c6af4b042d0019f7f73b1885e2c2db47f87eb3617c653d7e751f19e" PAYLOAD_FILES = ( RUNNER_NAME, WHEEL_NAME, @@ -88,12 +88,14 @@ def render_descriptor(patch_id: str, revision: str) -> bytes: template.count("__PATCH_ID__") != 1 or template.count("__CODE_REVISION__") != 1 or template.count("__RUNNER_SHA256__") != 1 + or template.count("__WHEEL_SHA256__") != 1 ): raise ArtifactBuildError("descriptor template placeholders changed") rendered = ( template.replace("__PATCH_ID__", patch_id) .replace("__CODE_REVISION__", revision) .replace("__RUNNER_SHA256__", sha256_file(RUNNER)) + .replace("__WHEEL_SHA256__", EXPECTED_WHEEL_SHA256) ) document = json.loads(rendered) if document["patch_id"] != patch_id or document["code_revision"] != revision: diff --git a/src/k1link/perception/contracts.py b/src/k1link/perception/contracts.py index 6cb6276..16a9c2e 100644 --- a/src/k1link/perception/contracts.py +++ b/src/k1link/perception/contracts.py @@ -56,6 +56,7 @@ class EvidenceCurrentness(StrEnum): class TemporalState(StrEnum): CURRENT = "current" + RETAINED = "retained" HELD = "held" EXPIRED = "expired" @@ -617,6 +618,15 @@ class TemporalObstacle: if self.state is TemporalState.CURRENT: if self.age_ns > self.ttl_ns or not self.cells: raise PerceptionContractError("current temporal occupancy requires bounded cells") + elif self.state is TemporalState.RETAINED: + if not 0 < self.age_ns <= self.ttl_ns or not self.cells: + raise PerceptionContractError( + "retained rolling-map occupancy must remain within its bound" + ) + if self.motion is not MotionState.UNKNOWN: + raise PerceptionContractError( + "retained rolling-map occupancy cannot claim object motion" + ) elif self.state is TemporalState.HELD: if not 0 < self.age_ns <= self.ttl_ns or not self.cells: raise PerceptionContractError("held temporal state must remain within TTL") @@ -755,10 +765,20 @@ class LocalObstacleMap: _nonnegative_integer(self.output_age_ns, "map output age") if self.free_space_claimed: raise PerceptionContractError("Milestone 4 cannot publish implicit free space") - if any(item.state is not TemporalState.CURRENT for item in self.occupied): - raise PerceptionContractError("occupied map entries must be current") - if any(item.state is TemporalState.CURRENT for item in self.unknown): - raise PerceptionContractError("held or expired map entries must remain unknown") + if any( + item.state not in {TemporalState.CURRENT, TemporalState.RETAINED} + for item in self.occupied + ): + raise PerceptionContractError( + "occupied map entries must be current hits or bounded rolling-map retention" + ) + if any( + item.state in {TemporalState.CURRENT, TemporalState.RETAINED} + for item in self.unknown + ): + raise PerceptionContractError( + "held or expired temporal entries must remain unknown" + ) component_ids = [item.component_id for item in (*self.occupied, *self.unknown)] if len(set(component_ids)) != len(component_ids): raise PerceptionContractError("map component identities must be unique") diff --git a/src/k1link/perception/rolling_map.py b/src/k1link/perception/rolling_map.py new file mode 100644 index 0000000..9e7ec0c --- /dev/null +++ b/src/k1link/perception/rolling_map.py @@ -0,0 +1,443 @@ +"""Bounded rolling occupancy reconstructed from registered map increments. + +The K1 ``lio_pcl`` recording is a sequence of post-LIO map increments, not a +complete scan at every timestamp. This provider preserves the exact current +increment elsewhere and materializes only the still-valid *retained* cells +which a later increment did not need to publish again. Missing points never +clear occupancy. +""" + +from __future__ import annotations + +import hashlib +import json +import math +from collections import deque +from dataclasses import dataclass +from pathlib import Path +from typing import Final, Protocol + +from .contracts import GridCell, HistorySample, MotionState, TemporalObstacle, TemporalState +from .providers import SourcePacket + +ROLLING_MAP_PROFILE_SCHEMA: Final = "missioncore.rolling-local-map-profile/v1" +ROLLING_MAP_PROVIDER_ID: Final = "rolling-local-obstacle-map/v1" +DEFAULT_ROLLING_MAP_PROFILE_PATH: Final = Path( + "config/perception/m4-rolling-local-map-v1.json" +) + + +class RollingMapError(RuntimeError): + """The rolling map input, bounds or state is incompatible.""" + + +class ReplayPoseResolver(Protocol): + def pose_values_for_frame( + self, + frame_id: str, + ) -> tuple[tuple[float, float, float], tuple[float, float, float, float]] | None: ... + + +@dataclass(frozen=True, slots=True) +class RollingMapBounds: + coordinate_frame: str + voxel_size_m: float + retention_seconds: float + local_radius_m: float + maximum_cells: int + maximum_cells_per_component: int + maximum_components: int + neighbor_radius_cells: int + + def __post_init__(self) -> None: + numeric = ( + self.voxel_size_m, + self.retention_seconds, + self.local_radius_m, + ) + if any(not _positive_finite(value) for value in numeric): + raise RollingMapError("rolling map numeric bound is invalid") + integer = ( + self.maximum_cells, + self.maximum_cells_per_component, + self.maximum_components, + self.neighbor_radius_cells, + ) + if any(not _positive_integer(value) for value in integer): + raise RollingMapError("rolling map integer bound is invalid") + if self.maximum_cells_per_component > self.maximum_cells: + raise RollingMapError("rolling component bound exceeds map capacity") + + @property + def retention_ns(self) -> int: + return round(self.retention_seconds * 1_000_000_000) + + +@dataclass(frozen=True, slots=True) +class RollingMapProfile: + profile_id: str + source_id: str + session_id: str + representation_id: str + bounds: RollingMapBounds + profile_sha256: str + + +@dataclass(frozen=True, slots=True) +class RollingMapSnapshot: + voxel_size_m: float + retention_ns: int + local_radius_m: float + maximum_cells: int + input_frames: int + current_increment_cells: int + retained_component_publications: int + retained_cell_publications: int + time_evicted_cells: int + radius_evicted_cells: int + capacity_evicted_cells: int + active_cells_at_end: int + peak_active_cells: int + peak_retained_components: int + maximum_retained_age_ns: int + + +@dataclass(slots=True) +class _CellEvidence: + first_hit_ns: int + last_hit_ns: int + last_frame_id: str + hit_count: int + + +class RollingLocalObstacleMapProvider: + """Accumulate occupied map cells without inventing scan-based clearing.""" + + provider_id: str = ROLLING_MAP_PROVIDER_ID + + def __init__( + self, + *, + pose_resolver: ReplayPoseResolver, + profile: RollingMapProfile, + ) -> None: + self.pose_resolver = pose_resolver + self.profile = profile + self.config = profile.bounds + self._cells: dict[GridCell, _CellEvidence] = {} + self._previous_sequence: int | None = None + self._previous_time_ns: int | None = None + self._input_frames = 0 + self._current_increment_cells = 0 + self._retained_component_publications = 0 + self._retained_cell_publications = 0 + self._time_evicted_cells = 0 + self._radius_evicted_cells = 0 + self._peak_active_cells = 0 + self._peak_retained_components = 0 + self._maximum_retained_age_ns = 0 + + def update( + self, + packet: SourcePacket, + temporal_obstacles: tuple[TemporalObstacle, ...], + ) -> tuple[TemporalObstacle, ...]: + self._validate(packet) + now_ns = packet.envelope.timestamps.source_ns + current_cells = { + cell + for obstacle in temporal_obstacles + if obstacle.state is TemporalState.CURRENT + for cell in obstacle.cells + } + self._current_increment_cells += len(current_cells) + for cell in current_cells: + evidence = self._cells.get(cell) + if evidence is None: + self._cells[cell] = _CellEvidence( + first_hit_ns=now_ns, + last_hit_ns=now_ns, + last_frame_id=packet.envelope.frame_id, + hit_count=1, + ) + else: + evidence.last_hit_ns = now_ns + evidence.last_frame_id = packet.envelope.frame_id + evidence.hit_count += 1 + + self._evict_by_time(now_ns) + self._evict_by_radius(packet) + if len(self._cells) > self.config.maximum_cells: + raise RollingMapError( + "rolling map capacity exceeded; dropping occupied cells is forbidden" + ) + retained_cells = set(self._cells) - current_cells + components = self._components(retained_cells) + if len(components) > self.config.maximum_components: + raise RollingMapError("rolling map component bound exceeded") + result = tuple(self._contract(packet, cells) for cells in components) + self._retained_component_publications += len(result) + self._retained_cell_publications += sum(len(item.cells) for item in result) + self._peak_active_cells = max(self._peak_active_cells, len(self._cells)) + self._peak_retained_components = max( + self._peak_retained_components, + len(result), + ) + self._previous_sequence = packet.envelope.sequence + self._previous_time_ns = now_ns + self._input_frames += 1 + return result + + def _validate(self, packet: SourcePacket) -> None: + envelope = packet.envelope + if ( + envelope.source_id != self.profile.source_id + or envelope.session_id != self.profile.session_id + or envelope.representation_id != self.profile.representation_id + ): + raise RollingMapError("packet escaped the rolling map source profile") + now_ns = envelope.timestamps.source_ns + if self._previous_sequence is not None and ( + envelope.sequence <= self._previous_sequence + or self._previous_time_ns is None + or now_ns <= self._previous_time_ns + ): + raise RollingMapError("rolling map packet order is not monotonic") + + def _evict_by_time(self, now_ns: int) -> None: + expired = tuple( + cell + for cell, evidence in self._cells.items() + if now_ns - evidence.last_hit_ns > self.config.retention_ns + ) + for cell in expired: + del self._cells[cell] + self._time_evicted_cells += len(expired) + + def _evict_by_radius(self, packet: SourcePacket) -> None: + pose = self.pose_resolver.pose_values_for_frame(packet.envelope.frame_id) + if pose is None: + return + x, y, _ = pose[0] + radius_squared = self.config.local_radius_m**2 + evicted = tuple( + cell + for cell in self._cells + if ( + ((cell.x + 0.5) * self.config.voxel_size_m - x) ** 2 + + ((cell.y + 0.5) * self.config.voxel_size_m - y) ** 2 + > radius_squared + ) + ) + for cell in evicted: + del self._cells[cell] + self._radius_evicted_cells += len(evicted) + + def _components(self, cells: set[GridCell]) -> tuple[frozenset[GridCell], ...]: + remaining = set(cells) + components: list[frozenset[GridCell]] = [] + radius = self.config.neighbor_radius_cells + offsets = tuple( + (dx, dy, dz) + for dx in range(-radius, radius + 1) + for dy in range(-radius, radius + 1) + for dz in range(-radius, radius + 1) + if dx or dy or dz + ) + while remaining: + start = min(remaining, key=_cell_key) + remaining.remove(start) + connected = {start} + queue = deque((start,)) + while queue: + cell = queue.popleft() + for dx, dy, dz in offsets: + neighbor = GridCell(cell.x + dx, cell.y + dy, cell.z + dz) + if neighbor not in remaining: + continue + remaining.remove(neighbor) + connected.add(neighbor) + queue.append(neighbor) + if len(connected) > self.config.maximum_cells_per_component: + raise RollingMapError("rolling map component cell bound exceeded") + components.append(frozenset(connected)) + return tuple(sorted(components, key=lambda value: _cell_key(min(value, key=_cell_key)))) + + def _contract( + self, + packet: SourcePacket, + cells: frozenset[GridCell], + ) -> TemporalObstacle: + now_ns = packet.envelope.timestamps.source_ns + last_hit_ns = max(self._cells[cell].last_hit_ns for cell in cells) + age_ns = now_ns - last_hit_ns + if not 0 < age_ns <= self.config.retention_ns: + raise RollingMapError("retained component escaped rolling bounds") + centers = tuple( + ( + (cell.x + 0.5) * self.config.voxel_size_m, + (cell.y + 0.5) * self.config.voxel_size_m, + (cell.z + 0.5) * self.config.voxel_size_m, + ) + for cell in cells + ) + centroid = ( + sum(point[0] for point in centers) / len(centers), + sum(point[1] for point in centers) / len(centers), + sum(point[2] for point in centers) / len(centers), + ) + digest = hashlib.sha256( + ";".join( + f"{cell.x},{cell.y},{cell.z}" + for cell in sorted(cells, key=_cell_key) + ).encode() + ).hexdigest()[:24] + self._maximum_retained_age_ns = max(self._maximum_retained_age_ns, age_ns) + latest_frame = min( + evidence.last_frame_id + for cell in cells + if (evidence := self._cells[cell]).last_hit_ns == last_hit_ns + ) + return TemporalObstacle( + component_id=f"rolling-{digest}", + identity_scope="ephemeral", + state=TemporalState.RETAINED, + ttl_ns=self.config.retention_ns, + last_hit_ns=last_hit_ns, + age_ns=age_ns, + association_basis="registered-map-increment-retention", + history=( + HistorySample( + frame_id=latest_frame, + evidence_time_ns=last_hit_ns, + centroid_xyz_m=centroid, + ), + ), + cells=tuple(sorted(cells, key=_cell_key)), + coordinate_frame=self.config.coordinate_frame, + last_centroid_xyz_m=centroid, + motion=MotionState.UNKNOWN, + motion_confidence=0.0, + motion_reason="retained-map-increment-no-current-motion", + semantic_hint=None, + ) + + def snapshot(self) -> RollingMapSnapshot: + return RollingMapSnapshot( + voxel_size_m=self.config.voxel_size_m, + retention_ns=self.config.retention_ns, + local_radius_m=self.config.local_radius_m, + maximum_cells=self.config.maximum_cells, + input_frames=self._input_frames, + current_increment_cells=self._current_increment_cells, + retained_component_publications=self._retained_component_publications, + retained_cell_publications=self._retained_cell_publications, + time_evicted_cells=self._time_evicted_cells, + radius_evicted_cells=self._radius_evicted_cells, + capacity_evicted_cells=0, + active_cells_at_end=len(self._cells), + peak_active_cells=self._peak_active_cells, + peak_retained_components=self._peak_retained_components, + maximum_retained_age_ns=self._maximum_retained_age_ns, + ) + + +def load_rolling_map_profile(path: Path) -> RollingMapProfile: + resolved = path.resolve(strict=True) + if resolved.is_symlink() or not resolved.is_file(): + raise RollingMapError("rolling map profile is not a regular file") + raw = resolved.read_bytes() + try: + document = _object(json.loads(raw), "rolling map profile") + except json.JSONDecodeError as exc: + raise RollingMapError("rolling map profile JSON is invalid") from exc + _exact_keys( + document, + {"schema_version", "profile_id", "provider_id", "source", "bounds", "policy", "authority"}, + "rolling map profile", + ) + if ( + document["schema_version"] != ROLLING_MAP_PROFILE_SCHEMA + or document["provider_id"] != ROLLING_MAP_PROVIDER_ID + ): + raise RollingMapError("rolling map profile identity is incompatible") + source = _object(document["source"], "rolling map source") + bounds = _object(document["bounds"], "rolling map bounds") + _exact_keys(source, {"source_id", "session_id", "representation_id"}, "rolling map source") + _exact_keys(bounds, set(RollingMapBounds.__dataclass_fields__), "rolling map bounds") + if document["policy"] != { + "input_is_complete_scan": False, + "input_is_registered_map_increment": True, + "absence_of_republication_means_free": False, + "clearing_from_missing_points": False, + "retained_occupancy_can_assert_threat": True, + "retained_motion_claimed": False, + "local_radius_eviction": True, + "time_bound_eviction": True, + "capacity_eviction_allowed": False, + }: + raise RollingMapError("rolling map policy is incompatible") + if document["authority"] != { + "ground_truth": False, + "physical_live": False, + "commands_enabled": False, + "actuation_allowed": False, + "navigation_or_safety_accepted": False, + }: + raise RollingMapError("rolling map authority is incompatible") + return RollingMapProfile( + profile_id=_string(document, "profile_id"), + source_id=_string(source, "source_id"), + session_id=_string(source, "session_id"), + representation_id=_string(source, "representation_id"), + bounds=RollingMapBounds(**bounds), # type: ignore[arg-type] + profile_sha256=hashlib.sha256(raw).hexdigest(), + ) + + +def _cell_key(cell: GridCell) -> tuple[int, int, int]: + return cell.x, cell.y, cell.z + + +def _positive_finite(value: object) -> bool: + return ( + isinstance(value, (int, float)) + and not isinstance(value, bool) + and math.isfinite(float(value)) + and float(value) > 0.0 + ) + + +def _positive_integer(value: object) -> bool: + return isinstance(value, int) and not isinstance(value, bool) and value > 0 + + +def _object(value: object, label: str) -> dict[str, object]: + if not isinstance(value, dict) or any(not isinstance(key, str) for key in value): + raise RollingMapError(f"{label} must be an object") + return value + + +def _exact_keys(document: dict[str, object], expected: set[str], label: str) -> None: + if set(document) != expected: + raise RollingMapError(f"{label} fields are incompatible") + + +def _string(document: dict[str, object], key: str) -> str: + value = document.get(key) + if not isinstance(value, str) or not value: + raise RollingMapError(f"{key} must be a nonempty string") + return value + + +__all__ = [ + "DEFAULT_ROLLING_MAP_PROFILE_PATH", + "ROLLING_MAP_PROFILE_SCHEMA", + "ROLLING_MAP_PROVIDER_ID", + "RollingLocalObstacleMapProvider", + "RollingMapBounds", + "RollingMapError", + "RollingMapProfile", + "RollingMapSnapshot", + "load_rolling_map_profile", +] diff --git a/src/k1link/perception/temporal_replay.py b/src/k1link/perception/temporal_replay.py index 9f52c82..aa87ad5 100644 --- a/src/k1link/perception/temporal_replay.py +++ b/src/k1link/perception/temporal_replay.py @@ -21,6 +21,12 @@ from .geometry import RecordedGeometryStore from .geometry_replay import read_geometry_replay_result from .motion import ClassIndependentMotionEstimator, MotionEstimatorSnapshot from .recorded_source import RecordedRavnoves00Source, ReplayPacing +from .rolling_map import ( + DEFAULT_ROLLING_MAP_PROFILE_PATH, + RollingLocalObstacleMapProvider, + RollingMapSnapshot, + load_rolling_map_profile, +) from .temporal import ( DEFAULT_TEMPORAL_MOTION_PROFILE_PATH, BoundedSpatialTemporalProvider, @@ -31,6 +37,9 @@ from .temporal import ( TEMPORAL_REPLAY_SCHEMA: Final = "missioncore.perception-temporal-replay-result/v1" TEMPORAL_REPLAY_FRAME_SCHEMA: Final = "missioncore.perception-temporal-replay-frame/v1" TEMPORAL_REPLAY_REPORT_SCHEMA: Final = "missioncore.perception-temporal-replay-report/v1" +TEMPORAL_REPLAY_SCHEMA_V2: Final = "missioncore.perception-temporal-replay-result/v2" +TEMPORAL_REPLAY_FRAME_SCHEMA_V2: Final = "missioncore.perception-temporal-replay-frame/v2" +TEMPORAL_REPLAY_REPORT_SCHEMA_V2: Final = "missioncore.perception-temporal-replay-report/v2" TEMPORAL_REPLAY_RESULT_PREFIX: Final = "m4-temporal-replay-" TEMPORAL_REPLAY_FRAMES_NAME: Final = "frames.jsonl" TEMPORAL_REPLAY_REPORT_NAME: Final = "report.json" @@ -91,6 +100,13 @@ def build_temporal_replay( store = RecordedGeometryStore.from_repository(repository) temporal = BoundedSpatialTemporalProvider(point_resolver=store, profile=profile) motion = ClassIndependentMotionEstimator(profile=profile) + rolling_profile = load_rolling_map_profile( + repository / DEFAULT_ROLLING_MAP_PROFILE_PATH + ) + rolling = RollingLocalObstacleMapProvider( + pose_resolver=store, + profile=rolling_profile, + ) references, clip_labels = _verified_historical_references(repository) source = RecordedRavnoves00Source.from_repository( repository, @@ -129,6 +145,7 @@ def build_temporal_replay( frame_started_ns = time.perf_counter_ns() temporal_obstacles = temporal.update(packet, observations) obstacles = motion.estimate(packet, temporal_obstacles) + rolling_retained = rolling.update(packet, obstacles) latencies_ms.append((time.perf_counter_ns() - frame_started_ns) / 1_000_000) current = tuple( item for item in obstacles if item.state is TemporalState.CURRENT @@ -137,7 +154,7 @@ def build_temporal_replay( expired = tuple(item for item in obstacles if item.state is TemporalState.EXPIRED) motion_counts = _motion_counts(current) frame_document = { - "schema_version": TEMPORAL_REPLAY_FRAME_SCHEMA, + "schema_version": TEMPORAL_REPLAY_FRAME_SCHEMA_V2, "sequence": frame_count, "frame_id": packet.envelope.frame_id, "source_time_ns": packet.envelope.timestamps.source_ns, @@ -156,6 +173,9 @@ def build_temporal_replay( "current": [item.to_dict() for item in current], "held": [item.to_dict() for item in held], "expired": [item.to_dict() for item in expired], + "rolling_retained": [ + item.to_dict() for item in rolling_retained + ], "motion_counts": motion_counts, "map_frame_jump_candidate": any( item.association_basis == "map-frame-discontinuity" @@ -173,21 +193,28 @@ def build_temporal_replay( raise TemporalReplayError("geometry frame ledger exceeds recorded source") temporal_snapshot = temporal.snapshot() motion_snapshot = motion.snapshot() + rolling_snapshot = rolling.snapshot() elapsed_ns = time.perf_counter_ns() - started_ns metrics = _metrics( frame_count=frame_count, input_observations=input_observations, temporal=temporal_snapshot, motion=motion_snapshot, + rolling=rolling_snapshot, latencies_ms=latencies_ms, elapsed_ns=elapsed_ns, clip_checks=clip_checks, ) - requirements = _requirements(metrics, profile.temporal.occupied_ttl_seconds) + requirements = _requirements_v2( + metrics, + profile.temporal.occupied_ttl_seconds, + rolling_profile.bounds.retention_seconds, + rolling_profile.bounds.maximum_cells, + ) accepted = all(value is True for value in requirements.values()) frames_sha256 = _file_sha256(frames_path) identity = { - "schema_version": TEMPORAL_REPLAY_SCHEMA, + "schema_version": TEMPORAL_REPLAY_SCHEMA_V2, "geometry_result_id": geometry.result_id, "geometry_manifest_sha256": _file_sha256( geometry.result_root / "manifest.json" @@ -197,6 +224,9 @@ def build_temporal_replay( "profile_sha256": profile.profile_sha256, "temporal_provider_id": temporal.provider_id, "motion_provider_id": motion.provider_id, + "rolling_map_profile_id": rolling_profile.profile_id, + "rolling_map_profile_sha256": rolling_profile.profile_sha256, + "rolling_map_provider_id": rolling.provider_id, "historical_references": references, "producer_sha256": _producer_hashes(repository), "frames_sha256": frames_sha256, @@ -209,7 +239,7 @@ def build_temporal_replay( identity_sha256 = hashlib.sha256(_canonical_json(identity)).hexdigest() result_id = f"{TEMPORAL_REPLAY_RESULT_PREFIX}{identity_sha256}" report = { - "schema_version": TEMPORAL_REPLAY_REPORT_SCHEMA, + "schema_version": TEMPORAL_REPLAY_REPORT_SCHEMA_V2, "result_id": result_id, "identity_sha256": identity_sha256, "status": "accepted" if accepted else "rejected", @@ -221,13 +251,21 @@ def build_temporal_replay( "Clip checks are aggregate frame comparisons without component correspondence.", "Motion confidence is bounded evidence sufficiency, not class probability.", "Component identity is ephemeral and cannot be used as long-term ReID.", + ( + "Rolling occupancy is reconstructed from registered map increments; " + "missing republication never proves free space." + ), + ( + "Retained cells are conservatively time/radius bounded and do not " + "claim current object motion or independent truth." + ), ], "authority": _false_authority(), } report_path = staging / TEMPORAL_REPLAY_REPORT_NAME _write_json(report_path, report) manifest = { - "schema_version": TEMPORAL_REPLAY_SCHEMA, + "schema_version": TEMPORAL_REPLAY_SCHEMA_V2, "result_id": result_id, "identity_sha256": identity_sha256, "identity": identity, @@ -270,33 +308,45 @@ def read_temporal_replay_result(root: Path) -> TemporalReplayResult: }, "temporal replay manifest", ) + schema_version = manifest.get("schema_version") + if schema_version not in {TEMPORAL_REPLAY_SCHEMA, TEMPORAL_REPLAY_SCHEMA_V2}: + raise TemporalReplayError("temporal replay schema is incompatible") + is_v2 = schema_version == TEMPORAL_REPLAY_SCHEMA_V2 identity = _object(manifest.get("identity"), "temporal replay identity") + identity_keys = { + "schema_version", + "geometry_result_id", + "geometry_manifest_sha256", + "geometry_frames_sha256", + "profile_id", + "profile_sha256", + "temporal_provider_id", + "motion_provider_id", + "historical_references", + "producer_sha256", + "frames_sha256", + "metrics", + "clip_checks", + "acceptance_requirements", + "accepted", + "authority", + } + if is_v2: + identity_keys.update( + { + "rolling_map_profile_id", + "rolling_map_profile_sha256", + "rolling_map_provider_id", + } + ) _exact_keys( identity, - { - "schema_version", - "geometry_result_id", - "geometry_manifest_sha256", - "geometry_frames_sha256", - "profile_id", - "profile_sha256", - "temporal_provider_id", - "motion_provider_id", - "historical_references", - "producer_sha256", - "frames_sha256", - "metrics", - "clip_checks", - "acceptance_requirements", - "accepted", - "authority", - }, + identity_keys, "temporal replay identity", ) identity_sha256 = hashlib.sha256(_canonical_json(identity)).hexdigest() if ( - manifest.get("schema_version") != TEMPORAL_REPLAY_SCHEMA - or manifest.get("result_id") != resolved.name + manifest.get("result_id") != resolved.name or manifest.get("identity_sha256") != identity_sha256 or resolved.name != f"{TEMPORAL_REPLAY_RESULT_PREFIX}{identity_sha256}" ): @@ -343,13 +393,23 @@ def read_temporal_replay_result(root: Path) -> TemporalReplayResult: accepted = all(value is True for value in requirements.values()) retention = _object(metrics.get("retention"), "retention metrics") ttl_ns = _integer(retention.get("ttl_ns"), "retention TTL") + expected_requirements = ( + _requirements_v2(metrics, ttl_ns / 1_000_000_000, 3.0, 65536) + if is_v2 + else _requirements(metrics, ttl_ns / 1_000_000_000) + ) if ( ttl_ns != 750_000_000 - or requirements != _requirements(metrics, ttl_ns / 1_000_000_000) + or requirements != expected_requirements ): raise TemporalReplayError("temporal replay acceptance was not derived from metrics") if ( - report.get("schema_version") != TEMPORAL_REPLAY_REPORT_SCHEMA + report.get("schema_version") + != ( + TEMPORAL_REPLAY_REPORT_SCHEMA_V2 + if is_v2 + else TEMPORAL_REPLAY_REPORT_SCHEMA + ) or report.get("result_id") != resolved.name or report.get("identity_sha256") != identity_sha256 or report.get("metrics") != metrics @@ -361,7 +421,7 @@ def read_temporal_replay_result(root: Path) -> TemporalReplayResult: or identity.get("accepted") is not accepted ): raise TemporalReplayError("temporal replay report changed") - _validate_frame_ledger(frames_path, metrics) + _validate_frame_ledger(frames_path, metrics, is_v2=is_v2) return TemporalReplayResult( result_id=resolved.name, result_root=resolved, @@ -378,6 +438,7 @@ def _metrics( input_observations: int, temporal: TemporalProviderSnapshot, motion: MotionEstimatorSnapshot, + rolling: RollingMapSnapshot, latencies_ms: list[float], elapsed_ns: int, clip_checks: list[dict[str, object]], @@ -388,6 +449,7 @@ def _metrics( "input_observations": input_observations, "temporal": asdict(temporal), "motion": asdict(motion), + "rolling_map": asdict(rolling), "retention": { "ttl_ns": 750_000_000, "maximum_held_age_ns": temporal.maximum_held_age_ns, @@ -417,6 +479,53 @@ def _metrics( } +def _requirements_v2( + metrics: dict[str, object], + ttl_seconds: float, + rolling_retention_seconds: float, + rolling_maximum_cells: int, +) -> dict[str, bool]: + requirements = _requirements(metrics, ttl_seconds) + rolling = _object(metrics.get("rolling_map"), "rolling map metrics") + requirements.update( + { + "registered_increment_is_not_treated_as_complete_scan": True, + "rolling_map_processed_every_source_frame": ( + rolling.get("input_frames") == 4489 + ), + "rolling_map_materialized_retained_occupancy": ( + _integer( + rolling.get("retained_component_publications"), + "rolling retained component publications", + ) + > 0 + ), + "rolling_map_is_time_radius_and_capacity_bounded": ( + _integer( + rolling.get("maximum_retained_age_ns"), + "maximum rolling retained age", + ) + <= round(rolling_retention_seconds * 1_000_000_000) + and rolling.get("retention_ns") + == round(rolling_retention_seconds * 1_000_000_000) + and rolling.get("maximum_cells") == rolling_maximum_cells + and _integer( + rolling.get("capacity_evicted_cells"), + "rolling capacity evictions", + ) + == 0 + and _integer( + rolling.get("peak_active_cells"), + "rolling peak cells", + ) + <= rolling_maximum_cells + ), + "missing_republication_never_claims_free_space": True, + } + ) + return requirements + + def _requirements(metrics: dict[str, object], ttl_seconds: float) -> dict[str, bool]: frames = _object(metrics.get("frames"), "temporal frame metrics") temporal = _object(metrics.get("temporal"), "temporal provider metrics") @@ -507,7 +616,12 @@ def _requirements(metrics: dict[str, object], ttl_seconds: float) -> dict[str, b } -def _validate_frame_ledger(path: Path, metrics: dict[str, object]) -> None: +def _validate_frame_ledger( + path: Path, + metrics: dict[str, object], + *, + is_v2: bool, +) -> None: frames = 0 observations = 0 current_inputs = 0 @@ -515,10 +629,11 @@ def _validate_frame_ledger(path: Path, metrics: dict[str, object]) -> None: current_publications = 0 held_publications = 0 expired_publications = 0 + rolling_publications = 0 motion_counts = {state.value: 0 for state in MotionState} with path.open("rb") as handle: for line in handle: - frame = _read_frame(line, frames) + frame = _read_frame(line, frames, is_v2=is_v2) if ( frame.get("policy") != _frame_policy() or frame.get("authority") != _false_authority() @@ -565,6 +680,23 @@ def _validate_frame_ledger(path: Path, metrics: dict[str, object]) -> None: ] if len(component_ids) != len(set(component_ids)): raise TemporalReplayError("temporal frame duplicated a component") + if is_v2: + rolling = tuple( + TemporalObstacle.from_dict(item) + for item in _array( + frame.get("rolling_retained"), + "rolling retained obstacles", + ) + ) + if any(item.state is not TemporalState.RETAINED for item in rolling): + raise TemporalReplayError( + "rolling map published a non-retained component" + ) + if set(component_ids) & {item.component_id for item in rolling}: + raise TemporalReplayError( + "rolling and temporal component identities overlap" + ) + rolling_publications += len(rolling) current_publications += len(groups[TemporalState.CURRENT]) held_publications += len(groups[TemporalState.HELD]) expired_publications += len(groups[TemporalState.EXPIRED]) @@ -574,6 +706,11 @@ def _validate_frame_ledger(path: Path, metrics: dict[str, object]) -> None: frame_metrics = _object(metrics.get("frames"), "temporal frames") temporal = _object(metrics.get("temporal"), "temporal metrics") motion = _object(metrics.get("motion"), "motion metrics") + rolling_metrics = ( + _object(metrics.get("rolling_map"), "rolling map metrics") + if is_v2 + else None + ) if ( frames != frame_metrics.get("total") or observations != metrics.get("input_observations") @@ -585,6 +722,11 @@ def _validate_frame_ledger(path: Path, metrics: dict[str, object]) -> None: or motion_counts[MotionState.MOVING.value] != motion.get("moving") or motion_counts[MotionState.STATIONARY.value] != motion.get("stationary") or motion_counts[MotionState.UNKNOWN.value] != motion.get("unknown") + or ( + rolling_metrics is not None + and rolling_publications + != rolling_metrics.get("retained_component_publications") + ) ): raise TemporalReplayError("temporal frame ledger and metrics disagree") @@ -681,7 +823,12 @@ def _frame_policy() -> dict[str, object]: def _producer_hashes(repository: Path) -> dict[str, str]: return { name: _file_sha256(repository / "src/k1link/perception" / name) - for name in ("temporal.py", "motion.py", "temporal_replay.py") + for name in ( + "temporal.py", + "motion.py", + "rolling_map.py", + "temporal_replay.py", + ) } @@ -697,16 +844,19 @@ def _read_geometry_frame(line: bytes, sequence: int) -> dict[str, object]: return frame -def _read_frame(line: bytes, sequence: int) -> dict[str, object]: +def _read_frame( + line: bytes, + sequence: int, + *, + is_v2: bool, +) -> dict[str, object]: try: frame = _object(json.loads(line), "temporal replay frame") except json.JSONDecodeError as exc: raise TemporalReplayError( f"temporal replay frame {sequence + 1} is invalid JSON" ) from exc - _exact_keys( - frame, - { + expected_keys = { "schema_version", "sequence", "frame_id", @@ -722,11 +872,13 @@ def _read_frame(line: bytes, sequence: int) -> dict[str, object]: "map_frame_jump_candidate", "policy", "authority", - }, - "temporal replay frame", - ) + } + if is_v2: + expected_keys.add("rolling_retained") + _exact_keys(frame, expected_keys, "temporal replay frame") if ( - frame.get("schema_version") != TEMPORAL_REPLAY_FRAME_SCHEMA + frame.get("schema_version") + != (TEMPORAL_REPLAY_FRAME_SCHEMA_V2 if is_v2 else TEMPORAL_REPLAY_FRAME_SCHEMA) or frame.get("sequence") != sequence or frame.get("frame_id") != f"frame-{sequence:06d}" or not isinstance(frame.get("source_available"), bool) @@ -793,6 +945,12 @@ def _object(value: object, label: str) -> dict[str, object]: return value +def _array(value: object, label: str) -> list[object]: + if not isinstance(value, list): + raise TemporalReplayError(f"{label} must be an array") + return value + + def _exact_keys(document: dict[str, object], expected: set[str], label: str) -> None: if set(document) != expected: raise TemporalReplayError(f"{label} fields changed") diff --git a/src/k1link/perception/threat.py b/src/k1link/perception/threat.py index 7a5b7de..1bf5bfa 100644 --- a/src/k1link/perception/threat.py +++ b/src/k1link/perception/threat.py @@ -10,6 +10,7 @@ from __future__ import annotations import hashlib import json import math +from collections.abc import Iterable from dataclasses import dataclass from pathlib import Path from typing import Final, Protocol @@ -26,9 +27,14 @@ from .contracts import ( ) from .geometry_math import quaternion_xyzw_to_rotation_matrix -REPLAY_THREAT_PROFILE_SCHEMA: Final = "missioncore.replay-threat-profile/v2" -REPLAY_THREAT_PROVIDER_ID: Final = "dual-evidence-replay-threat/v2" -DEFAULT_REPLAY_THREAT_PROFILE_PATH: Final = "config/perception/m4-replay-threat-v2.json" +REPLAY_THREAT_PROFILE_SCHEMA: Final = "missioncore.replay-threat-profile/v3" +REPLAY_THREAT_PROVIDER_ID: Final = "dual-evidence-replay-threat/v3" +DEFAULT_REPLAY_THREAT_PROFILE_PATH: Final = "config/perception/m4-replay-threat-v3.json" +LEGACY_REPLAY_THREAT_PROFILE_SCHEMA: Final = "missioncore.replay-threat-profile/v2" +LEGACY_REPLAY_THREAT_PROVIDER_ID: Final = "dual-evidence-replay-threat/v2" + +type Vector3 = tuple[float, float, float] +type Matrix3 = tuple[Vector3, Vector3, Vector3] class ReplayThreatError(ValueError): @@ -38,12 +44,8 @@ class ReplayThreatError(ValueError): @dataclass(frozen=True, slots=True) class ReplayBodyFrame: frame_id: str - origin_map_xyz_m: tuple[float, float, float] - basis_map_from_body: tuple[ - tuple[float, float, float], - tuple[float, float, float], - tuple[float, float, float], - ] + origin_map_xyz_m: Vector3 + basis_map_from_body: Matrix3 sensor_height_m: float surface_slope_deg: float forward_source: str @@ -68,8 +70,22 @@ class ReplayBodyFrame: ) ): raise ReplayThreatError("replay body frame is not finite") - columns = tuple( - tuple(self.basis_map_from_body[row][column] for row in range(3)) for column in range(3) + columns: Matrix3 = ( + ( + self.basis_map_from_body[0][0], + self.basis_map_from_body[1][0], + self.basis_map_from_body[2][0], + ), + ( + self.basis_map_from_body[0][1], + self.basis_map_from_body[1][1], + self.basis_map_from_body[2][1], + ), + ( + self.basis_map_from_body[0][2], + self.basis_map_from_body[1][2], + self.basis_map_from_body[2][2], + ), ) if ( not self.forward_source @@ -87,15 +103,17 @@ class ReplayBodyFrame: def map_point_to_body( self, - point_map_xyz_m: tuple[float, float, float], - ) -> tuple[float, float, float]: - delta = tuple(point_map_xyz_m[index] - self.origin_map_xyz_m[index] for index in range(3)) + point_map_xyz_m: Vector3, + ) -> Vector3: + delta = _vector3( + point_map_xyz_m[index] - self.origin_map_xyz_m[index] for index in range(3) + ) return self.map_vector_to_body(delta) def map_vector_to_body( self, - vector_map_xyz_m: tuple[float, float, float], - ) -> tuple[float, float, float]: + vector_map_xyz_m: Vector3, + ) -> Vector3: values = tuple( float( sum( @@ -172,14 +190,14 @@ class RecordedReplayBodyFrameResolver: ) if inputs is None: return self._store(frame_id, None, "source-or-surface-unavailable") - position = tuple(float(value) for value in inputs.sensor_position_map) + position = _vector3(float(value) for value in inputs.sensor_position_map) plane = tuple(float(value) for value in inputs.ground_plane_coefficients_map) normal_norm = math.sqrt(sum(value * value for value in plane[:3])) if normal_norm < 1e-9: return self._store(frame_id, None, "ground-normal-invalid") - ground_normal = tuple(value / normal_norm for value in plane[:3]) + ground_normal = _vector3(value / normal_norm for value in plane[:3]) if ground_normal[2] < 0.0: - ground_normal = tuple(-value for value in ground_normal) + ground_normal = _vector3(-value for value in ground_normal) plane = tuple(-value for value in plane) sensor_height = _dot(position, ground_normal) + plane[3] / normal_norm if ( @@ -200,15 +218,15 @@ class RecordedReplayBodyFrameResolver: vertical_height = sensor_height / vertical_denominator rotation = quaternion_xyzw_to_rotation_matrix(inputs.sensor_orientation_map_from_lidar_xyzw) calibration = inputs.t_camera_from_lidar - camera_forward_lidar = tuple(float(calibration[2, index]) for index in range(3)) - camera_forward_map = tuple( + camera_forward_lidar = _vector3(float(calibration[2, index]) for index in range(3)) + camera_forward_map = _vector3( float(sum(rotation[row, column] * camera_forward_lidar[column] for column in range(3))) for row in range(3) ) camera_forward = _normalize(_reject(camera_forward_map, up)) if camera_forward is None: return self._store(frame_id, None, "camera-forward-invalid") - route = tuple( + route = _vector3( float( inputs.trajectory_end_position_map[index] - inputs.trajectory_start_position_map[index] @@ -235,8 +253,12 @@ class RecordedReplayBodyFrameResolver: return self._store(frame_id, None, "body-left-invalid") forward = _normalize(_cross(left, up)) assert forward is not None - origin = tuple(position[index] - vertical_height * up[index] for index in range(3)) - basis = tuple((forward[row], left[row], up[row]) for row in range(3)) + origin = _vector3(position[index] - vertical_height * up[index] for index in range(3)) + basis: Matrix3 = ( + (forward[0], left[0], up[0]), + (forward[1], left[1], up[1]), + (forward[2], left[2], up[2]), + ) frame = ReplayBodyFrame( frame_id=frame_id, origin_map_xyz_m=origin, @@ -326,7 +348,10 @@ class DualEvidenceReplayThreatProvider: body_frame_resolver: ReplayBodyFrameResolver, profile: ReplayThreatProfile, ) -> None: - if profile.provider_id != self.provider_id: + if profile.provider_id not in { + self.provider_id, + LEGACY_REPLAY_THREAT_PROVIDER_ID, + }: raise ReplayThreatError("threat provider identity changed") self.body_frame_resolver = body_frame_resolver self.profile = profile @@ -354,7 +379,7 @@ class DualEvidenceReplayThreatProvider: obstacle: TemporalObstacle, body_frame: ReplayBodyFrame | None, ) -> ThreatAssessment: - if obstacle.state is not TemporalState.CURRENT: + if obstacle.state not in {TemporalState.CURRENT, TemporalState.RETAINED}: return self._unknown( frame_id, obstacle.component_id, @@ -390,12 +415,21 @@ class DualEvidenceReplayThreatProvider: rig=self.profile.rig, corridor=self.profile.corridor, ) - motion_complete = obstacle.motion is not MotionState.UNKNOWN and velocity_body is not None + retained = obstacle.state is TemporalState.RETAINED + motion_complete = ( + not retained + and obstacle.motion is not MotionState.UNKNOWN + and velocity_body is not None + ) if current_intersection or (motion_complete and corridor_entry is not None): intersection = CorridorIntersection.INTERSECTS decision = ThreatDecision.THREAT reasons = [ - "current-corridor-intersection" + ( + "retained-corridor-intersection" + if retained + else "current-corridor-intersection" + ) if current_intersection else "predicted-corridor-intersection", "metric-lidar-geometry", @@ -404,6 +438,14 @@ class DualEvidenceReplayThreatProvider: intersection = CorridorIntersection.CLEAR decision = ThreatDecision.NOT_THREAT reasons = ["predicted-corridor-clear", "metric-lidar-geometry"] + elif retained: + intersection = CorridorIntersection.UNKNOWN + decision = ThreatDecision.UNKNOWN + reasons = [ + "retained-map-occupancy-outside-current-corridor", + "metric-lidar-geometry", + "absence-of-republication-is-not-free", + ] else: intersection = CorridorIntersection.UNKNOWN decision = ThreatDecision.UNKNOWN @@ -423,7 +465,9 @@ class DualEvidenceReplayThreatProvider: horizon_seconds=self.profile.corridor.prediction_horizon_seconds, ) relative_speed = _closing_speed_mps(centroid_body, velocity_body) - if velocity_body is None: + if retained: + reasons.append("motion-retained-map-increment-no-current-motion") + elif velocity_body is None: reasons.append(f"motion-{obstacle.motion_reason}") else: reasons.append(f"motion-{obstacle.motion.value}") @@ -465,14 +509,14 @@ class DualEvidenceReplayThreatProvider: or last.frame_id != current_body_frame.frame_id ): return None - obstacle_delta = tuple( + obstacle_delta = _vector3( last.centroid_xyz_m[index] - first.centroid_xyz_m[index] for index in range(3) ) - rig_delta = tuple( + rig_delta = _vector3( last_body_frame.origin_map_xyz_m[index] - first_body_frame.origin_map_xyz_m[index] for index in range(3) ) - relative_map = tuple( + relative_map = _vector3( (obstacle_delta[index] - rig_delta[index]) / span_seconds for index in range(3) ) body = current_body_frame.map_vector_to_body(relative_map) @@ -530,11 +574,13 @@ def load_replay_threat_profile(path: Path) -> ReplayThreatProfile: }, "replay threat profile", ) - if ( - document["schema_version"] != REPLAY_THREAT_PROFILE_SCHEMA - or document["provider_id"] != REPLAY_THREAT_PROVIDER_ID - ): + identity = (document["schema_version"], document["provider_id"]) + if identity not in { + (REPLAY_THREAT_PROFILE_SCHEMA, REPLAY_THREAT_PROVIDER_ID), + (LEGACY_REPLAY_THREAT_PROFILE_SCHEMA, LEGACY_REPLAY_THREAT_PROVIDER_ID), + }: raise ReplayThreatError("replay threat profile identity is incompatible") + is_v3 = identity == (REPLAY_THREAT_PROFILE_SCHEMA, REPLAY_THREAT_PROVIDER_ID) source = _object(document["source"], "threat source") calibration = _object(document["calibration"], "threat calibration") body_frame = _object(document["body_frame"], "virtual body frame") @@ -603,18 +649,17 @@ def load_replay_threat_profile(path: Path) -> ReplayThreatProfile: }, "virtual corridor", ) - _exact_keys( - policy, - { + policy_keys = { "camera_only_decision", "held_or_stale_decision", "semantic_class_used", "detector_identity_used", "absence_of_points_means_free", "geometry_only_is_eligible", - }, - "threat policy", - ) + } + if is_v3: + policy_keys.add("retained_map_intersection_decision") + _exact_keys(policy, policy_keys, "threat policy") _exact_keys( authority, { @@ -634,14 +679,18 @@ def load_replay_threat_profile(path: Path) -> ReplayThreatProfile: or body_frame.get("up") != "vendor-slam-map-gravity-axis" or body_frame.get("forward") != "smoothed-slam-trajectory-validated-by-camera-axis" or rig.get("physical_mount_claimed") is not False - or policy - != { + or policy != { "camera_only_decision": "unknown", "held_or_stale_decision": "unknown", "semantic_class_used": False, "detector_identity_used": False, "absence_of_points_means_free": False, "geometry_only_is_eligible": True, + **( + {"retained_map_intersection_decision": "threat"} + if is_v3 + else {} + ), } or authority != { @@ -936,17 +985,19 @@ def _positive_integer(document: dict[str, object], key: str) -> int: return value -def _dot( - first: tuple[float, float, float], - second: tuple[float, float, float], -) -> float: +def _vector3(values: Iterable[float]) -> Vector3: + first, second, third = values + return float(first), float(second), float(third) + + +def _dot(first: Vector3, second: Vector3) -> float: return sum(first[index] * second[index] for index in range(3)) def _cross( - first: tuple[float, float, float], - second: tuple[float, float, float], -) -> tuple[float, float, float]: + first: Vector3, + second: Vector3, +) -> Vector3: return ( first[1] * second[2] - first[2] * second[1], first[2] * second[0] - first[0] * second[2], @@ -955,25 +1006,29 @@ def _cross( def _normalize( - value: tuple[float, float, float], -) -> tuple[float, float, float] | None: + value: Vector3, +) -> Vector3 | None: norm = math.sqrt(_dot(value, value)) if norm < 1e-9: return None - return tuple(item / norm for item in value) + return value[0] / norm, value[1] / norm, value[2] / norm def _reject( - value: tuple[float, float, float], - normal: tuple[float, float, float], -) -> tuple[float, float, float]: + value: Vector3, + normal: Vector3, +) -> Vector3: along = _dot(value, normal) - return tuple(value[index] - along * normal[index] for index in range(3)) + return ( + value[0] - along * normal[0], + value[1] - along * normal[1], + value[2] - along * normal[2], + ) def _angle_degrees( - first: tuple[float, float, float], - second: tuple[float, float, float], + first: Vector3, + second: Vector3, ) -> float: return math.degrees(math.acos(max(-1.0, min(1.0, _dot(first, second))))) diff --git a/src/k1link/perception/threat_replay.py b/src/k1link/perception/threat_replay.py index b2e8415..442a924 100644 --- a/src/k1link/perception/threat_replay.py +++ b/src/k1link/perception/threat_replay.py @@ -53,6 +53,11 @@ THREAT_REPLAY_FRAME_SCHEMA: Final = "missioncore.perception-threat-replay-frame/ THREAT_REPLAY_VISUAL_SCHEMA: Final = "missioncore.perception-threat-visual-frame/v1" THREAT_REPLAY_FIXTURE_SCHEMA: Final = "missioncore.perception-threat-fixtures/v1" THREAT_REPLAY_REPORT_SCHEMA: Final = "missioncore.perception-threat-replay-report/v1" +THREAT_REPLAY_SCHEMA_V2: Final = "missioncore.perception-threat-replay-result/v2" +THREAT_REPLAY_FRAME_SCHEMA_V2: Final = "missioncore.perception-threat-replay-frame/v2" +THREAT_REPLAY_VISUAL_SCHEMA_V2: Final = "missioncore.perception-threat-visual-frame/v2" +THREAT_REPLAY_FIXTURE_SCHEMA_V2: Final = "missioncore.perception-threat-fixtures/v2" +THREAT_REPLAY_REPORT_SCHEMA_V2: Final = "missioncore.perception-threat-replay-report/v2" THREAT_REPLAY_RESULT_PREFIX: Final = "m4-threat-replay-" THREAT_REPLAY_FRAMES_NAME: Final = "frames.jsonl" THREAT_REPLAY_VISUALS_NAME: Final = "visual-frames.jsonl" @@ -61,7 +66,23 @@ THREAT_REPLAY_REPORT_NAME: Final = "report.json" THREAT_REPLAY_MANIFEST_NAME: Final = "manifest.json" VISUAL_FRAME_COUNT: Final = 32 VISUAL_POINT_LIMIT: Final = 4_000 -VISUAL_GEOMETRY_REGRESSION_SEQUENCES: Final = (138, 274) +VISUAL_GEOMETRY_REGRESSION_SEQUENCES: Final = (138, 274, 1880) +FRAME_1880_ENGINEERING_ANCHORS: Final = ( + { + "anchor_id": "near-concrete-hemisphere", + "x_bounds_m": (0.3, 1.2), + "y_bounds_m": (-0.8, 0.2), + "z_bounds_m": (-0.1, 0.9), + "must_assert_threat": True, + }, + { + "anchor_id": "far-concrete-hemisphere", + "x_bounds_m": (1.5, 2.7), + "y_bounds_m": (0.6, 1.7), + "z_bounds_m": (-0.1, 0.9), + "must_assert_threat": False, + }, +) class ThreatReplayError(RuntimeError): @@ -121,6 +142,7 @@ def build_threat_replay( reason_counts: Counter[str] = Counter() latencies_ms: list[float] = [] visual_count = 0 + frame_1880_regression: dict[str, object] | None = None try: temporal_frames_path = temporal.result_root / "frames.jsonl" geometry_frames_path = geometry.result_root / "frames.jsonl" @@ -159,6 +181,20 @@ def build_threat_replay( for key in ("held", "expired") for value in _array(temporal_frame.get(key), f"{key} obstacles") ) + rolling_retained = tuple( + TemporalObstacle.from_dict(value) + for value in _array( + temporal_frame.get("rolling_retained"), + "rolling retained obstacles", + ) + ) + if any( + item.state is not TemporalState.RETAINED + for item in rolling_retained + ): + raise ThreatReplayError( + "temporal replay rolling map escaped retained state" + ) geometry_observations = _array( geometry_frame.get("observations"), "geometry observations" ) @@ -183,7 +219,7 @@ def build_threat_replay( graph_id="reference-perception-graph/v1", generated_monotonic_ns=0, output_age_ns=0, - occupied=current, + occupied=(*current, *rolling_retained), unknown=unknown, camera_uncertainty=camera_uncertainty, accounting=SourceAccounting(1, 1, 0, 0), @@ -192,7 +228,10 @@ def build_threat_replay( assessments = provider.assess(obstacle_map) latencies_ms.append((time.perf_counter_ns() - frame_started_ns) / 1_000_000) by_id = {item.component_id: item for item in assessments} - expected_ids = {item.component_id for item in (*current, *unknown)} | { + expected_ids = { + item.component_id + for item in (*current, *rolling_retained, *unknown) + } | { item.proposal_id for item in camera_uncertainty } if set(by_id) != expected_ids: @@ -203,12 +242,21 @@ def build_threat_replay( by_id, ) metric_rows = [ - _metric_row(item, by_id[item.component_id]) for item in (*current, *unknown) + _metric_row(item, by_id[item.component_id]) + for item in (*current, *rolling_retained, *unknown) ] + if frame_count == 1880: + frame_1880_regression = _frame_1880_regression( + metric_rows, + body_frame_resolver.body_frame_for_frame( + packet.envelope.frame_id + ), + ) for item in assessments: assessment_counts[item.decision.value] += 1 reason_counts.update(item.reason_codes) evidence_counts["current-metric"] += len(current) + evidence_counts["rolling-map-retained"] += len(rolling_retained) evidence_counts["stale-or-held"] += len(unknown) evidence_counts["camera-only"] += len(camera_uncertainty) for obstacle in current: @@ -216,7 +264,7 @@ def build_threat_replay( f"{obstacle.motion.value}:{by_id[obstacle.component_id].decision.value}" ] += 1 frame_document = { - "schema_version": THREAT_REPLAY_FRAME_SCHEMA, + "schema_version": THREAT_REPLAY_FRAME_SCHEMA_V2, "sequence": frame_count, "frame_id": packet.envelope.frame_id, "source_time_ns": packet.envelope.timestamps.source_ns, @@ -232,6 +280,8 @@ def build_threat_replay( "metric_obstacles": len(metric_rows), "camera_proposals": len(proposals), "camera_only": len(camera_uncertainty), + "current_increment_metric": len(current), + "rolling_map_retained": len(rolling_retained), "assessments": len(assessments), }, "authority": _false_authority(), @@ -276,14 +326,15 @@ def build_threat_replay( visual_count=visual_count, fixtures=fixtures, body_frame=body_frame_resolver.qualification_summary(), + frame_1880_regression=frame_1880_regression, ) - requirements = _requirements(metrics, fixtures) + requirements = _requirements_v2(metrics, fixtures) accepted = all(value is True for value in requirements.values()) frames_sha256 = _file_sha256(frames_path) visuals_sha256 = _file_sha256(visuals_path) fixtures_sha256 = _file_sha256(fixtures_path) identity = { - "schema_version": THREAT_REPLAY_SCHEMA, + "schema_version": THREAT_REPLAY_SCHEMA_V2, "profile_id": profile.profile_id, "profile_sha256": profile.profile_sha256, "provider_id": provider.provider_id, @@ -319,7 +370,7 @@ def build_threat_replay( identity_sha256 = hashlib.sha256(_canonical_json(identity)).hexdigest() result_id = f"{THREAT_REPLAY_RESULT_PREFIX}{identity_sha256}" report = { - "schema_version": THREAT_REPLAY_REPORT_SCHEMA, + "schema_version": THREAT_REPLAY_REPORT_SCHEMA_V2, "result_id": result_id, "identity_sha256": identity_sha256, "status": "accepted" if accepted else "rejected", @@ -355,7 +406,7 @@ def build_threat_replay( report_path = staging / THREAT_REPLAY_REPORT_NAME _write_json(report_path, report) manifest = { - "schema_version": THREAT_REPLAY_SCHEMA, + "schema_version": THREAT_REPLAY_SCHEMA_V2, "result_id": result_id, "identity_sha256": identity_sha256, "identity": identity, @@ -400,11 +451,14 @@ def read_threat_replay_result(root: Path) -> ThreatReplayResult: }, "threat replay manifest", ) + schema_version = manifest.get("schema_version") + if schema_version not in {THREAT_REPLAY_SCHEMA, THREAT_REPLAY_SCHEMA_V2}: + raise ThreatReplayError("threat replay schema is incompatible") + is_v2 = schema_version == THREAT_REPLAY_SCHEMA_V2 identity = _object(manifest.get("identity"), "threat replay identity") identity_sha256 = hashlib.sha256(_canonical_json(identity)).hexdigest() if ( - manifest.get("schema_version") != THREAT_REPLAY_SCHEMA - or manifest.get("result_id") != resolved.name + manifest.get("result_id") != resolved.name or manifest.get("identity_sha256") != identity_sha256 or resolved.name != f"{THREAT_REPLAY_RESULT_PREFIX}{identity_sha256}" ): @@ -433,8 +487,14 @@ def read_threat_replay_result(root: Path) -> ThreatReplayResult: requirements = _object(identity.get("acceptance_requirements"), "threat requirements") fixtures = _read_json(paths["threat-deterministic-fixtures"]) accepted = all(value is True for value in requirements.values()) + expected_requirements = ( + _requirements_v2(metrics, fixtures) + if is_v2 + else _requirements_v1(metrics, fixtures) + ) if ( - report.get("schema_version") != THREAT_REPLAY_REPORT_SCHEMA + report.get("schema_version") + != (THREAT_REPLAY_REPORT_SCHEMA_V2 if is_v2 else THREAT_REPLAY_REPORT_SCHEMA) or report.get("result_id") != resolved.name or report.get("identity_sha256") != identity_sha256 or report.get("metrics") != metrics @@ -443,13 +503,14 @@ def read_threat_replay_result(root: Path) -> ThreatReplayResult: or identity.get("authority") != _false_authority() or manifest.get("accepted") is not accepted or identity.get("accepted") is not accepted - or requirements != _requirements(metrics, fixtures) + or requirements != expected_requirements ): raise ThreatReplayError("threat replay report or acceptance changed") _validate_ledgers( paths["threat-replay-frames"], paths["threat-visual-frames"], metrics, + is_v2=is_v2, ) return ThreatReplayResult( result_id=resolved.name, @@ -617,13 +678,18 @@ def _visual_frame( } ) return { - "schema_version": THREAT_REPLAY_VISUAL_SCHEMA, + "schema_version": THREAT_REPLAY_VISUAL_SCHEMA_V2, "sequence": packet.envelope.sequence, "frame_id": packet.envelope.frame_id, "source_time_ns": packet.envelope.timestamps.source_ns, "point_cloud_body_xyz_m": np.round(sampled, 6).tolist(), "point_cloud_source_count": int(points.shape[0]), "point_cloud_sample_count": int(sampled.shape[0]), + "point_cloud_layer": "current-increment", + "rolling_map_component_count": sum( + row.get("state") == TemporalState.RETAINED.value + for row in metric_rows + ), "metric_obstacles": metric_visuals, "camera_proposals": camera_rows, "body_frame": { @@ -649,6 +715,99 @@ def _visual_frame( } +def _frame_1880_regression( + metric_rows: list[dict[str, object]], + body_frame: ReplayBodyFrame | None, +) -> dict[str, object]: + if body_frame is None: + raise ThreatReplayError("frame 1880 has no qualified body frame") + retained_threats = 0 + retained_components = 0 + retained_rows: list[tuple[dict[str, object], tuple[float, float, float]]] = [] + for row in metric_rows: + if row.get("state") != TemporalState.RETAINED.value: + continue + retained_components += 1 + assessment = _object(row.get("assessment"), "frame 1880 assessment") + if assessment.get("decision") == ThreatDecision.THREAT.value: + retained_threats += 1 + centroid_map = _array( + row.get("centroid_map_xyz_m"), + "frame 1880 retained centroid", + ) + if len(centroid_map) != 3: + raise ThreatReplayError("frame 1880 retained centroid is invalid") + centroid_values = tuple( + _number_value(value, "frame 1880 centroid") for value in centroid_map + ) + centroid_body = body_frame.map_point_to_body( + (centroid_values[0], centroid_values[1], centroid_values[2]) + ) + retained_rows.append((row, centroid_body)) + + anchors: list[dict[str, object]] = [] + matched_ids: set[str] = set() + for raw_anchor in FRAME_1880_ENGINEERING_ANCHORS: + anchor = _object(raw_anchor, "frame 1880 engineering anchor") + x_bounds = _bounds(anchor.get("x_bounds_m"), "frame 1880 x bounds") + y_bounds = _bounds(anchor.get("y_bounds_m"), "frame 1880 y bounds") + z_bounds = _bounds(anchor.get("z_bounds_m"), "frame 1880 z bounds") + match = next( + ( + (row, centroid) + for row, centroid in retained_rows + if row.get("component_id") not in matched_ids + and x_bounds[0] <= centroid[0] <= x_bounds[1] + and y_bounds[0] <= centroid[1] <= y_bounds[1] + and z_bounds[0] <= centroid[2] <= z_bounds[1] + ), + None, + ) + component_id = None if match is None else str(match[0]["component_id"]) + if component_id is not None: + matched_ids.add(component_id) + anchor_assessment = ( + None + if match is None + else _object(match[0].get("assessment"), "frame 1880 anchor assessment") + ) + anchors.append( + { + "anchor_id": anchor["anchor_id"], + "bounds_body_xyz_m": [list(x_bounds), list(y_bounds), list(z_bounds)], + "must_assert_threat": anchor["must_assert_threat"], + "matched": match is not None, + "component_id": component_id, + "centroid_body_xyz_m": ( + None if match is None else list(match[1]) + ), + "decision": ( + None + if anchor_assessment is None + else anchor_assessment.get("decision") + ), + } + ) + required_threats_passed = all( + item["matched"] is True + and ( + item["must_assert_threat"] is False + or item["decision"] == ThreatDecision.THREAT.value + ) + for item in anchors + ) + return { + "sequence": 1880, + "retained_components": retained_components, + "retained_threat_components": retained_threats, + "engineering_anchors": anchors, + "matched_anchor_count": sum(item["matched"] is True for item in anchors), + "required_threats_passed": required_threats_passed, + "camera_visible_hemispheres_independent_truth": False, + "gate": "two-visible-hemisphere-regression", + } + + class _FixtureBodyFrames: def body_frame_for_frame(self, frame_id: str) -> ReplayBodyFrame: return ReplayBodyFrame( @@ -766,6 +925,19 @@ def _fixture_document(profile: ReplayThreatProfile) -> dict[str, object]: ), ThreatDecision.UNKNOWN, ), + _fixture_case( + provider, + "retained-in-corridor", + _fixture_obstacle( + "fixture-retained-in", + GridCell(6, 0, 0), + MotionState.UNKNOWN, + ((frame_id, 200_000_000, (2.925, 0.225, 0.225)),), + state=TemporalState.RETAINED, + ), + ThreatDecision.THREAT, + critical=True, + ), _fixture_camera_case(provider, frame_id), _fixture_case( provider, @@ -784,7 +956,7 @@ def _fixture_document(profile: ReplayThreatProfile) -> dict[str, object]: ), ] return { - "schema_version": THREAT_REPLAY_FIXTURE_SCHEMA, + "schema_version": THREAT_REPLAY_FIXTURE_SCHEMA_V2, "cases": cases, "critical_case_count": sum(item["critical"] is True for item in cases), "critical_false_not_threat_count": sum( @@ -810,7 +982,11 @@ def _fixture_obstacle( component_id=component_id, identity_scope="ephemeral", state=state, - ttl_ns=750_000_000, + ttl_ns=( + 3_000_000_000 + if state is TemporalState.RETAINED + else 750_000_000 + ), last_hit_ns=last.evidence_time_ns, age_ns=0 if state is TemporalState.CURRENT else 100_000_000, association_basis="deterministic-fixture", @@ -849,8 +1025,12 @@ def _fixture_case( graph_id="reference-perception-graph/v1", generated_monotonic_ns=0, output_age_ns=0, - occupied=(obstacle,) if obstacle.state is TemporalState.CURRENT else (), - unknown=(obstacle,) if obstacle.state is not TemporalState.CURRENT else (), + occupied=(obstacle,) + if obstacle.state in {TemporalState.CURRENT, TemporalState.RETAINED} + else (), + unknown=(obstacle,) + if obstacle.state not in {TemporalState.CURRENT, TemporalState.RETAINED} + else (), camera_uncertainty=(), accounting=SourceAccounting(1, 1, 0, 0), ) @@ -915,6 +1095,7 @@ def _metrics( visual_count: int, fixtures: dict[str, object], body_frame: dict[str, object], + frame_1880_regression: dict[str, object] | None, ) -> dict[str, object]: values = np.asarray(latencies_ms, dtype=np.float64) return { @@ -934,6 +1115,7 @@ def _metrics( "virtual_corridor_available": True, "qualified_base_footprint_available": True, "geometry_regression_sequences": list(VISUAL_GEOMETRY_REGRESSION_SEQUENCES), + "frame_1880_regression": frame_1880_regression, }, "fixtures": { "passed": fixtures["passed_count"], @@ -951,7 +1133,102 @@ def _metrics( } -def _requirements( +def _requirements_v1( + metrics: dict[str, object], + fixtures: dict[str, object], +) -> dict[str, bool]: + frames = _object(metrics.get("frames"), "frame metrics") + evidence = _object(metrics.get("evidence"), "evidence metrics") + decisions = _object(metrics.get("decisions"), "decision metrics") + visual = _object(metrics.get("visual_evidence"), "visual metrics") + body_frame = _object(metrics.get("body_frame"), "body frame metrics") + total_evidence = sum(_integer(value, "evidence count") for value in evidence.values()) + total_decisions = sum(_integer(value, "decision count") for value in decisions.values()) + cases = _array(fixtures.get("cases"), "fixture cases") + camera_case = next( + ( + _object(item, "fixture") + for item in cases + if isinstance(item, dict) and item.get("name") == "camera-only" + ), + {}, + ) + stale_cases = [ + _object(item, "fixture") + for item in cases + if isinstance(item, dict) + and item.get("name") in {"occluded-held", "stale-expired"} + ] + return { + "full_ravnoves00_replay_completed": ( + frames.get("total") == 4489 and frames.get("failed") == 0 + ), + "every_metric_or_camera_evidence_received_one_assessment": ( + total_evidence == total_decisions and total_evidence > 0 + ), + "camera_only_is_unknown_never_safe": camera_case.get("actual") == "unknown", + "held_and_stale_are_unknown_never_safe": ( + len(stale_cases) == 2 + and all(item.get("actual") == "unknown" for item in stale_cases) + ), + "geometry_only_evidence_is_assessed": ( + _integer( + _object(metrics.get("reason_counts"), "reason metrics").get( + "geometry-only-evidence", 0 + ), + "geometry-only count", + ) + > 0 + ), + "deterministic_fixture_matrix_passed": ( + fixtures.get("passed_count") == fixtures.get("total_count") == 9 + ), + "zero_critical_fixture_false_not_threat": ( + fixtures.get("critical_false_not_threat_count") == 0 + ), + "visual_video_camera_cloud_distance_and_corridor_are_available": ( + visual.get("frame_count") == VISUAL_FRAME_COUNT + and all( + visual.get(key) is True + for key in ( + "video_overlay_available", + "camera_boxes_available", + "point_cloud_available", + "metric_distance_available", + "virtual_corridor_available", + "qualified_base_footprint_available", + ) + ) + and visual.get("geometry_regression_sequences") == [138, 274] + ), + "body_frame_is_grounded_gravity_stable_and_route_aligned": ( + body_frame.get("available") + == _integer(body_frame.get("qualified"), "qualified body frames") + + _integer(body_frame.get("rejected"), "rejected body frames") + and _integer(body_frame.get("qualified"), "qualified body frames") + >= math.ceil( + _integer(body_frame.get("available"), "available body frames") * 0.95 + ) + and body_frame.get("origin") == "local-surface-vertical-projection" + and body_frame.get("up") == "vendor-slam-map-gravity-axis" + and body_frame.get("forward") + == "smoothed-slam-trajectory-validated-by-camera-axis" + and _number_value( + _object( + body_frame.get("camera_forward_alignment_deg"), + "body alignment metrics", + ).get("maximum"), + "maximum body alignment", + ) + <= 25.0 + ), + "physical_collision_and_actuation_authority_remain_false": ( + fixtures.get("authority") == _false_authority() + ), + } + + +def _requirements_v2( metrics: dict[str, object], fixtures: dict[str, object], ) -> dict[str, bool]: @@ -997,7 +1274,7 @@ def _requirements( > 0 ), "deterministic_fixture_matrix_passed": ( - fixtures.get("passed_count") == fixtures.get("total_count") == 9 + fixtures.get("passed_count") == fixtures.get("total_count") == 10 ), "zero_critical_fixture_false_not_threat": ( fixtures.get("critical_false_not_threat_count") == 0 @@ -1018,6 +1295,19 @@ def _requirements( and visual.get("geometry_regression_sequences") == list(VISUAL_GEOMETRY_REGRESSION_SEQUENCES) ), + "frame_1880_retains_two_hemispheres_and_blocks_near_corridor": ( + isinstance(visual.get("frame_1880_regression"), dict) + and _object( + visual.get("frame_1880_regression"), + "frame 1880 regression", + ).get("matched_anchor_count") + == len(FRAME_1880_ENGINEERING_ANCHORS) + and _object( + visual.get("frame_1880_regression"), + "frame 1880 regression", + ).get("required_threats_passed") + is True + ), "body_frame_is_grounded_gravity_stable_and_route_aligned": ( body_frame.get("available") == _integer(body_frame.get("qualified"), "qualified body frames") @@ -1046,12 +1336,15 @@ def _validate_ledgers( frames_path: Path, visuals_path: Path, metrics: dict[str, object], + *, + is_v2: bool, ) -> None: frame_count = 0 assessment_count = 0 for sequence, frame in enumerate(_read_jsonl(frames_path)): if ( - frame.get("schema_version") != THREAT_REPLAY_FRAME_SCHEMA + frame.get("schema_version") + != (THREAT_REPLAY_FRAME_SCHEMA_V2 if is_v2 else THREAT_REPLAY_FRAME_SCHEMA) or frame.get("sequence") != sequence or frame.get("authority") != _false_authority() ): @@ -1071,7 +1364,11 @@ def _validate_ledgers( for value in _object(metrics.get("decisions"), "decisions").values() ) or len(visuals) != VISUAL_FRAME_COUNT - or any(item.get("schema_version") != THREAT_REPLAY_VISUAL_SCHEMA for item in visuals) + or any( + item.get("schema_version") + != (THREAT_REPLAY_VISUAL_SCHEMA_V2 if is_v2 else THREAT_REPLAY_VISUAL_SCHEMA) + for item in visuals + ) ): raise ThreatReplayError("threat replay ledger and metrics disagree") @@ -1233,6 +1530,16 @@ def _number_value(value: object, label: str) -> float: return float(value) +def _bounds(value: object, label: str) -> tuple[float, float]: + if not isinstance(value, tuple) or len(value) != 2: + raise ThreatReplayError(f"{label} must contain two values") + lower = _number_value(value[0], label) + upper = _number_value(value[1], label) + if lower >= upper: + raise ThreatReplayError(f"{label} must be ordered") + return lower, upper + + def _signed_integer(value: object, label: str) -> int: if not isinstance(value, int) or isinstance(value, bool): raise ThreatReplayError(f"{label} must be an integer") diff --git a/src/k1link/web/m4_threat_replay_api.py b/src/k1link/web/m4_threat_replay_api.py index f8482fc..94b8986 100644 --- a/src/k1link/web/m4_threat_replay_api.py +++ b/src/k1link/web/m4_threat_replay_api.py @@ -14,8 +14,10 @@ from fastapi import APIRouter, HTTPException, Query from k1link.perception.threat_replay import ( THREAT_REPLAY_FRAME_SCHEMA, + THREAT_REPLAY_FRAME_SCHEMA_V2, THREAT_REPLAY_RESULT_PREFIX, THREAT_REPLAY_VISUAL_SCHEMA, + THREAT_REPLAY_VISUAL_SCHEMA_V2, ThreatReplayError, ThreatReplayResult, read_threat_replay_result, @@ -57,10 +59,11 @@ def build_m4_threat_replay_router( items: list[dict[str, object]] = [] invalid_total = 0 for candidate in candidates: + if len(items) >= limit: + break try: frozen = result(candidate.name) - if len(items) < limit: - items.append(_project_result(frozen)) + items.append(_project_result(frozen)) except HTTPException: invalid_total += 1 return { @@ -149,7 +152,8 @@ def _cached_video_overlay( frames = [] for expected_sequence, row in enumerate(_iter_jsonl(root / "frames.jsonl")): if ( - row.get("schema_version") != THREAT_REPLAY_FRAME_SCHEMA + row.get("schema_version") + not in {THREAT_REPLAY_FRAME_SCHEMA, THREAT_REPLAY_FRAME_SCHEMA_V2} or row.get("sequence") != expected_sequence ): raise ValueError("M4.6 video frame order changed") @@ -288,7 +292,9 @@ def _iter_jsonl(path: Path) -> Iterator[dict[str, object]]: raise ValueError("M4.6 JSONL row is invalid") if value.get("schema_version") not in { THREAT_REPLAY_FRAME_SCHEMA, + THREAT_REPLAY_FRAME_SCHEMA_V2, THREAT_REPLAY_VISUAL_SCHEMA, + THREAT_REPLAY_VISUAL_SCHEMA_V2, }: raise ValueError("M4.6 JSONL schema is invalid") yield value diff --git a/tests/test_m4_threat_replay_result.py b/tests/test_m4_threat_replay_result.py index c404130..ae29f3c 100644 --- a/tests/test_m4_threat_replay_result.py +++ b/tests/test_m4_threat_replay_result.py @@ -8,7 +8,7 @@ from k1link.perception.threat_replay import read_threat_replay_result from k1link.web.m4_threat_replay_api import build_m4_threat_replay_router REPOSITORY_ROOT = Path(__file__).resolve().parents[1] -RESULT_ID = "m4-threat-replay-78a06d96c4db5263dc63fc4e6e067c07fc81370d3f5085ff43361af89cec1e9e" +RESULT_ID = "m4-threat-replay-ef521b23eee704dee99856b6e93d5047a9b358e21ffda3ea9cacc2ef768164d9" RESULTS_ROOT = REPOSITORY_ROOT / ".runtime/compute-experiments/m4/replay-threat" @@ -29,18 +29,19 @@ def test_full_source_threat_result_closes_m4_6_contract() -> None: assert result.metrics["evidence"] == { "camera-only": 10158, "current-metric": 27299, + "rolling-map-retained": 69855, "stale-or-held": 37995, } assert result.metrics["decisions"] == { "not-threat": 10700, - "threat": 2716, - "unknown": 62036, + "threat": 6626, + "unknown": 127981, } assert result.metrics["fixtures"] == { - "critical": 4, + "critical": 5, "critical_false_not_threat": 0, - "passed": 9, - "total": 9, + "passed": 10, + "total": 10, } @@ -50,10 +51,10 @@ def test_threat_result_is_content_bound_and_visual_evidence_is_complete() -> Non assert isinstance(identity, dict) assert identity["frames_sha256"] == ( - "d55e7651f0b16a62c6b61c5cb2358dd8dff87dbfa57a59e9ec350bc38b156bc1" + "57219acd7dfe1cf04b12d4a415d1819e0a4947bc3173333bb56eea8fd2bd47b9" ) assert identity["visuals_sha256"] == ( - "957c35d46ae30143beb6b2f26f8f722853ef2a1e91a41d5dc1a03fbf723a54e0" + "bda54e144a1b4c878ba645a8dafedcdc0332e16a1489b62b7100670e8eae6f25" ) visual = result.metrics["visual_evidence"] assert isinstance(visual, dict) @@ -69,7 +70,44 @@ def test_threat_result_is_content_bound_and_visual_evidence_is_complete() -> Non "qualified_base_footprint_available", ) ) - assert visual["geometry_regression_sequences"] == [138, 274] + assert visual["geometry_regression_sequences"] == [138, 274, 1880] + assert visual["frame_1880_regression"] == { + "camera_visible_hemispheres_independent_truth": False, + "engineering_anchors": [ + { + "anchor_id": "near-concrete-hemisphere", + "bounds_body_xyz_m": [[0.3, 1.2], [-0.8, 0.2], [-0.1, 0.9]], + "centroid_body_xyz_m": [ + 0.7594228459267565, + -0.2681278641873485, + 0.3753253937774115, + ], + "component_id": "rolling-5329b5d2ef498e6250c931ff", + "decision": "threat", + "matched": True, + "must_assert_threat": True, + }, + { + "anchor_id": "far-concrete-hemisphere", + "bounds_body_xyz_m": [[1.5, 2.7], [0.6, 1.7], [-0.1, 0.9]], + "centroid_body_xyz_m": [ + 2.058019871618555, + 1.1276051922616088, + 0.2553253937774115, + ], + "component_id": "rolling-7ba116b07683abeca7c8005b", + "decision": "threat", + "matched": True, + "must_assert_threat": False, + }, + ], + "gate": "two-visible-hemisphere-regression", + "matched_anchor_count": 2, + "required_threats_passed": True, + "retained_components": 10, + "retained_threat_components": 2, + "sequence": 1880, + } body_frame = result.metrics["body_frame"] assert body_frame["qualified"] == 3861 assert body_frame["rejected"] == 67 @@ -88,7 +126,8 @@ def test_m4_6_lab_api_projects_report_and_exact_visual_frame() -> None: assert len(visuals["items"]) == 32 assert [item["sequence"] for item in visuals["items"][:3]] == [62, 138, 274] frame = get_visual(RESULT_ID, 1) - assert frame["schema_version"] == "missioncore.perception-threat-visual-frame/v1" + assert frame["schema_version"] == "missioncore.perception-threat-visual-frame/v2" + assert frame["point_cloud_layer"] == "current-increment" assert frame["point_cloud_sample_count"] > 0 assert frame["rig"] == { "length_m": 1.0, diff --git a/tests/test_m4_worker_shadow_artifact.py b/tests/test_m4_worker_shadow_artifact.py index ba66210..22daa79 100644 --- a/tests/test_m4_worker_shadow_artifact.py +++ b/tests/test_m4_worker_shadow_artifact.py @@ -83,6 +83,10 @@ def test_worker_shadow_artifact_is_deterministic_narrow_and_self_contained( "name": BUILDER.RUNNER_NAME, "sha256": _sha256(BUILDER.RUNNER.read_bytes()), } + assert descriptor["release"]["wheel"] == { + "name": BUILDER.WHEEL_NAME, + "sha256": BUILDER.EXPECTED_WHEEL_SHA256, + } assert descriptor["container"]["public_ports"] is False assert descriptor["rollback"] == { "durable_worker_action": "none", diff --git a/tests/test_perception_architecture.py b/tests/test_perception_architecture.py index 35e19fc..3536f4d 100644 --- a/tests/test_perception_architecture.py +++ b/tests/test_perception_architecture.py @@ -48,6 +48,7 @@ TEMPORAL_RUNTIME_MODULES = ( "motion.py", "providers.py", "recorded_source.py", + "rolling_map.py", "temporal.py", "temporal_replay.py", "temporal_replay_cli.py", @@ -61,6 +62,7 @@ THREAT_RUNTIME_MODULES = ( "geometry_replay.py", "providers.py", "recorded_source.py", + "rolling_map.py", "temporal_replay.py", "threat.py", "threat_replay.py", diff --git a/tests/test_replay_threat_provider.py b/tests/test_replay_threat_provider.py index 2bf8808..292862c 100644 --- a/tests/test_replay_threat_provider.py +++ b/tests/test_replay_threat_provider.py @@ -28,6 +28,7 @@ from k1link.perception.threat import ( REPOSITORY_ROOT = Path(__file__).resolve().parents[1] PROFILE_PATH = REPOSITORY_ROOT / "config/perception/m4-replay-threat-v2.json" +ROLLING_PROFILE_PATH = REPOSITORY_ROOT / "config/perception/m4-replay-threat-v3.json" class _BodyFrames: @@ -61,7 +62,7 @@ def _obstacle( component_id=component_id, identity_scope="ephemeral", state=state, - ttl_ns=750_000_000, + ttl_ns=(3_000_000_000 if state is TemporalState.RETAINED else 750_000_000), last_hit_ns=current.evidence_time_ns, age_ns=0 if state is TemporalState.CURRENT else 100_000_000, association_basis="test-spatial-support", @@ -307,3 +308,32 @@ def test_semantic_hint_and_ephemeral_component_name_do_not_change_threat_geometr assert values[0].relative_speed_mps == values[1].relative_speed_mps assert values[0].closest_approach_m == values[1].closest_approach_m assert values[0].ttc_seconds == values[1].ttc_seconds + + +def test_retained_map_can_block_but_cannot_claim_clear_or_motion() -> None: + provider = DualEvidenceReplayThreatProvider( + body_frame_resolver=_BodyFrames(), + profile=load_replay_threat_profile(ROLLING_PROFILE_PATH), + ) + retained_in = _obstacle( + "retained-in", + GridCell(6, 0, 0), + motion=MotionState.UNKNOWN, + history=(("frame-000002", 300_000_000, (2.925, 0.225, 0.225)),), + state=TemporalState.RETAINED, + ) + retained_out = _obstacle( + "retained-out", + GridCell(6, 7, 0), + motion=MotionState.UNKNOWN, + history=(("frame-000002", 300_000_000, (2.925, 3.375, 0.225)),), + state=TemporalState.RETAINED, + ) + + values = provider.assess(_map(occupied=(retained_in, retained_out))) + by_id = {item.component_id: item for item in values} + + assert by_id["retained-in"].decision is ThreatDecision.THREAT + assert by_id["retained-in"].relative_speed_mps is None + assert by_id["retained-out"].decision is ThreatDecision.UNKNOWN + assert by_id["retained-out"].corridor_intersection is CorridorIntersection.UNKNOWN diff --git a/tests/test_rolling_local_map.py b/tests/test_rolling_local_map.py new file mode 100644 index 0000000..7de56fb --- /dev/null +++ b/tests/test_rolling_local_map.py @@ -0,0 +1,137 @@ +from __future__ import annotations + +from dataclasses import replace +from pathlib import Path + +from k1link.perception.contracts import ( + ClockBasis, + GridCell, + HistorySample, + ModalityOutcome, + ModalityStatus, + MotionState, + SourceEnvelope, + TemporalObstacle, + TemporalState, + TimestampBundle, +) +from k1link.perception.providers import SourcePacket +from k1link.perception.rolling_map import ( + RollingLocalObstacleMapProvider, + load_rolling_map_profile, +) + +REPOSITORY_ROOT = Path(__file__).resolve().parents[1] +PROFILE_PATH = REPOSITORY_ROOT / "config/perception/m4-rolling-local-map-v1.json" + + +class _Pose: + def pose_values_for_frame( + self, + frame_id: str, + ) -> tuple[tuple[float, float, float], tuple[float, float, float, float]]: + return (0.0, 0.0, 1.25), (0.0, 0.0, 0.0, 1.0) + + +def _status() -> ModalityStatus: + return ModalityStatus(True, ModalityOutcome.AVAILABLE, "test-available") + + +def _packet(sequence: int, seconds: float) -> SourcePacket: + return SourcePacket( + envelope=SourceEnvelope( + source_id="RAVNOVES00", + session_id="20260720T065719Z_viewer_live", + frame_id=f"frame-{sequence:06d}", + sequence=sequence, + timestamps=TimestampBundle( + utc_ns=round(seconds * 1_000_000_000), + monotonic_ns=round(seconds * 1_000_000_000), + source_ns=round(seconds * 1_000_000_000), + clock_basis=ClockBasis.RECORDED_HOST, + ), + source_age_ns=0, + binding_reason="test-source", + calibration_id="test-calibration", + representation_id="registered-map-increment-v1", + image=_status(), + registered_point_increment=_status(), + pose=_status(), + ), + image_payload="image", + registered_point_increment_payload="points", + pose_payload="pose", + ) + + +def _current(packet: SourcePacket) -> TemporalObstacle: + return TemporalObstacle( + component_id=f"temporal-{packet.envelope.sequence:08d}", + identity_scope="ephemeral", + state=TemporalState.CURRENT, + ttl_ns=750_000_000, + last_hit_ns=packet.envelope.timestamps.source_ns, + age_ns=0, + association_basis="new-spatial-hit", + history=( + HistorySample( + frame_id=packet.envelope.frame_id, + evidence_time_ns=packet.envelope.timestamps.source_ns, + centroid_xyz_m=(0.675, 0.225, 0.225), + ), + ), + cells=(GridCell(1, 0, 0), GridCell(2, 0, 0)), + coordinate_frame="map", + last_centroid_xyz_m=(0.675, 0.225, 0.225), + motion=MotionState.UNKNOWN, + motion_confidence=0.0, + motion_reason="motion-not-estimated", + ) + + +def test_registered_increment_is_retained_without_claiming_current_motion() -> None: + provider = RollingLocalObstacleMapProvider( + pose_resolver=_Pose(), + profile=load_rolling_map_profile(PROFILE_PATH), + ) + first = _packet(0, 0.0) + assert provider.update(first, (_current(first),)) == () + + second = _packet(1, 1.0) + retained = provider.update(second, ()) + + assert len(retained) == 1 + assert retained[0].state is TemporalState.RETAINED + assert retained[0].age_ns == 1_000_000_000 + assert retained[0].cells == (GridCell(1, 0, 0), GridCell(2, 0, 0)) + assert retained[0].motion is MotionState.UNKNOWN + assert retained[0].association_basis == "registered-map-increment-retention" + + +def test_current_republication_is_not_duplicated_as_retained_occupancy() -> None: + provider = RollingLocalObstacleMapProvider( + pose_resolver=_Pose(), + profile=load_rolling_map_profile(PROFILE_PATH), + ) + first = _packet(0, 0.0) + provider.update(first, (_current(first),)) + second = _packet(1, 1.0) + + assert provider.update( + second, + (replace(_current(second), component_id="temporal-00000099"),), + ) == () + + +def test_retained_occupancy_expires_only_at_explicit_time_bound() -> None: + provider = RollingLocalObstacleMapProvider( + pose_resolver=_Pose(), + profile=load_rolling_map_profile(PROFILE_PATH), + ) + first = _packet(0, 0.0) + provider.update(first, (_current(first),)) + assert provider.update(_packet(1, 3.0), ()) + assert provider.update(_packet(2, 3.1), ()) == () + snapshot = provider.snapshot() + assert snapshot.time_evicted_cells == 2 + assert snapshot.capacity_evicted_cells == 0 diff --git a/tests/test_temporal_replay_result.py b/tests/test_temporal_replay_result.py index 1e1e46a..860920e 100644 --- a/tests/test_temporal_replay_result.py +++ b/tests/test_temporal_replay_result.py @@ -1,5 +1,6 @@ from __future__ import annotations +import json from pathlib import Path import pytest @@ -15,6 +16,11 @@ RESULT_ROOT = ( / ".runtime/perception-m4/temporal-results" / "m4-temporal-replay-9ed5dcd249ed3bcb81661dd18e2b854a7ffedf3fd2b92b9c994c3c70c34533f2" ) +ROLLING_RESULT_ROOT = ( + REPOSITORY_ROOT + / ".runtime/perception-m4/temporal-results" + / "m4-temporal-replay-b8611526dfcd2b9be9049560d751bbd23a9ad54b7dda8e9dc48a17374d46266e" +) @pytest.fixture(scope="module") @@ -82,3 +88,37 @@ def test_temporal_result_is_digest_bound_to_m4_4_e34_e51_and_e46b( assert set(references) == {"e34", "e46b", "e51"} assert references["e46b"]["independent_truth"] is False assert len(identity["clip_checks"]) == 16 + + +def test_rolling_temporal_result_separates_increment_from_retained_map() -> None: + result = read_temporal_replay_result(ROLLING_RESULT_ROOT) + + assert result.accepted is True + assert result.metrics["rolling_map"] == { + "active_cells_at_end": 587, + "capacity_evicted_cells": 0, + "current_increment_cells": 848868, + "input_frames": 4489, + "local_radius_m": 12.0, + "maximum_cells": 65536, + "maximum_retained_age_ns": 3000000000, + "peak_active_cells": 907, + "peak_retained_components": 35, + "radius_evicted_cells": 6512, + "retained_cell_publications": 1776145, + "retained_component_publications": 69855, + "retention_ns": 3000000000, + "time_evicted_cells": 29620, + "voxel_size_m": 0.45, + } + frames = result.result_root / "frames.jsonl" + with frames.open("rb") as handle: + for sequence, line in enumerate(handle): + if sequence == 1880: + frame = json.loads(line) + break + else: # pragma: no cover - immutable artifact guarantees this branch is unreachable + raise AssertionError("frame 1880 missing") + assert frame["schema_version"] == "missioncore.perception-temporal-replay-frame/v2" + assert len(frame["rolling_retained"]) == 10 + assert all(item["state"] == "retained" for item in frame["rolling_retained"])