SILVER/NODEDC_MISSION_CORE
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

fix(perception): reconstruct rolling occupancy from K1 increments

Browse Source
This commit is contained in:
DCCONSTRUCTIONS
2026-08-05 21:12:47 +03:00
parent c70ad345ea
commit 7aa3ce55c0
16 changed files with 1489 additions and 141 deletions
Download Patch File Download Diff File Expand all files Collapse all files
config/deployment/mission-core-worker-shadow-v1.template.json
+1 -1
View File
@@ -139,7 +139,7 @@
},
"wheel": {
"name": "nodedc_mission_core-0.1.0-py3-none-any.whl",
"sha256": "19d8caf9a522747c461fb3ca30aafe54169959d8bd8e671fa6fc8c0ac107875d"
"sha256": "__WHEEL_SHA256__"
}
},
"rollback": {
config/perception/m4-replay-threat-v3.json
+67
View File
@@ -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
}
}
config/perception/m4-rolling-local-map-v1.json
+38
View File
@@ -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
}
}
scripts/build_m4_worker_shadow_artifact.py
+3 -1
View File
@@ -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:
src/k1link/perception/contracts.py
+24 -4
View File
@@ -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")
src/k1link/perception/rolling_map.py
+443
View File
@@ -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",
]
src/k1link/perception/temporal_replay.py
+197 -39
View File
@@ -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")
src/k1link/perception/threat.py
+115 -60
View File
@@ -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)))))
src/k1link/perception/threat_replay.py
+329 -22
View File
@@ -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")
src/k1link/web/m4_threat_replay_api.py
+9 -3
View File
@@ -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
tests/test_m4_threat_replay_result.py
+49 -10
View File
@@ -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,
tests/test_m4_worker_shadow_artifact.py
+4
View File
@@ -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",
tests/test_perception_architecture.py
+2
View File
@@ -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",
tests/test_replay_threat_provider.py
+31 -1
View File
@@ -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
tests/test_rolling_local_map.py
+137
View File
@@ -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
tests/test_temporal_replay_result.py
+40
View File
@@ -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"])