feat(perception): add diagnostic semantic SLAM replay
This commit is contained in:
DCCONSTRUCTIONS
@@ -0,0 +1,506 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import hashlib
|
||||
import io
|
||||
import json
|
||||
import tarfile
|
||||
from dataclasses import dataclass, replace
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
from PIL import Image
|
||||
|
||||
import k1link.perception.semantic_slam_replay as replay
|
||||
from k1link.perception.contracts import (
|
||||
EvidenceBasis,
|
||||
EvidenceCurrentness,
|
||||
MetricGeometry,
|
||||
ObstacleObservation,
|
||||
)
|
||||
from k1link.perception.geometry import GeometryFrame, RecordedFrameTemporalBinding
|
||||
from k1link.perception.geometry_math import Kb4ProjectionProfile
|
||||
from k1link.perception.geometry_replay import GeometryReplayResult
|
||||
from k1link.perception.semantic_fusion import (
|
||||
SemanticClassDisposition,
|
||||
SemanticEvidenceStatus,
|
||||
)
|
||||
from k1link.perception.threat_replay import ThreatReplayResult
|
||||
|
||||
|
||||
def _canonical_json(value: object) -> bytes:
|
||||
return json.dumps(
|
||||
value,
|
||||
ensure_ascii=False,
|
||||
sort_keys=True,
|
||||
separators=(",", ":"),
|
||||
allow_nan=False,
|
||||
).encode("utf-8")
|
||||
|
||||
|
||||
def _sha256(path: Path) -> str:
|
||||
return hashlib.sha256(path.read_bytes()).hexdigest()
|
||||
|
||||
|
||||
def _write_jsonl(path: Path, rows: list[dict[str, object]]) -> None:
|
||||
path.write_bytes(b"".join(_canonical_json(row) + b"\n" for row in rows))
|
||||
|
||||
|
||||
def _png(labels: np.ndarray) -> bytes:
|
||||
buffer = io.BytesIO()
|
||||
Image.fromarray(labels, mode="L").save(buffer, format="PNG")
|
||||
return buffer.getvalue()
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class _Store:
|
||||
frame: GeometryFrame
|
||||
lidar_delta_ms: float = 4.0
|
||||
pose_delta_ms: float = 2.0
|
||||
|
||||
def frame_for_index(self, frame_index: int) -> GeometryFrame | None:
|
||||
return self.frame if frame_index == 0 else None
|
||||
|
||||
def temporal_binding_for_index(self, frame_index: int) -> RecordedFrameTemporalBinding:
|
||||
return RecordedFrameTemporalBinding(
|
||||
frame_index=frame_index,
|
||||
source_time_ns=(frame_index + 1) * 1_000_000_000,
|
||||
source_available=frame_index == 0,
|
||||
lidar_camera_delta_ms=self.lidar_delta_ms if frame_index == 0 else None,
|
||||
pose_point_delta_ms=self.pose_delta_ms if frame_index == 0 else None,
|
||||
)
|
||||
|
||||
|
||||
def _observation() -> ObstacleObservation:
|
||||
return ObstacleObservation(
|
||||
observation_id="frame-000000:obstacle-0",
|
||||
occupancy_key="frame-000000:obstacle-0",
|
||||
source_id="RAVNOVES00",
|
||||
frame_id="frame-000000",
|
||||
evidence_time_ns=1,
|
||||
basis=EvidenceBasis.FUSED,
|
||||
currentness=EvidenceCurrentness.CURRENT,
|
||||
occupied_support=True,
|
||||
source_point_ids=(0, 1),
|
||||
metric_geometry=MetricGeometry(
|
||||
coordinate_frame="map",
|
||||
centroid_xyz_m=(0.0, 0.0, 1.0),
|
||||
range_m=1.0,
|
||||
covariance_diagonal_m2=(0.0, 0.0, 0.0),
|
||||
),
|
||||
proposal_ids=("proposal-0",),
|
||||
semantic_hint="car",
|
||||
reason_codes=("current-test-support",),
|
||||
)
|
||||
|
||||
|
||||
def _fixture(tmp_path: Path) -> replay._AdmittedInputs:
|
||||
source = tmp_path / "source"
|
||||
source.mkdir()
|
||||
profile_path = source / "profile.json"
|
||||
profile_path.write_text('{"fixture":true}\n', encoding="utf-8")
|
||||
authority = {
|
||||
"ground_truth": False,
|
||||
"physical_live": False,
|
||||
"commands_enabled": False,
|
||||
"actuation_allowed": False,
|
||||
"navigation_or_safety_accepted": False,
|
||||
"semantic_authority": "diagnostic-only",
|
||||
}
|
||||
fusion = {
|
||||
"projection": "factory-kb4-current-increment/v1",
|
||||
"point_index_space": "frame-local-source-point-id/v1",
|
||||
"observation_aggregation": "dominant-labeled-majority-diagnostic/v1",
|
||||
"unprojected_status": "unprojected",
|
||||
"semantic_absence_means_free": False,
|
||||
"semantic_can_create_obstacle": False,
|
||||
"semantic_can_change_identity": False,
|
||||
"semantic_can_change_metric_geometry": False,
|
||||
"semantic_can_change_occupancy": False,
|
||||
"semantic_can_change_motion": False,
|
||||
"semantic_can_change_threat": False,
|
||||
}
|
||||
profile = replay._SemanticSlamProfile(
|
||||
path=profile_path,
|
||||
sha256=_sha256(profile_path),
|
||||
profile_id="fixture-semantic-slam/v1",
|
||||
source_id="RAVNOVES00",
|
||||
session_id="fixture-session",
|
||||
frame_count=2,
|
||||
image_width=4,
|
||||
image_height=4,
|
||||
source_pack_id="fixture-source-pack",
|
||||
source_pack_sha256="1" * 64,
|
||||
calibration_sha256="2" * 64,
|
||||
provider_id="fixture-semantic-provider/v1",
|
||||
model_id="fixture-model",
|
||||
model_revision="fixture-revision",
|
||||
model_weights_sha256="3" * 64,
|
||||
preprocess_id="fixture-preprocess/v1",
|
||||
mask_metadata_schema_version="missioncore.panoptic-frame/v1",
|
||||
mask_payload={
|
||||
"media_type": "image/png",
|
||||
"encoding": "uint8-class-id",
|
||||
"width": 4,
|
||||
"height": 4,
|
||||
"sequence_binding": "sequence-0-to-frame-000001",
|
||||
},
|
||||
provider_role="fixed-control-not-selected-production-provider",
|
||||
classes=(
|
||||
replay._TaxonomyClass(
|
||||
class_id=0,
|
||||
label="outside_valid_fov",
|
||||
disposition=SemanticClassDisposition.AMBIGUOUS,
|
||||
color_rgb=(0, 0, 0),
|
||||
),
|
||||
replay._TaxonomyClass(
|
||||
class_id=4,
|
||||
label="car",
|
||||
disposition=SemanticClassDisposition.LABELED,
|
||||
color_rgb=(0, 0, 142),
|
||||
),
|
||||
),
|
||||
fusion=fusion,
|
||||
temporal_binding={
|
||||
"semantic_to_camera": "exact-sequence-and-session-time",
|
||||
"camera_to_lidar": "accepted-e6-nearest-host-arrival-best-effort",
|
||||
"clock_basis": "recorded-host-monotonic-arrival",
|
||||
"maximum_lidar_camera_delta_ms": 100.0,
|
||||
"maximum_pose_point_delta_ms": 100.0,
|
||||
"physical_synchronization_proven": False,
|
||||
},
|
||||
acceptance={
|
||||
"full_frame_accounting_required": True,
|
||||
"point_accounting_required": True,
|
||||
"observation_binding_required": True,
|
||||
"exact_mask_archive_required": True,
|
||||
"independent_semantic_truth_required_for_provider_promotion": True,
|
||||
},
|
||||
authority=authority,
|
||||
)
|
||||
|
||||
semantic_root = source / "semantic"
|
||||
semantic_root.mkdir()
|
||||
result_json = semantic_root / "result.json"
|
||||
result_json.write_text('{"sealed":"fixture"}\n', encoding="utf-8")
|
||||
masks = []
|
||||
first = np.zeros((4, 4), dtype=np.uint8)
|
||||
first[2, 2] = 4
|
||||
masks.append(_png(first))
|
||||
masks.append(_png(np.zeros((4, 4), dtype=np.uint8)))
|
||||
mask_archive = semantic_root / "masks.tar.gz"
|
||||
with tarfile.open(mask_archive, mode="w:gz") as archive:
|
||||
directory = tarfile.TarInfo("semantic-masks")
|
||||
directory.type = tarfile.DIRTYPE
|
||||
archive.addfile(directory)
|
||||
for sequence, payload in enumerate(masks, start=1):
|
||||
member = tarfile.TarInfo(f"semantic-masks/frame-{sequence:06d}.png")
|
||||
member.size = len(payload)
|
||||
archive.addfile(member, io.BytesIO(payload))
|
||||
semantic_frames = semantic_root / "frames.jsonl"
|
||||
_write_jsonl(
|
||||
semantic_frames,
|
||||
[
|
||||
{
|
||||
"schema_version": "missioncore.panoptic-frame/v1",
|
||||
"frame_index": 0,
|
||||
"sequence": 1,
|
||||
"session_seconds": 1.0,
|
||||
"instances": [],
|
||||
"semantic_classes": [
|
||||
{
|
||||
"id": 4,
|
||||
"label": "car",
|
||||
"pixels": 1,
|
||||
"fraction_of_valid_fov": 0.0625,
|
||||
}
|
||||
],
|
||||
},
|
||||
{
|
||||
"schema_version": "missioncore.panoptic-frame/v1",
|
||||
"frame_index": 1,
|
||||
"sequence": 2,
|
||||
"session_seconds": 2.0,
|
||||
"instances": [],
|
||||
"semantic_classes": [],
|
||||
},
|
||||
],
|
||||
)
|
||||
semantic = replay._SemanticUpstream(
|
||||
result_id="result-" + "4" * 64,
|
||||
result_root=semantic_root,
|
||||
result_manifest_sha256=_sha256(result_json),
|
||||
frames_path=semantic_frames,
|
||||
frames_sha256=_sha256(semantic_frames),
|
||||
masks_path=mask_archive,
|
||||
masks_sha256=_sha256(mask_archive),
|
||||
created_at_utc="2026-08-06T00:00:00.000Z",
|
||||
job_id="fixture-job",
|
||||
input_sha256="5" * 64,
|
||||
source_id="sensor.camera.right",
|
||||
session_id="fixture-session",
|
||||
calibration_sha256="2" * 64,
|
||||
configuration_profile_sha256="6" * 64,
|
||||
model_id="fixture-model",
|
||||
model_revision="fixture-revision",
|
||||
model_weights_sha256="3" * 64,
|
||||
)
|
||||
|
||||
observation = _observation()
|
||||
geometry_root = source / "geometry"
|
||||
geometry_root.mkdir()
|
||||
geometry_frames = geometry_root / "frames.jsonl"
|
||||
_write_jsonl(
|
||||
geometry_frames,
|
||||
[
|
||||
{
|
||||
"schema_version": "missioncore.perception-geometry-replay-frame/v1",
|
||||
"sequence": 0,
|
||||
"frame_id": "frame-000000",
|
||||
"source_available": True,
|
||||
"observations": [observation.to_dict()],
|
||||
},
|
||||
{
|
||||
"schema_version": "missioncore.perception-geometry-replay-frame/v1",
|
||||
"sequence": 1,
|
||||
"frame_id": "frame-000001",
|
||||
"source_available": False,
|
||||
"observations": [],
|
||||
},
|
||||
],
|
||||
)
|
||||
geometry_sha256 = _sha256(geometry_frames)
|
||||
geometry = GeometryReplayResult(
|
||||
result_id="m4-geometry-replay-" + "7" * 64,
|
||||
result_root=geometry_root,
|
||||
accepted=True,
|
||||
metrics={"frames": {"total": 2}},
|
||||
report={},
|
||||
manifest={"identity": {"frames_sha256": geometry_sha256}},
|
||||
)
|
||||
|
||||
threat_root = source / "threat"
|
||||
threat_root.mkdir()
|
||||
threat_frames = threat_root / "frames.jsonl"
|
||||
_write_jsonl(threat_frames, [{"decision": "unchanged"}])
|
||||
threat_sha256 = _sha256(threat_frames)
|
||||
threat = ThreatReplayResult(
|
||||
result_id="m4-threat-replay-" + "8" * 64,
|
||||
result_root=threat_root,
|
||||
accepted=True,
|
||||
metrics={"frames": {"total": 2}},
|
||||
report={},
|
||||
manifest={"identity": {"frames_sha256": threat_sha256}},
|
||||
)
|
||||
|
||||
transform = np.eye(4, dtype=np.float64)
|
||||
frame = GeometryFrame(
|
||||
frame_index=0,
|
||||
points_map=np.asarray(((0.0, 0.0, 1.0), (100.0, 0.0, 1.0)), dtype=np.float64),
|
||||
point_class=np.zeros(2, dtype=np.uint8),
|
||||
sensor_position_map=np.zeros(3, dtype=np.float64),
|
||||
sensor_orientation_xyzw=np.asarray((0.0, 0.0, 0.0, 1.0), dtype=np.float64),
|
||||
projection=Kb4ProjectionProfile(
|
||||
width=4,
|
||||
height=4,
|
||||
intrinsic_fx_fy_cx_cy=(2.0, 2.0, 2.0, 2.0),
|
||||
distortion_kb4=(0.0, 0.0, 0.0, 0.0),
|
||||
t_camera_from_lidar=transform,
|
||||
),
|
||||
surface_valid=True,
|
||||
)
|
||||
return replay._AdmittedInputs(
|
||||
profile=profile,
|
||||
semantic=semantic,
|
||||
geometry=geometry,
|
||||
threat=threat,
|
||||
store=_Store(frame), # type: ignore[arg-type]
|
||||
geometry_frames_path=geometry_frames,
|
||||
geometry_frames_sha256=geometry_sha256,
|
||||
threat_frames_path=threat_frames,
|
||||
threat_frames_sha256=threat_sha256,
|
||||
)
|
||||
|
||||
|
||||
def _build(
|
||||
tmp_path: Path,
|
||||
monkeypatch: pytest.MonkeyPatch,
|
||||
) -> tuple[replay.SemanticSlamReplayResult, replay._AdmittedInputs]:
|
||||
admitted = _fixture(tmp_path)
|
||||
monkeypatch.setattr(replay, "_admit_inputs", lambda **_kwargs: admitted)
|
||||
result = replay.build_semantic_slam_replay(
|
||||
repository_root=tmp_path,
|
||||
semantic_result_root=tmp_path,
|
||||
threat_result_root=tmp_path,
|
||||
geometry_result_root=tmp_path,
|
||||
output_root=tmp_path / "output",
|
||||
)
|
||||
return result, admitted
|
||||
|
||||
|
||||
def test_builder_is_idempotent_and_preserves_geometry_and_threat_ledgers(
|
||||
tmp_path: Path,
|
||||
monkeypatch: pytest.MonkeyPatch,
|
||||
) -> None:
|
||||
admitted = _fixture(tmp_path)
|
||||
geometry_before = admitted.geometry_frames_path.read_bytes()
|
||||
threat_before = admitted.threat_frames_path.read_bytes()
|
||||
monkeypatch.setattr(replay, "_admit_inputs", lambda **_kwargs: admitted)
|
||||
arguments = {
|
||||
"repository_root": tmp_path,
|
||||
"semantic_result_root": tmp_path,
|
||||
"threat_result_root": tmp_path,
|
||||
"geometry_result_root": tmp_path,
|
||||
"output_root": tmp_path / "output",
|
||||
}
|
||||
first = replay.build_semantic_slam_replay(**arguments)
|
||||
manifest_before = (first.result_root / replay.SEMANTIC_SLAM_MANIFEST_NAME).read_bytes()
|
||||
second = replay.build_semantic_slam_replay(**arguments)
|
||||
|
||||
assert second.result_id == first.result_id
|
||||
assert (second.result_root / replay.SEMANTIC_SLAM_MANIFEST_NAME).read_bytes() == manifest_before
|
||||
assert admitted.geometry_frames_path.read_bytes() == geometry_before
|
||||
assert admitted.threat_frames_path.read_bytes() == threat_before
|
||||
identity = first.manifest["identity"]
|
||||
assert identity["geometry_frames_sha256"] == hashlib.sha256(geometry_before).hexdigest()
|
||||
assert identity["base_m4_frames_sha256"] == hashlib.sha256(threat_before).hexdigest()
|
||||
|
||||
|
||||
def test_unprojected_source_point_is_uint8_zero_with_explicit_status(
|
||||
tmp_path: Path,
|
||||
monkeypatch: pytest.MonkeyPatch,
|
||||
) -> None:
|
||||
result, _ = _build(tmp_path, monkeypatch)
|
||||
with np.load(
|
||||
result.result_root / replay.SEMANTIC_SLAM_POINTS_NAME,
|
||||
allow_pickle=False,
|
||||
) as archive:
|
||||
assert archive["point_labels"].dtype == np.uint8
|
||||
assert archive["point_labels"].tolist() == [4, 0]
|
||||
assert archive["point_status_codes"].tolist() == [
|
||||
int(SemanticEvidenceStatus.LABELED),
|
||||
int(SemanticEvidenceStatus.UNPROJECTED),
|
||||
]
|
||||
assert archive["point_projected"].tolist() == [1, 0]
|
||||
|
||||
rows = [
|
||||
json.loads(line)
|
||||
for line in (result.result_root / replay.SEMANTIC_SLAM_OBSERVATIONS_NAME)
|
||||
.read_text("utf-8")
|
||||
.splitlines()
|
||||
]
|
||||
assert rows[0]["observations"][0]["observation_id"] == _observation().observation_id
|
||||
assert rows[0]["observations"][0]["status"] == "labeled"
|
||||
assert rows[0]["observations"][0]["unprojected_point_count"] == 1
|
||||
assert "threat" not in rows[0]["observations"][0]
|
||||
assert rows[0]["source_time_ns"] == 1_000_000_000
|
||||
assert rows[0]["temporal_binding"] == {
|
||||
"semantic_to_camera": "exact-sequence-and-session-time",
|
||||
"camera_to_lidar": "accepted-e6-nearest-host-arrival-best-effort",
|
||||
"lidar_camera_delta_ms": 4.0,
|
||||
"pose_point_delta_ms": 2.0,
|
||||
"physical_synchronization_proven": False,
|
||||
}
|
||||
assert rows[1]["temporal_binding"]["lidar_camera_delta_ms"] is None
|
||||
|
||||
|
||||
def test_reader_rejects_tampered_point_artifact(
|
||||
tmp_path: Path,
|
||||
monkeypatch: pytest.MonkeyPatch,
|
||||
) -> None:
|
||||
result, _ = _build(tmp_path, monkeypatch)
|
||||
points = result.result_root / replay.SEMANTIC_SLAM_POINTS_NAME
|
||||
points.write_bytes(points.read_bytes() + b"tamper")
|
||||
|
||||
with pytest.raises(replay.SemanticSlamReplayError, match="digest changed"):
|
||||
replay.read_semantic_slam_replay_result(result.result_root)
|
||||
|
||||
|
||||
def test_builder_rejects_semantic_and_source_pack_session_time_mismatch(
|
||||
tmp_path: Path,
|
||||
monkeypatch: pytest.MonkeyPatch,
|
||||
) -> None:
|
||||
admitted = _fixture(tmp_path)
|
||||
rows = [
|
||||
json.loads(line) for line in admitted.semantic.frames_path.read_text("utf-8").splitlines()
|
||||
]
|
||||
rows[1]["session_seconds"] = 2.001
|
||||
_write_jsonl(admitted.semantic.frames_path, rows)
|
||||
semantic = replace(
|
||||
admitted.semantic,
|
||||
frames_sha256=_sha256(admitted.semantic.frames_path),
|
||||
)
|
||||
monkeypatch.setattr(
|
||||
replay,
|
||||
"_admit_inputs",
|
||||
lambda **_kwargs: replace(admitted, semantic=semantic),
|
||||
)
|
||||
|
||||
with pytest.raises(replay.SemanticSlamReplayError, match="session time disagree"):
|
||||
replay.build_semantic_slam_replay(
|
||||
repository_root=tmp_path,
|
||||
semantic_result_root=tmp_path,
|
||||
threat_result_root=tmp_path,
|
||||
geometry_result_root=tmp_path,
|
||||
output_root=tmp_path / "output",
|
||||
)
|
||||
|
||||
|
||||
def test_builder_rejects_best_effort_delta_outside_admitted_bound(
|
||||
tmp_path: Path,
|
||||
monkeypatch: pytest.MonkeyPatch,
|
||||
) -> None:
|
||||
admitted = _fixture(tmp_path)
|
||||
frame = admitted.store.frame_for_index(0)
|
||||
assert frame is not None
|
||||
monkeypatch.setattr(
|
||||
replay,
|
||||
"_admit_inputs",
|
||||
lambda **_kwargs: replace(
|
||||
admitted,
|
||||
store=_Store(frame, lidar_delta_ms=100.001), # type: ignore[arg-type]
|
||||
),
|
||||
)
|
||||
|
||||
with pytest.raises(replay.SemanticSlamReplayError, match="delta exceeds"):
|
||||
replay.build_semantic_slam_replay(
|
||||
repository_root=tmp_path,
|
||||
semantic_result_root=tmp_path,
|
||||
threat_result_root=tmp_path,
|
||||
geometry_result_root=tmp_path,
|
||||
output_root=tmp_path / "output",
|
||||
)
|
||||
|
||||
|
||||
def test_reader_rejects_leaf_result_symlink(
|
||||
tmp_path: Path,
|
||||
monkeypatch: pytest.MonkeyPatch,
|
||||
) -> None:
|
||||
result, _ = _build(tmp_path, monkeypatch)
|
||||
alias_parent = tmp_path / "alias"
|
||||
alias_parent.mkdir()
|
||||
alias = alias_parent / result.result_id
|
||||
alias.symlink_to(result.result_root, target_is_directory=True)
|
||||
|
||||
with pytest.raises(replay.SemanticSlamReplayError, match="result root is invalid"):
|
||||
replay.read_semantic_slam_replay_result(alias)
|
||||
|
||||
|
||||
def test_builder_rejects_existing_destination_symlink(
|
||||
tmp_path: Path,
|
||||
monkeypatch: pytest.MonkeyPatch,
|
||||
) -> None:
|
||||
result, admitted = _build(tmp_path, monkeypatch)
|
||||
monkeypatch.setattr(replay, "_admit_inputs", lambda **_kwargs: admitted)
|
||||
second_output = tmp_path / "second-output"
|
||||
second_output.mkdir()
|
||||
(second_output / result.result_id).symlink_to(result.result_root, target_is_directory=True)
|
||||
|
||||
with pytest.raises(replay.SemanticSlamReplayError, match="destination cannot be a symlink"):
|
||||
replay.build_semantic_slam_replay(
|
||||
repository_root=tmp_path,
|
||||
semantic_result_root=tmp_path,
|
||||
threat_result_root=tmp_path,
|
||||
geometry_result_root=tmp_path,
|
||||
output_root=second_output,
|
||||
)
|
||||
Reference in New Issue
Block a user