From 7d1a70d8e0ebab129d7c2240e0f47095d7f72c4c Mon Sep 17 00:00:00 2001 From: DCCONSTRUCTIONS Date: Thu, 30 Jul 2026 11:37:57 +0300 Subject: [PATCH] feat(perception): qualify lossless lidar observations --- docs/01_IMPLEMENTATION_PLAN.md | 5 + docs/13_LIDAR_WORKER_PRODUCT_AND_ROADMAP.md | 58 +- ...lay-and-diagnostic-observation-boundary.md | 91 ++ .../perception/LAB_E51_REPORT_2026-07-30.md | 133 +++ .../e51_motion_semantic_profile.json | 47 + .../run_e51_motion_semantic_qualification.py | 44 + .../perception/run_k1_local_surface.py | 16 +- src/k1link/compute/__init__.py | 22 + .../e51_motion_semantic_qualification.py | 940 ++++++++++++++++++ src/k1link/compute/lidar_local_surface.py | 247 ++++- src/k1link/compute/lidar_replay.py | 237 +++-- .../test_e51_motion_semantic_qualification.py | 112 +++ tests/test_lidar_replay.py | 41 +- 13 files changed, 1874 insertions(+), 119 deletions(-) create mode 100644 docs/adr/0034-lossless-replay-and-diagnostic-observation-boundary.md create mode 100644 experiments/perception/LAB_E51_REPORT_2026-07-30.md create mode 100644 experiments/perception/e51_motion_semantic_profile.json create mode 100644 experiments/perception/run_e51_motion_semantic_qualification.py create mode 100644 src/k1link/compute/e51_motion_semantic_qualification.py create mode 100644 tests/test_e51_motion_semantic_qualification.py diff --git a/docs/01_IMPLEMENTATION_PLAN.md b/docs/01_IMPLEMENTATION_PLAN.md index 097dadd..bc63e69 100644 --- a/docs/01_IMPLEMENTATION_PLAN.md +++ b/docs/01_IMPLEMENTATION_PLAN.md @@ -19,6 +19,8 @@ Each gate produces evidence and an explicit GO, PAUSE or BLOCKED result. | Detector candidate comparison | FROZEN BEFORE TRUTH — E47 freezes raw-KB4 and fixed-valid-FOV-fill predictions from the same exact Mask R-CNN checkpoint. No accuracy result or winner exists before the E46 truth seal. | | Detector truth/evaluation executors | DEFERRED BY OWNER, READY, NOT RUN — E48 fail-closed review/adjudication sealing and separate post-seal E49 scoring are implemented. Neither result exists because real independent reviews are absent. The deferral does not convert E37–E40 into blind truth and does not block R2/R4 operational work. | | RAVNOVES00 recorded AI publication | GO — after a real backend restart, the admitted E10 overlay `e10-integrated-perception-36964643f1c0727a434671e3cddc2e76c536b9c28b8c56845f18941ae791c39a` recovered from its sealed cache without model execution or source revalidation. Recording `f83e29dc-5d25-42a5-824e-228cdbb83d21` streamed `69,227,327` bytes with verified SHA-256 `c2bb73fca8bb616a3929df236c6dc63d071b6466a6ca57d06487ef18ed985b57`; a warm localhost request completed in `0.391 s`, and its steady-state server RSS delta was `48 KiB`. | +| L2.6 lossless replay parity | GO — replay-pack-v2 exact equivalence passed for all 4,570 native LiDAR frames, 4,598 pose frames and 10,751,258 points. The same local-surface algorithm produced 4,570/4,570 valid frames with 20.6421 ms pose-age p95 while retaining separate v2 identity, RGBI/intensity and host-time provenance. | +| L2.6 motion/semantic qualification | GO (diagnostic only) — E51 processed 4,489/4,489 E32/E34 frames, preserved all 2,119,302 current occupied point rows, measured 0.4148 ms frame p95 and 0.2969 MiB peak-RSS growth, and emitted explicit freshness/conflict/proximity/motion evidence. Dynamic class, collision state, free space, command, navigation and safety authority remain unavailable. | | Product interface | DEFERRED — no new windows, page anatomy or design changes are part of this stabilization increment. | The governing decision is @@ -26,6 +28,9 @@ The governing decision is Historical E37–E40 artifacts remain immutable; only the claims made from them change. +The L2.6 replay and observation decisions are fixed by +[`ADR 0034`](adr/0034-lossless-replay-and-diagnostic-observation-boundary.md). + ## Earlier checkpoint — 2026-07-24 | Stage | Result | diff --git a/docs/13_LIDAR_WORKER_PRODUCT_AND_ROADMAP.md b/docs/13_LIDAR_WORKER_PRODUCT_AND_ROADMAP.md index 7081ac8..ca92977 100644 --- a/docs/13_LIDAR_WORKER_PRODUCT_AND_ROADMAP.md +++ b/docs/13_LIDAR_WORKER_PRODUCT_AND_ROADMAP.md @@ -10,7 +10,8 @@ parallel geometry-only replay implemented; E30–E35 source-scoped qualification accepted; RAVNOVES00 reference-source product maturation active; E36 transfer preregistered and deferred by ADR 0030/0032; E41 methodology boundary, E42 metamorphic checks, E44 amplification audit and E50 exact-content reference -index complete +index complete; E51 motion/proximity/semantic derivative and full lossless +replay-pack-v2 local-surface parity accepted Scope: passively received real-time K1 point/pose evidence, immutable replay and future live shadow processing Explicitly out of scope: K1 firmware modification, a new onboard exporter, new @@ -395,7 +396,7 @@ Dataset expansion is no longer the next gate. - [x] Bind the immutable `RAVNOVES00` E10 source by pack identity and artifact hash without copying or rewriting the source generation. -- [ ] Mirror the same accepted profile over replay-pack-v2 evidence while +- [x] Mirror the same accepted profile over replay-pack-v2 evidence while preserving its separate identity and field-retention contract. - [x] Reject stale pose binding and publish pose-binding age explicitly; keep map-native processing honest instead of claiming that pose inversion @@ -411,9 +412,12 @@ Dataset expansion is no longer the next gate. Do not infer `free` merely because a mapped point is absent. - [x] Leave the immutable persistent reconstruction untouched by the local derivative. -- [ ] Add recent-collision and dynamic-observation layers as separate - derivatives with independent decay and provenance. -- [ ] Reuse the accepted camera-to-LiDAR projection as an optional semantic +- [x] Add a separate diagnostic motion-observation and near-occupied proximity + derivative with temporal freshness, source provenance and no persistent-map + mutation. +- [ ] Admit a `recent-collision` state only after vehicle-body and LiDAR-mount + geometry are bound; proximity is not collision truth. +- [x] Reuse the accepted camera-to-LiDAR projection as an optional semantic layer with source, confidence, freshness and conflict fields. - [x] Qualify next-frame prediction and temporal stability with the evaluated frame excluded from prediction input. @@ -423,7 +427,7 @@ Dataset expansion is no longer the next gate. - [x] Preserve the prior prediction plane and point/cell-aligned residual evidence so a selected tail can be explained spatially instead of only by an aggregate p95. -- [ ] Complete the remaining qualification report with per-frame latency, +- [x] Complete the remaining qualification report with per-frame latency, point age, obstacle preservation and memory growth. - [x] Replay the same profile through a bounded latest-wins shadow queue at the recorded 1× source rate; no K1 command, navigation or safety authority is @@ -455,6 +459,30 @@ distributions are: derived sensor-to-surface height `1.2816 m` p50, roughness These values describe this recording only; they are not calibration, ground truth or a navigation gate. +The lossless replay-pack-v2 path is now qualified independently of that +camera-aligned E10 generation. Its immutable pack +`lidar-replay-pack-8fc0fb418578b8ee2ac88d502d2acbc63ae533437a9da14f1a9f9d8916f613ce` +retains all `4,570` native LiDAR frames, `4,598` pose frames, `10,751,258` +points, raw RGBI/intensity and exact host timing. Source-to-pack equivalence +passed with zero array mismatches and `100%` pose coverage. The higher frame +and point counts are expected: replay-pack-v2 is native LiDAR cadence, whereas +E10 contains only camera-aligned available slices. + +The same local-surface parameters produced immutable model +`k1-local-surface-61a0307497b1e9b32d5521f59aa853bce22d81def7ca4d3fc81273a4162e6d22`. +All `4,570/4,570` frames are valid, with zero stale-pose, insufficient-surface +or fit-failure results. Pose-binding age is `20.6421 ms` p95. Prediction has +`4,569` prior-only samples, `0.04119 m` residual p50 and `0.07983 m` p95. +The source identity remains replay-pack-v2 and intensity remains available; +the accepted E10 artifact is neither rewritten nor relabelled. + +The v2 builder uses a bounded two-pass ingest. It counts and freezes scalar +frame contracts first, then fills preallocated numeric arrays one decoded +frame at a time. The strict reader materializes each compressed retained array +once before repeated validation. On the full capture the old repeated-NPZ +access was rejected after it demonstrated unbounded CPU amplification; the +accepted path completed exact equivalence without Docker or parallel workers. + The L2.6b qualification scores each available frame against a local plane built only from the preceding TTL window; the frame being scored is excluded from the prediction input. It produced `3,927` independent next-frame samples. The @@ -563,6 +591,24 @@ health. Physical K1 execution is deferred until hardware-clock, field-network or acquisition behavior is the test subject. Commands, free-space, navigation and safety authority remain disabled. +LAB E51 consumes accepted E32/E34 evidence without modifying it and publishes +only bounded diagnostic signals. It processed `4,489/4,489` frames with zero +map-frame jump candidates. The derivative observed exactly the same +`2,119,302` current occupied point rows as E34. Frame processing was +`0.4148 ms` p95; process peak-RSS growth was `0.2969 MiB`. Accepted E10 point +age was `70.7886 ms` p95 from LiDAR to camera and `21.8545 ms` p95 from pose +to LiDAR. + +E51 emitted `22,885` motion candidates, `2,430` current near-occupied +proximity candidates, `15,604` camera-owned semantic signals and `90` +explicit conflicts. A semantic numeric-confidence field is present but marked +unavailable because the upstream E32 contract has no admitted numeric +confidence. Held evidence cannot publish current proximity. Map-frame jump +candidates cannot publish motion. Dynamic class and collision state remain +unavailable, and every signal carries false command/navigation/safety +authority. The accepted result is +`e51-motion-semantic-1abb7eb9940608fc5af95a1f318cadfbc42ac2412a8662b6622e000e03da1555`. + Exit: one immutable K1 session yields both a persistent reconstruction and a bounded local world state without hard-coded terrain height or scanner-side changes. diff --git a/docs/adr/0034-lossless-replay-and-diagnostic-observation-boundary.md b/docs/adr/0034-lossless-replay-and-diagnostic-observation-boundary.md new file mode 100644 index 0000000..1064eb9 --- /dev/null +++ b/docs/adr/0034-lossless-replay-and-diagnostic-observation-boundary.md @@ -0,0 +1,91 @@ +# ADR 0034: Lossless replay and diagnostic observation boundary + +Date: 2026-07-30 +Status: accepted and implemented + +## Decision + +The accepted K1 local-surface algorithm consumes one normalized, read-only +source view. Two immutable source schemas are admitted: + +- camera-aligned `missioncore.e10-lidar-replay-pack/v1`; +- native-cadence, field-retaining `missioncore.lidar-replay-pack/v2`. + +The normalized view carries exact source pack identity, artifact SHA-256, +session identity, representation, schema, native point offsets, map-frame XYZ, +host-monotonic timing and nearest-pose age. A replay-pack-v2 source additionally +retains RGBI/intensity provenance. The derivative never rewrites or disguises +one source as the other. + +The lossless v2 builder uses two bounded passes over raw capture evidence. The +first pass freezes frame counts and scalar header/timing contracts. The second +pass preallocates and fills numeric arrays one decoded frame at a time, failing +closed if the source differs between passes. The strict reader decompresses +each NPZ member once before repeated integrity, field and logical-content +validation. Repeated on-demand decompression inside a frame loop is forbidden. + +## Diagnostic observation derivative + +E51 is separate from the persistent reconstruction and the E34 short-TTL +occupied/unknown layer. It may publish: + +- temporal motion candidates derived from a bounded centroid history; +- current near-occupied proximity candidates from admitted E32 range; +- camera-owned semantics and semantic provenance; +- explicit current/held freshness and evidence conflict. + +It may not publish: + +- a dynamic object class; +- a collision state; +- free space from missing points; +- numeric semantic confidence when the upstream contract has none; +- command, navigation or safety authority. + +A map-frame jump candidate rejects motion publication. Held evidence cannot +publish current proximity. Collision remains unavailable until the physical +vehicle body and LiDAR mount/extrinsic geometry are separately bound and +qualified. + +## Resource boundary + +All full-session Mac execution is sequential. The current 14-inch 2023 MacBook +Pro has 18 GiB RAM; replay build, strict validation, local-surface build and +test execution are never intentionally overlapped. Docker is not part of this +path. Port `8000` remains the canonical running Mission Core service. + +## Accepted evidence + +Full native replay: + +```text +lidar-replay-pack-8fc0fb418578b8ee2ac88d502d2acbc63ae533437a9da14f1a9f9d8916f613ce +``` + +- 4,570 LiDAR frames; +- 4,598 pose frames; +- 10,751,258 points; +- exact live/replay equivalence passed; +- pose coverage 100%. + +Full v2 local surface: + +```text +k1-local-surface-61a0307497b1e9b32d5521f59aa853bce22d81def7ca4d3fc81273a4162e6d22 +``` + +- 4,570/4,570 valid; +- zero stale-pose, insufficient-surface or fit-failure frames; +- pose-binding age 20.6421 ms p95. + +E51: + +```text +e51-motion-semantic-1abb7eb9940608fc5af95a1f318cadfbc42ac2412a8662b6622e000e03da1555 +``` + +- 4,489/4,489 frames; +- 2,119,302/2,119,302 current occupied point rows preserved; +- 0.4148 ms frame-processing p95; +- 0.2969 MiB process peak-RSS growth; +- all acceptance checks passed. diff --git a/experiments/perception/LAB_E51_REPORT_2026-07-30.md b/experiments/perception/LAB_E51_REPORT_2026-07-30.md new file mode 100644 index 0000000..5e3fced --- /dev/null +++ b/experiments/perception/LAB_E51_REPORT_2026-07-30.md @@ -0,0 +1,133 @@ +# LAB E51 · lossless replay parity and diagnostic observation layer + +Date: 2026-07-30 + +Status: accepted diagnostic derivative; no production motion, collision, +navigation or safety authority + +## Goal + +Close the source-contract and qualification gaps that remained in L2.6: + +1. run the accepted local-surface algorithm over native + `lidar-replay-pack/v2` without losing its identity or retained fields; +2. publish motion, proximity and camera-semantic evidence as a separate + derivative rather than mutating the persistent map; +3. measure per-frame latency, accepted point age, obstacle preservation and + process memory growth over the complete accepted RAVNOVES00 source. + +No UI, K1 command, firmware behavior or physical acquisition path changed. + +## Immutable inputs + +- E10 LiDAR: + `e10-lidar-pack-576c994a6c814e2592dd6240ace3902a5db94843312c759a73ba0c9166157d2b`; +- E32 track geometry: + `e32-track-geometry-a14ca0e7fb3850ca0dfa3c41634e1b490a2d58ab74d101afc6d6921fbdb0e6fd`; +- E34 temporal occupied/unknown: + `e34-temporal-occupied-8d9abb3f2cc072cfdbb16cc4e55798e05c35a0abe0b8f691096770e091573a73`; +- raw session: + `20260720T065719Z_viewer_live` (`RAVNOVES00`). + +All upstream artifact hashes were verified before and after E51. + +## Lossless replay-pack-v2 + +Accepted pack: + +```text +lidar-replay-pack-8fc0fb418578b8ee2ac88d502d2acbc63ae533437a9da14f1a9f9d8916f613ce +``` + +| Measurement | Result | +| --- | ---: | +| Native LiDAR frames | 4,570 | +| Pose frames | 4,598 | +| Retained points | 10,751,258 | +| Pose coverage | 100% | +| Exact array mismatches | 0 | +| Equivalence | passed | + +The pack contains more frames and points than E10 because it retains native +LiDAR cadence rather than only camera-aligned slices. Raw XYZ, scaled map XYZ, +RGBI, low-byte intensity, device header fields, capture sequence and exact host +epoch/monotonic time remain distinct retained fields. + +The initial full-scale reader attempt exposed repeated NPZ decompression inside +the XYZ validation loop. That attempt was stopped before acceptance. The +implemented reader now materializes every compressed member once. The builder +uses a bounded two-pass source scan and preallocated arrays; it does not retain +all decoded point objects. + +## Local-surface parity + +Accepted v2 model: + +```text +k1-local-surface-61a0307497b1e9b32d5521f59aa853bce22d81def7ca4d3fc81273a4162e6d22 +``` + +| Measurement | E10 camera-aligned | v2 native cadence | +| --- | ---: | ---: | +| Source frames | 4,489 | 4,570 | +| Available/valid | 3,928/3,928 | 4,570/4,570 | +| Pose stale | 0 | 0 | +| Fit failed | 0 | 0 | +| Pose age p95 | 21.8545 ms | 20.6421 ms | +| Prediction samples | 3,927 | 4,569 | +| Prediction residual p50 | 0.04065 m | 0.04119 m | +| Prediction residual p95 | 0.07304 m | 0.07983 m | + +Rolling surface, classification, pose-binding and temporal parameters are +identical. The input representation is not: v2 remains separately identified +and reports intensity availability and complete field retention. + +## E51 method + +For every accepted E32/E34 frame, E51: + +- carries current/held freshness and E32 evidence conflict; +- derives a motion candidate only from a bounded E34 centroid history; +- suppresses motion on a map-frame jump candidate; +- derives proximity only from a current hit-backed E32 range; +- carries camera-owned semantic value or held E34 semantic provenance; +- publishes numeric confidence as explicitly unavailable; +- preserves E34 current occupied rows without rewriting any cell or point; +- leaves collision unavailable because vehicle-body and LiDAR-mount geometry + are not bound. + +The result contains at most 25 signals in any frame against a frozen maximum +of 256. + +## Accepted E51 result + +```text +e51-motion-semantic-1abb7eb9940608fc5af95a1f318cadfbc42ac2412a8662b6622e000e03da1555 +``` + +| Measurement | Result | +| --- | ---: | +| Frames processed | 4,489/4,489 | +| Map-frame jump candidates | 0 | +| Current occupied rows | 2,119,302/2,119,302 exact | +| Frame processing p50 / p95 / max | 0.2358 / 0.4148 / 8.0526 ms | +| Process peak-RSS growth | 0.2969 MiB | +| LiDAR-to-camera point age p95 / max | 70.7886 / 99.5738 ms | +| Pose-to-LiDAR age p95 / max | 21.8545 / 73.7976 ms | +| Motion candidates | 22,885 | +| Proximity candidates | 2,430 | +| Semantic signals | 15,604 | +| Explicit conflicts | 90 | + +All acceptance checks passed. + +## Decision + +The lossless v2 source path, local-surface parity and bounded diagnostic +motion/proximity/semantic derivative are accepted for replay evidence. + +This result does not admit a dynamic class or collision truth. The next +geometry gate is a versioned vehicle body plus LiDAR mount/extrinsic contract. +Only after that gate may near-occupied evidence be evaluated as +`recent-collision`. LiDAR-native 3D detection remains deferred behind the +remaining L2.6 physical/geometry boundary. diff --git a/experiments/perception/e51_motion_semantic_profile.json b/experiments/perception/e51_motion_semantic_profile.json new file mode 100644 index 0000000..f8cc211 --- /dev/null +++ b/experiments/perception/e51_motion_semantic_profile.json @@ -0,0 +1,47 @@ +{ + "schema_version": "missioncore.e51-motion-semantic-profile/v1", + "profile_id": "e51-motion-proximity-semantic-qualification/v1", + "expected": { + "e32_result_id": "e32-track-geometry-a14ca0e7fb3850ca0dfa3c41634e1b490a2d58ab74d101afc6d6921fbdb0e6fd", + "e34_result_id": "e34-temporal-occupied-8d9abb3f2cc072cfdbb16cc4e55798e05c35a0abe0b8f691096770e091573a73", + "source_pack_id": "e10-lidar-pack-576c994a6c814e2592dd6240ace3902a5db94843312c759a73ba0c9166157d2b" + }, + "motion": { + "minimum_observations": 3, + "minimum_span_seconds": 0.2, + "minimum_displacement_m": 0.25, + "minimum_speed_mps": 0.4, + "maximum_speed_mps": 20.0, + "classification": "diagnostic-motion-candidate", + "dynamic_class_available": false, + "reject_map_frame_jump_candidates": true + }, + "proximity": { + "threshold_m": 2.0, + "classification": "diagnostic-near-occupied-candidate", + "collision_state_available": false, + "reason": "vehicle-body-and-lidar-mount-geometry-not-bound" + }, + "semantic": { + "camera_owns_semantics": true, + "numeric_confidence_available": false, + "require_explicit_freshness": true, + "require_explicit_conflict": true + }, + "acceptance": { + "maximum_signals_per_frame": 256, + "maximum_latency_p95_ms": 20.0, + "maximum_rss_growth_mib": 256.0, + "maximum_lidar_camera_age_p95_ms": 100.0, + "maximum_pose_age_p95_ms": 100.0, + "require_exact_current_obstacle_row_preservation": true, + "require_complete_frame_accounting": true, + "require_no_free_space_publication": true, + "require_no_dynamic_or_collision_authority": true, + "require_upstream_immutability": true + }, + "authority": { + "commands_enabled": false, + "navigation_or_safety_accepted": false + } +} diff --git a/experiments/perception/run_e51_motion_semantic_qualification.py b/experiments/perception/run_e51_motion_semantic_qualification.py new file mode 100644 index 0000000..a2fcaa2 --- /dev/null +++ b/experiments/perception/run_e51_motion_semantic_qualification.py @@ -0,0 +1,44 @@ +#!/usr/bin/env python3 +"""Build the immutable E51 motion/proximity/semantic qualification.""" + +from __future__ import annotations + +import argparse +import json +from pathlib import Path + +from k1link.compute import build_e51_motion_semantic_qualification + + +def main() -> int: + parser = argparse.ArgumentParser() + parser.add_argument("--e32-result", type=Path, required=True) + parser.add_argument("--e34-result", type=Path, required=True) + parser.add_argument("--e10-source", type=Path, required=True) + parser.add_argument("--profile", type=Path, required=True) + parser.add_argument("--output-root", type=Path, required=True) + args = parser.parse_args() + result = build_e51_motion_semantic_qualification( + e32_result_root=args.e32_result, + e34_result_root=args.e34_result, + e10_source_root=args.e10_source, + profile_path=args.profile, + output_root=args.output_root, + ) + print( + json.dumps( + { + "result_id": result.result_id, + "result_root": str(result.result_root), + "accepted": result.accepted, + "metrics": result.report["metrics"], + }, + ensure_ascii=False, + sort_keys=True, + ) + ) + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/experiments/perception/run_k1_local_surface.py b/experiments/perception/run_k1_local_surface.py index bb34ba6..a36fc09 100644 --- a/experiments/perception/run_k1_local_surface.py +++ b/experiments/perception/run_k1_local_surface.py @@ -6,9 +6,12 @@ import json from pathlib import Path from k1link.compute import ( + E10_LIDAR_PACK_SCHEMA, + LIDAR_REPLAY_PACK_SCHEMA, E10LidarFieldSource, K1LocalSurfaceProfile, K1LocalSurfaceV1, + LidarReplayPackV2, build_k1_local_surface, ) @@ -43,7 +46,18 @@ def main() -> int: obstacle_min_height_m=arguments.obstacle_min_height_m, obstacle_max_height_m=arguments.obstacle_max_height_m, ) - source = E10LidarFieldSource(arguments.source_pack) + manifest = json.loads( + (arguments.source_pack / "manifest.json").read_text(encoding="utf-8") + ) + schema_version = manifest.get("schema_version") + source: E10LidarFieldSource | LidarReplayPackV2 + if schema_version == E10_LIDAR_PACK_SCHEMA: + source = E10LidarFieldSource(arguments.source_pack) + elif schema_version == LIDAR_REPLAY_PACK_SCHEMA: + source = LidarReplayPackV2(arguments.source_pack) + else: + parser_schema = str(schema_version) if schema_version is not None else "missing" + raise SystemExit(f"unsupported local-surface source schema: {parser_schema}") try: output = build_k1_local_surface( source, diff --git a/src/k1link/compute/__init__.py b/src/k1link/compute/__init__.py index 05d1688..92c2ab9 100644 --- a/src/k1link/compute/__init__.py +++ b/src/k1link/compute/__init__.py @@ -40,6 +40,18 @@ from .e33_worker_shadow import ( read_e33_worker_shadow_result, run_e33_worker_shadow, ) +from .e51_motion_semantic_qualification import ( + E51_FRAME_SCHEMA, + E51_PROFILE_SCHEMA, + E51_REPORT_SCHEMA, + E51_RESULT_SCHEMA, + E51_SIGNAL_SCHEMA, + E51MotionSemanticError, + E51MotionSemanticResult, + build_e51_motion_semantic_qualification, + derive_motion_semantic_signal, + read_e51_motion_semantic_qualification, +) from .evaluation_pack import ( ANNOTATION_CONTRACT_SCHEMA, EVALUATION_PACK_SCHEMA, @@ -397,6 +409,16 @@ __all__ = [ "E33WorkerShadowResult", "read_e33_worker_shadow_result", "run_e33_worker_shadow", + "E51_FRAME_SCHEMA", + "E51_PROFILE_SCHEMA", + "E51_REPORT_SCHEMA", + "E51_RESULT_SCHEMA", + "E51_SIGNAL_SCHEMA", + "E51MotionSemanticError", + "E51MotionSemanticResult", + "build_e51_motion_semantic_qualification", + "derive_motion_semantic_signal", + "read_e51_motion_semantic_qualification", "build_lidar_ground_annotation_template", "build_lidar_ground_benchmark", "build_k1_local_surface", diff --git a/src/k1link/compute/e51_motion_semantic_qualification.py b/src/k1link/compute/e51_motion_semantic_qualification.py new file mode 100644 index 0000000..c308f3b --- /dev/null +++ b/src/k1link/compute/e51_motion_semantic_qualification.py @@ -0,0 +1,940 @@ +"""Immutable E51 qualification of motion, proximity and semantic evidence.""" + +from __future__ import annotations + +import hashlib +import json +import math +import os +import re +import resource +import shutil +import sys +import time +from dataclasses import dataclass +from datetime import UTC, datetime +from itertools import zip_longest +from pathlib import Path +from typing import Any, Final, TextIO, cast + +import numpy as np + +from .e32_track_geometry_replay import read_e32_track_geometry_replay +from .e34_temporal_occupied_replay import ( + E34TemporalOccupiedReplay, + read_e34_temporal_occupied_replay, +) +from .lidar_field_review import E10LidarFieldSource + +E51_PROFILE_SCHEMA: Final = "missioncore.e51-motion-semantic-profile/v1" +E51_RESULT_SCHEMA: Final = "missioncore.e51-motion-semantic-result/v1" +E51_FRAME_SCHEMA: Final = "missioncore.e51-motion-semantic-frame/v1" +E51_SIGNAL_SCHEMA: Final = "missioncore.e51-motion-semantic-signal/v1" +E51_REPORT_SCHEMA: Final = "missioncore.e51-motion-semantic-report/v1" + +E51_FRAMES_NAME: Final = "motion-semantic-frames.jsonl" +E51_REPORT_NAME: Final = "run-report.json" +E51_MANIFEST_NAME: Final = "manifest.json" + +_RESULT_ID = re.compile(r"^e51-motion-semantic-[a-f0-9]{64}$") +_SHA256 = re.compile(r"^[a-f0-9]{64}$") + + +class E51MotionSemanticError(RuntimeError): + """An E51 profile, source, replay or immutable result is invalid.""" + + +@dataclass(frozen=True, slots=True) +class E51MotionSemanticResult: + """One validated immutable E51 diagnostic result.""" + + result_root: Path + result_id: str + manifest: dict[str, Any] + report: dict[str, Any] + + @property + def accepted(self) -> bool: + return bool(_object(self.report.get("acceptance"), "E51 acceptance")["accepted"]) + + +@dataclass(frozen=True, slots=True) +class _Profile: + raw: dict[str, Any] + expected_e32_result_id: str + expected_e34_result_id: str + expected_source_pack_id: str + minimum_motion_observations: int + minimum_motion_span_seconds: float + minimum_motion_displacement_m: float + minimum_motion_speed_mps: float + maximum_motion_speed_mps: float + proximity_threshold_m: float + maximum_signals_per_frame: int + maximum_latency_p95_ms: float + maximum_rss_growth_mib: float + maximum_lidar_camera_age_p95_ms: float + maximum_pose_age_p95_ms: float + + +def build_e51_motion_semantic_qualification( + *, + e32_result_root: Path, + e34_result_root: Path, + e10_source_root: Path, + profile_path: Path, + output_root: Path, +) -> E51MotionSemanticResult: + """Build or verify the bounded E51 diagnostic derivative.""" + + profile = _read_profile(profile_path) + e32 = read_e32_track_geometry_replay(e32_result_root) + e34 = read_e34_temporal_occupied_replay(e34_result_root) + source = E10LidarFieldSource(e10_source_root) + try: + _validate_bindings(profile=profile, e32=e32, e34=e34, source=source) + e32_artifacts = _verified_artifacts( + e32.result_root, + e32.manifest.get("artifacts"), + key="role", + ) + e34_artifacts = _verified_artifacts( + e34.result_root, + e34.manifest.get("artifacts"), + key="kind", + ) + source_artifact = _object( + source.manifest.get("artifact"), + "E51 E10 source artifact", + ) + upstream_before = { + "e32": _artifact_identity(e32_artifacts), + "e34": _artifact_identity(e34_artifacts), + "e10": { + "lidar-pack": { + "byte_length": source_artifact["byte_length"], + "sha256": source_artifact["sha256"], + } + }, + } + identity = { + "schema_version": E51_RESULT_SCHEMA, + "profile": profile.raw, + "profile_sha256": _sha256(profile_path.resolve(strict=True)), + "source_session_id": source.identity["session_id"], + "frame_count": e32.manifest["identity"]["frame_count"], + "e32_result_id": e32.result_id, + "e32_identity_sha256": e32.manifest["identity_sha256"], + "e34_result_id": e34.result_id, + "e34_identity_sha256": e34.manifest["identity_sha256"], + "source_pack_id": source.pack_id, + "source_pack_identity_sha256": source.manifest["identity_sha256"], + "upstream_artifacts": upstream_before, + "producer_sha256": _sha256(Path(__file__).resolve(strict=True)), + "policy": { + "dynamic_class_available": False, + "collision_state_available": False, + "free_space_available": False, + "absence_of_points_means_free": False, + "persistent_reconstruction_mutated": False, + }, + "authority": _authority(), + } + identity_sha256 = hashlib.sha256(_canonical_json(identity)).hexdigest() + result_id = f"e51-motion-semantic-{identity_sha256}" + destination = output_root.expanduser().absolute() + destination.mkdir(mode=0o700, parents=True, exist_ok=True) + result_root = destination / result_id + if result_root.exists(): + return read_e51_motion_semantic_qualification(result_root) + + staging = destination / f".{result_id}.{os.getpid()}.incomplete" + staging.mkdir(mode=0o700, exist_ok=False) + try: + report = _run_qualification( + staging=staging, + result_id=result_id, + e32_frames=e32_artifacts["track-geometry-frames"], + e34=e34, + e34_frames=e34_artifacts["temporal-occupied-frames"], + source=source, + profile=profile, + ) + upstream_after = { + "e32": _artifact_identity( + _verified_artifacts( + e32.result_root, + e32.manifest.get("artifacts"), + key="role", + ) + ), + "e34": _artifact_identity( + _verified_artifacts( + e34.result_root, + e34.manifest.get("artifacts"), + key="kind", + ) + ), + "e10": { + "lidar-pack": { + "byte_length": source_artifact["byte_length"], + "sha256": _sha256(source.root / str(source_artifact["path"])), + } + }, + } + report["acceptance"]["upstream_unchanged"] = ( + upstream_after == upstream_before + ) + checks = cast(dict[str, bool], report["acceptance"]["checks"]) + checks["upstream_unchanged"] = upstream_after == upstream_before + report["acceptance"]["accepted"] = all(checks.values()) + _write_json(staging / E51_REPORT_NAME, report) + artifacts = [ + _artifact(staging / E51_FRAMES_NAME, "motion-semantic-frames"), + _artifact(staging / E51_REPORT_NAME, "run-report"), + ] + manifest = { + "schema_version": E51_RESULT_SCHEMA, + "result_id": result_id, + "identity_sha256": identity_sha256, + "identity": identity, + "created_at_utc": datetime.now(UTC) + .isoformat(timespec="milliseconds") + .replace("+00:00", "Z"), + "classification": "private-diagnostic-derivative", + "ground_truth": False, + "artifacts": artifacts, + } + _write_json(staging / E51_MANIFEST_NAME, manifest) + os.replace(staging, result_root) + except BaseException: + shutil.rmtree(staging, ignore_errors=True) + raise + return read_e51_motion_semantic_qualification(result_root) + finally: + source.close() + + +def read_e51_motion_semantic_qualification( + result_root: Path, +) -> E51MotionSemanticResult: + """Read and verify one immutable E51 result.""" + + root = result_root.expanduser().absolute().resolve(strict=True) + if not root.is_dir() or _RESULT_ID.fullmatch(root.name) is None: + raise E51MotionSemanticError("E51 result id is invalid") + manifest = _read_json(root / E51_MANIFEST_NAME) + identity = _object(manifest.get("identity"), "E51 identity") + identity_sha256 = manifest.get("identity_sha256") + if ( + manifest.get("schema_version") != E51_RESULT_SCHEMA + or identity.get("schema_version") != E51_RESULT_SCHEMA + or not isinstance(identity_sha256, str) + or _SHA256.fullmatch(identity_sha256) is None + or hashlib.sha256(_canonical_json(identity)).hexdigest() != identity_sha256 + or root.name != f"e51-motion-semantic-{identity_sha256}" + or manifest.get("result_id") != root.name + ): + raise E51MotionSemanticError("E51 result identity is invalid") + artifacts = _verified_artifacts(root, manifest.get("artifacts"), key="kind") + required = {"motion-semantic-frames", "run-report"} + if set(artifacts) != required: + raise E51MotionSemanticError("E51 artifact set is invalid") + report = _read_json(artifacts["run-report"]) + acceptance = _object(report.get("acceptance"), "E51 acceptance") + checks = _object(acceptance.get("checks"), "E51 acceptance checks") + if ( + report.get("schema_version") != E51_REPORT_SCHEMA + or report.get("result_id") != root.name + or acceptance.get("accepted") is not all(value is True for value in checks.values()) + or report.get("authority") != _authority() + ): + raise E51MotionSemanticError("E51 report is invalid") + return E51MotionSemanticResult( + result_root=root, + result_id=root.name, + manifest=manifest, + report=report, + ) + + +def derive_motion_semantic_signal( + component: dict[str, Any], + geometry: dict[str, Any] | None, + *, + map_frame_jump_candidate: bool, + profile: dict[str, float | int], +) -> dict[str, Any] | None: + """Derive one conservative diagnostic signal from existing evidence.""" + + state = component.get("state") + if state not in {"current", "held"}: + raise E51MotionSemanticError("E51 component freshness is invalid") + history = _list(component.get("history_tail"), "E51 component history") + motion = _motion_metrics( + history, + map_frame_jump_candidate=map_frame_jump_candidate, + minimum_observations=int(profile["minimum_motion_observations"]), + minimum_span_seconds=float(profile["minimum_motion_span_seconds"]), + minimum_displacement_m=float(profile["minimum_motion_displacement_m"]), + minimum_speed_mps=float(profile["minimum_motion_speed_mps"]), + maximum_speed_mps=float(profile["maximum_motion_speed_mps"]), + ) + evidence_state = ( + str(geometry.get("evidence_state")) + if geometry is not None + else "held-temporal-evidence" + ) + reason_codes = ( + [ + str(value) + for value in _list( + geometry.get("reason_codes"), + "E51 geometry reason codes", + ) + ] + if geometry is not None + else [] + ) + conflict = evidence_state == "conflict" or any( + "conflict" in value or "collision" in value for value in reason_codes + ) + semantic = geometry.get("semantic") if geometry is not None else None + if semantic is None: + provenance = _object( + component.get("semantic_provenance"), + "E51 semantic provenance", + ) + labels = _list(provenance.get("labels"), "E51 semantic labels") + track_ids = _list(provenance.get("track_ids"), "E51 semantic track ids") + if labels or track_ids or provenance.get("owner") is not None: + semantic = { + "owner": provenance.get("owner"), + "labels": labels, + "track_ids": track_ids, + "source": "e34-held-semantic-provenance", + } + range_m = geometry.get("range_m") if geometry is not None else None + range_value = ( + float(range_m) + if isinstance(range_m, int | float) and math.isfinite(float(range_m)) + else None + ) + proximity_candidate = ( + state == "current" + and range_value is not None + and range_value <= float(profile["proximity_threshold_m"]) + ) + if ( + not motion["candidate"] + and not proximity_candidate + and semantic is None + and not conflict + ): + return None + return { + "schema_version": E51_SIGNAL_SCHEMA, + "temporal_id": component["temporal_id"], + "source_owner_key": component["source_owner_key"], + "owner_kind": component["owner_kind"], + "freshness": { + "state": state, + "age_seconds": component["last_observed_age_seconds"], + "current_hit_backed": state == "current", + }, + "semantic": { + "available": semantic is not None, + "value": semantic, + "confidence": { + "available": False, + "value": None, + "reason": "upstream-contract-has-no-numeric-confidence", + }, + }, + "evidence": { + "state": evidence_state, + "conflict": conflict, + "reason_codes": reason_codes, + }, + "motion": motion, + "proximity": { + "candidate": proximity_candidate, + "range_m": range_value, + "threshold_m": float(profile["proximity_threshold_m"]), + "classification": "diagnostic-near-occupied-candidate", + }, + "collision": { + "state": "unavailable", + "reason": "vehicle-body-and-lidar-mount-geometry-not-bound", + }, + "authority": _authority(), + } + + +def _run_qualification( + *, + staging: Path, + result_id: str, + e32_frames: Path, + e34: E34TemporalOccupiedReplay, + e34_frames: Path, + source: E10LidarFieldSource, + profile: _Profile, +) -> dict[str, Any]: + started = time.perf_counter() + rss_start = _process_peak_rss_mib() + frame_latencies_ms: list[float] = [] + signal_counts = { + "total": 0, + "motion_candidates": 0, + "proximity_candidates": 0, + "semantic_available": 0, + "conflicts": 0, + "current": 0, + "held": 0, + } + frame_count = 0 + accepted_current_point_rows = 0 + map_frame_jump_candidates = 0 + maximum_signals_observed = 0 + frames_path = staging / E51_FRAMES_NAME + with ( + e32_frames.open("r", encoding="utf-8") as e32_stream, + e34_frames.open("r", encoding="utf-8") as e34_stream, + frames_path.open("x", encoding="utf-8") as output, + ): + for e32_line, e34_line in zip_longest(e32_stream, e34_stream): + frame_started = time.perf_counter() + if e32_line is None or e34_line is None: + raise E51MotionSemanticError("E51 upstream frame counts differ") + e32_frame = _parse_json_line(e32_line, "E51 E32 frame") + e34_frame = _parse_json_line(e34_line, "E51 E34 frame") + _validate_frame_pair(e32_frame, e34_frame, frame_count) + geometries = { + str(geometry["owner_key"]): geometry + for geometry in _object_list( + e32_frame.get("geometries"), + "E51 E32 geometries", + ) + } + jump = _object( + e34_frame.get("map_frame_jump"), + "E51 map-frame jump", + ) + jump_candidate = jump.get("candidate") is True + map_frame_jump_candidates += int(jump_candidate) + signals: list[dict[str, Any]] = [] + for component in [ + *_object_list(e34_frame.get("current"), "E51 current components"), + *_object_list(e34_frame.get("held"), "E51 held components"), + ]: + owner_key = str(component.get("source_owner_key")) + signal = derive_motion_semantic_signal( + component, + geometries.get(owner_key), + map_frame_jump_candidate=jump_candidate, + profile={ + "minimum_motion_observations": profile.minimum_motion_observations, + "minimum_motion_span_seconds": profile.minimum_motion_span_seconds, + "minimum_motion_displacement_m": profile.minimum_motion_displacement_m, + "minimum_motion_speed_mps": profile.minimum_motion_speed_mps, + "maximum_motion_speed_mps": profile.maximum_motion_speed_mps, + "proximity_threshold_m": profile.proximity_threshold_m, + }, + ) + if signal is not None: + signals.append(signal) + maximum_signals_observed = max(maximum_signals_observed, len(signals)) + if len(signals) > profile.maximum_signals_per_frame: + raise E51MotionSemanticError( + "E51 signal count exceeds the bounded profile" + ) + input_summary = _object(e34_frame.get("input"), "E51 E34 input") + current_point_rows = _nonnegative_int( + input_summary.get("accepted_current_point_rows"), + "E51 accepted current point rows", + ) + accepted_current_point_rows += current_point_rows + for signal in signals: + _count_signal(signal_counts, signal) + record = { + "schema_version": E51_FRAME_SCHEMA, + "frame_index": frame_count, + "source_frame_index": e34_frame["source_frame_index"], + "session_seconds": e34_frame["session_seconds"], + "source_available": e34_frame["source_available"], + "layer_state": e34_frame["layer_state"], + "accepted_current_point_rows": current_point_rows, + "signal_count": len(signals), + "signals": signals, + "policy": { + "dynamic_class_available": False, + "collision_state_available": False, + "free_space_available": False, + "absence_of_points_means_free": False, + "persistent_reconstruction_mutated": False, + }, + "authority": _authority(), + } + _write_json_line(output, record) + frame_count += 1 + frame_latencies_ms.append( + (time.perf_counter() - frame_started) * 1_000.0 + ) + + rss_end = _process_peak_rss_mib() + lidar_age = _finite_abs(source.arrays["lidar_camera_delta_ms"]) + pose_age = _finite_abs(source.arrays["pose_point_delta_ms"]) + latency = _distribution(np.asarray(frame_latencies_ms, dtype=np.float64)) + lidar_age_report = _distribution(lidar_age) + pose_age_report = _distribution(pose_age) + e34_occupancy = _object( + _object(e34.report.get("metrics"), "E51 E34 metrics").get("occupancy"), + "E51 E34 occupancy metrics", + ) + expected_point_rows = _nonnegative_int( + e34_occupancy.get("e34_consumed_current_point_rows"), + "E51 E34 consumed point rows", + ) + rss_growth = max(0.0, rss_end - rss_start) + checks = { + "complete_frame_accounting": frame_count + == _nonnegative_int( + e34.manifest["identity"].get("frame_count"), + "E51 E34 frame count", + ), + "map_frame_jump_candidates_zero": map_frame_jump_candidates == 0, + "current_obstacle_rows_preserved": ( + accepted_current_point_rows == expected_point_rows + ), + "latency_p95_within_gate": _required_float(latency["p95"]) + <= profile.maximum_latency_p95_ms, + "rss_growth_within_gate": rss_growth <= profile.maximum_rss_growth_mib, + "lidar_camera_age_p95_within_gate": _required_float( + lidar_age_report["p95"] + ) + <= profile.maximum_lidar_camera_age_p95_ms, + "pose_age_p95_within_gate": _required_float(pose_age_report["p95"]) + <= profile.maximum_pose_age_p95_ms, + "bounded_signal_state": maximum_signals_observed + <= profile.maximum_signals_per_frame, + "free_space_not_published": True, + "dynamic_class_not_invented": True, + "collision_state_not_invented": True, + "upstream_unchanged": False, + } + return { + "schema_version": E51_REPORT_SCHEMA, + "result_id": result_id, + "status": "diagnostic-only", + "ground_truth": False, + "metrics": { + "frames": { + "processed": frame_count, + "map_frame_jump_candidates": map_frame_jump_candidates, + }, + "signals": { + **signal_counts, + "maximum_per_frame": maximum_signals_observed, + }, + "obstacle_preservation": { + "e34_consumed_current_point_rows": expected_point_rows, + "e51_observed_current_point_rows": accepted_current_point_rows, + "exact": accepted_current_point_rows == expected_point_rows, + "persistent_reconstruction_mutated": False, + }, + "runtime": { + "elapsed_ms": (time.perf_counter() - started) * 1_000.0, + "frame_processing_ms": latency, + "process_peak_rss_start_mib": rss_start, + "process_peak_rss_end_mib": rss_end, + "process_peak_rss_growth_mib": rss_growth, + "rss_measurement": "process-peak-rss", + }, + "point_age_ms": { + "lidar_to_camera": lidar_age_report, + "pose_to_lidar": pose_age_report, + "basis": "accepted-e10-nearest-host-arrival-best-effort", + }, + }, + "semantic_contract": { + "camera_owns_semantics": True, + "numeric_confidence_available": False, + "freshness_explicit": True, + "conflict_explicit": True, + }, + "motion_contract": { + "classification": "diagnostic-motion-candidate", + "dynamic_class_available": False, + "map_frame_jump_rejected": True, + }, + "proximity_contract": { + "classification": "diagnostic-near-occupied-candidate", + "collision_state_available": False, + "reason": "vehicle-body-and-lidar-mount-geometry-not-bound", + }, + "acceptance": { + "accepted": False, + "upstream_unchanged": False, + "checks": checks, + }, + "authority": _authority(), + } + + +def _motion_metrics( + history: list[object], + *, + map_frame_jump_candidate: bool, + minimum_observations: int, + minimum_span_seconds: float, + minimum_displacement_m: float, + minimum_speed_mps: float, + maximum_speed_mps: float, +) -> dict[str, Any]: + points = [_object(value, "E51 history observation") for value in history] + if len(points) < 2: + span_seconds = 0.0 + displacement_m = 0.0 + speed_mps = 0.0 + else: + first = points[0] + last = points[-1] + first_xyz = _xyz(first.get("centroid_map_xyz_m")) + last_xyz = _xyz(last.get("centroid_map_xyz_m")) + span_seconds = float(last["session_seconds"]) - float(first["session_seconds"]) + displacement_m = math.dist(first_xyz, last_xyz) + speed_mps = displacement_m / span_seconds if span_seconds > 0.0 else 0.0 + candidate = ( + not map_frame_jump_candidate + and len(points) >= minimum_observations + and span_seconds >= minimum_span_seconds + and displacement_m >= minimum_displacement_m + and minimum_speed_mps <= speed_mps <= maximum_speed_mps + ) + return { + "candidate": candidate, + "classification": "diagnostic-motion-candidate", + "observation_count": len(points), + "span_seconds": span_seconds, + "displacement_m": displacement_m, + "speed_mps": speed_mps, + "map_frame_jump_rejected": map_frame_jump_candidate, + "dynamic_class_available": False, + } + + +def _validate_bindings( + *, + profile: _Profile, + e32: Any, + e34: E34TemporalOccupiedReplay, + source: E10LidarFieldSource, +) -> None: + e34_identity = _object(e34.manifest.get("identity"), "E51 E34 identity") + if ( + e32.result_id != profile.expected_e32_result_id + or e34.result_id != profile.expected_e34_result_id + or source.pack_id != profile.expected_source_pack_id + or e34_identity.get("e32_result_id") != e32.result_id + or e34_identity.get("source_session_id") != source.identity.get("session_id") + or not e34.accepted + ): + raise E51MotionSemanticError("E51 upstream binding is invalid") + + +def _validate_frame_pair( + e32_frame: dict[str, Any], + e34_frame: dict[str, Any], + expected_index: int, +) -> None: + e32_seconds = e32_frame.get("session_seconds") + e34_seconds = e34_frame.get("session_seconds") + if ( + e32_frame.get("frame_index") != expected_index + or e34_frame.get("frame_index") != expected_index + or not isinstance(e32_seconds, int | float) + or not isinstance(e34_seconds, int | float) + or abs(float(e32_seconds) - float(e34_seconds)) > 1e-9 + or e32_frame.get("source_frame_index") + != e34_frame.get("source_frame_index") + ): + raise E51MotionSemanticError("E51 upstream frame alignment is invalid") + + +def _read_profile(path: Path) -> _Profile: + raw = _read_json(path.expanduser().resolve(strict=True)) + motion = _object(raw.get("motion"), "E51 motion profile") + proximity = _object(raw.get("proximity"), "E51 proximity profile") + acceptance = _object(raw.get("acceptance"), "E51 acceptance profile") + expected = _object(raw.get("expected"), "E51 expected sources") + profile = _Profile( + raw=raw, + expected_e32_result_id=_required_string(expected.get("e32_result_id")), + expected_e34_result_id=_required_string(expected.get("e34_result_id")), + expected_source_pack_id=_required_string(expected.get("source_pack_id")), + minimum_motion_observations=_positive_int( + motion.get("minimum_observations") + ), + minimum_motion_span_seconds=_positive_float( + motion.get("minimum_span_seconds") + ), + minimum_motion_displacement_m=_positive_float( + motion.get("minimum_displacement_m") + ), + minimum_motion_speed_mps=_positive_float( + motion.get("minimum_speed_mps") + ), + maximum_motion_speed_mps=_positive_float( + motion.get("maximum_speed_mps") + ), + proximity_threshold_m=_positive_float( + proximity.get("threshold_m") + ), + maximum_signals_per_frame=_positive_int( + acceptance.get("maximum_signals_per_frame") + ), + maximum_latency_p95_ms=_positive_float( + acceptance.get("maximum_latency_p95_ms") + ), + maximum_rss_growth_mib=_positive_float( + acceptance.get("maximum_rss_growth_mib") + ), + maximum_lidar_camera_age_p95_ms=_positive_float( + acceptance.get("maximum_lidar_camera_age_p95_ms") + ), + maximum_pose_age_p95_ms=_positive_float( + acceptance.get("maximum_pose_age_p95_ms") + ), + ) + if ( + raw.get("schema_version") != E51_PROFILE_SCHEMA + or raw.get("profile_id") + != "e51-motion-proximity-semantic-qualification/v1" + or profile.maximum_motion_speed_mps <= profile.minimum_motion_speed_mps + ): + raise E51MotionSemanticError("E51 profile is invalid") + return profile + + +def _count_signal(counts: dict[str, int], signal: dict[str, Any]) -> None: + counts["total"] += 1 + freshness = _object(signal["freshness"], "E51 signal freshness") + counts[str(freshness["state"])] += 1 + if _object(signal["motion"], "E51 signal motion")["candidate"] is True: + counts["motion_candidates"] += 1 + if _object(signal["proximity"], "E51 signal proximity")["candidate"] is True: + counts["proximity_candidates"] += 1 + if _object(signal["semantic"], "E51 signal semantic")["available"] is True: + counts["semantic_available"] += 1 + if _object(signal["evidence"], "E51 signal evidence")["conflict"] is True: + counts["conflicts"] += 1 + + +def _finite_abs(value: Any) -> np.ndarray[Any, np.dtype[np.float64]]: + array = np.abs(np.asarray(value, dtype=np.float64)) + return array[np.isfinite(array)] + + +def _distribution(values: np.ndarray[Any, np.dtype[np.float64]]) -> dict[str, Any]: + finite = values[np.isfinite(values)] + if finite.size == 0: + return { + "sample_count": 0, + "minimum": None, + "mean": None, + "p50": None, + "p95": None, + "maximum": None, + } + return { + "sample_count": int(finite.size), + "minimum": float(np.min(finite)), + "mean": float(np.mean(finite)), + "p50": float(np.percentile(finite, 50)), + "p95": float(np.percentile(finite, 95)), + "maximum": float(np.max(finite)), + } + + +def _process_peak_rss_mib() -> float: + value = float(resource.getrusage(resource.RUSAGE_SELF).ru_maxrss) + divisor = 1024.0 * 1024.0 if sys.platform == "darwin" else 1024.0 + return value / divisor + + +def _verified_artifacts( + root: Path, + raw: object, + *, + key: str, +) -> dict[str, Path]: + result: dict[str, Path] = {} + for value in _list(raw, "E51 artifacts"): + artifact = _object(value, "E51 artifact") + name = artifact.get(key) + relative = artifact.get("path") + sha256 = artifact.get("sha256") + byte_length = artifact.get("byte_length") + if ( + not isinstance(name, str) + or not name + or name in result + or not isinstance(relative, str) + or Path(relative).name != relative + or not isinstance(sha256, str) + or _SHA256.fullmatch(sha256) is None + or not isinstance(byte_length, int) + or isinstance(byte_length, bool) + or byte_length < 0 + ): + raise E51MotionSemanticError("E51 artifact descriptor is invalid") + path = (root / relative).resolve(strict=True) + if ( + path.parent != root + or not path.is_file() + or path.stat().st_size != byte_length + or _sha256(path) != sha256 + ): + raise E51MotionSemanticError("E51 artifact integrity failed") + result[name] = path + return result + + +def _artifact_identity(artifacts: dict[str, Path]) -> dict[str, dict[str, Any]]: + return { + name: { + "byte_length": path.stat().st_size, + "sha256": _sha256(path), + } + for name, path in sorted(artifacts.items()) + } + + +def _artifact(path: Path, kind: str) -> dict[str, Any]: + return { + "kind": kind, + "path": path.name, + "media_type": ( + "application/x-ndjson" + if path.suffix == ".jsonl" + else "application/json" + ), + "byte_length": path.stat().st_size, + "sha256": _sha256(path), + } + + +def _parse_json_line(line: str, label: str) -> dict[str, Any]: + try: + value = json.loads(line) + except json.JSONDecodeError as exc: + raise E51MotionSemanticError(f"{label} is invalid JSON") from exc + return _object(value, label) + + +def _read_json(path: Path) -> dict[str, Any]: + try: + value = json.loads(path.read_text(encoding="utf-8")) + except (OSError, json.JSONDecodeError) as exc: + raise E51MotionSemanticError(f"E51 JSON is invalid: {path}") from exc + return _object(value, f"E51 JSON {path.name}") + + +def _write_json(path: Path, value: object) -> None: + path.write_bytes(_canonical_json(value) + b"\n") + + +def _write_json_line(stream: TextIO, value: object) -> None: + stream.write(_canonical_json(value).decode("utf-8")) + stream.write("\n") + + +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: + digest = hashlib.sha256() + with path.open("rb") as stream: + for chunk in iter(lambda: stream.read(1024 * 1024), b""): + digest.update(chunk) + return digest.hexdigest() + + +def _authority() -> dict[str, bool]: + return { + "commands_enabled": False, + "navigation_or_safety_accepted": False, + } + + +def _object(value: object, label: str) -> dict[str, Any]: + if not isinstance(value, dict): + raise E51MotionSemanticError(f"{label} must be an object") + return cast(dict[str, Any], value) + + +def _list(value: object, label: str) -> list[object]: + if not isinstance(value, list): + raise E51MotionSemanticError(f"{label} must be a list") + return value + + +def _object_list(value: object, label: str) -> list[dict[str, Any]]: + return [_object(item, label) for item in _list(value, label)] + + +def _xyz(value: object) -> tuple[float, float, float]: + values = _list(value, "E51 centroid") + if ( + len(values) != 3 + or not all( + isinstance(item, int | float) and math.isfinite(float(item)) + for item in values + ) + ): + raise E51MotionSemanticError("E51 centroid is invalid") + numeric = cast(list[int | float], values) + return (float(numeric[0]), float(numeric[1]), float(numeric[2])) + + +def _required_string(value: object) -> str: + if not isinstance(value, str) or not value: + raise E51MotionSemanticError("E51 required string is invalid") + return value + + +def _positive_int(value: object) -> int: + if not isinstance(value, int) or isinstance(value, bool) or value <= 0: + raise E51MotionSemanticError("E51 positive integer is invalid") + return value + + +def _nonnegative_int(value: object, label: str) -> int: + if not isinstance(value, int) or isinstance(value, bool) or value < 0: + raise E51MotionSemanticError(f"{label} is invalid") + return value + + +def _positive_float(value: object) -> float: + if ( + not isinstance(value, int | float) + or isinstance(value, bool) + or not math.isfinite(float(value)) + or float(value) <= 0.0 + ): + raise E51MotionSemanticError("E51 positive number is invalid") + return float(value) + + +def _required_float(value: object) -> float: + if not isinstance(value, int | float) or not math.isfinite(float(value)): + raise E51MotionSemanticError("E51 required number is invalid") + return float(value) diff --git a/src/k1link/compute/lidar_local_surface.py b/src/k1link/compute/lidar_local_surface.py index 209ff1c..3b5013e 100644 --- a/src/k1link/compute/lidar_local_surface.py +++ b/src/k1link/compute/lidar_local_surface.py @@ -24,6 +24,7 @@ from .lidar_field_review import ( RAVNOVES00_CENTRAL_WINDOWS, E10LidarFieldSource, ) +from .lidar_replay import LIDAR_REPLAY_PACK_SCHEMA, LidarReplayPackV2 K1_LOCAL_SURFACE_SCHEMA: Final = "missioncore.k1-local-surface/v1" K1_LOCAL_SURFACE_REPORT_SCHEMA: Final = "missioncore.k1-local-surface-report/v1" @@ -53,6 +54,25 @@ _LOCAL_SURFACE_ID = re.compile(r"^k1-local-surface-[a-f0-9]{64}$") _E10_PACK_ID = re.compile(r"^e10-lidar-pack-[a-f0-9]{64}$") _SHA256 = re.compile(r"^[a-f0-9]{64}$") +@dataclass(frozen=True, slots=True) +class _LocalSurfaceSourceView: + pack_id: str + identity_sha256: str + artifact_sha256: str + session_id: str + representation: str + schema_version: str + intensity_available: bool + field_retention: dict[str, object] | None + arrays: Mapping[str, npt.NDArray[Any]] + frame_count: int + point_count: int + + +LocalSurfaceSource = ( + E10LidarFieldSource | LidarReplayPackV2 | _LocalSurfaceSourceView +) + @dataclass(frozen=True, slots=True) class K1LocalSurfaceProfile: @@ -511,9 +531,11 @@ class K1LocalSurfaceV1: def frame_detail( self, - source: E10LidarFieldSource, + source: LocalSurfaceSource, frame_index: int, ) -> dict[str, object]: + source_view = _local_surface_source_view(source) + source = source_view _validate_source_binding(self, source) if not 0 <= frame_index < source.frame_count: raise IndexError(frame_index) @@ -550,7 +572,7 @@ class K1LocalSurfaceV1: "schema_version": K1_LOCAL_SURFACE_FRAME_SCHEMA, "model_id": self.model_id, "source_pack_id": source.pack_id, - "session_id": source.identity["session_id"], + "session_id": source.session_id, "frame_index": frame_index, "frame_count": source.frame_count, "source_frame_index": int(source.arrays["source_frame_indices"][frame_index]), @@ -716,7 +738,9 @@ class K1LocalSurfaceV1: "ground_truth": False, } - def timeline_detail(self, source: E10LidarFieldSource) -> dict[str, object]: + def timeline_detail(self, source: LocalSurfaceSource) -> dict[str, object]: + source_view = _local_surface_source_view(source) + source = source_view _validate_source_binding(self, source) frame_count = source.frame_count if self.has_temporal_qualification: @@ -745,7 +769,7 @@ class K1LocalSurfaceV1: "schema_version": K1_LOCAL_SURFACE_TIMELINE_SCHEMA, "model_id": self.model_id, "source_pack_id": source.pack_id, - "session_id": source.identity["session_id"], + "session_id": source.session_id, "frame_count": frame_count, "source_frame_index": source.arrays["source_frame_indices"] .astype(np.int64) @@ -772,7 +796,9 @@ class K1LocalSurfaceV1: "authority": self.report["authority"], } - def review_detail(self, source: E10LidarFieldSource) -> dict[str, object]: + def review_detail(self, source: LocalSurfaceSource) -> dict[str, object]: + source_view = _local_surface_source_view(source) + source = source_view _validate_source_binding(self, source) criteria = self._review_criteria() reason_counts = { @@ -788,7 +814,7 @@ class K1LocalSurfaceV1: "review_profile_id": K1_LOCAL_SURFACE_REVIEW_PROFILE_ID, "model_id": self.model_id, "source_pack_id": source.pack_id, - "session_id": source.identity["session_id"], + "session_id": source.session_id, "available": False, "criteria": criteria, "summary": { @@ -918,7 +944,7 @@ class K1LocalSurfaceV1: "review_profile_id": K1_LOCAL_SURFACE_REVIEW_PROFILE_ID, "model_id": self.model_id, "source_pack_id": source.pack_id, - "session_id": source.identity["session_id"], + "session_id": source.session_id, "available": True, "criteria": criteria, "summary": { @@ -985,7 +1011,7 @@ class K1LocalSurfaceV1: def build_k1_local_surface( - source: E10LidarFieldSource, + source: LocalSurfaceSource, output_root: Path, *, profile: K1LocalSurfaceProfile = DEFAULT_K1_LOCAL_SURFACE_PROFILE, @@ -995,6 +1021,8 @@ def build_k1_local_surface( if not display_name.strip() or len(display_name) > 200: raise LidarGroundError("K1 local-surface display name is invalid") + source_view = _local_surface_source_view(source) + source = source_view started = time.perf_counter() frame_count = source.frame_count point_count = source.point_count @@ -1174,9 +1202,11 @@ def build_k1_local_surface( identity = { "schema_version": K1_LOCAL_SURFACE_SCHEMA, "source_pack_id": source.pack_id, - "source_pack_identity_sha256": source.manifest["identity_sha256"], - "source_artifact_sha256": source.manifest["artifact"]["sha256"], - "session_id": source.identity["session_id"], + "source_pack_identity_sha256": source.identity_sha256, + "source_artifact_sha256": source.artifact_sha256, + "source_schema_version": source.schema_version, + "source_representation": source.representation, + "session_id": source.session_id, "display_name": display_name, "frame_count": frame_count, "valid_frame_count": int(np.count_nonzero(valid)), @@ -1204,12 +1234,15 @@ def build_k1_local_surface( "schema_version": K1_LOCAL_SURFACE_REPORT_SCHEMA, "model_id": model_id, "display_name": display_name, - "session_id": source.identity["session_id"], + "session_id": source.session_id, "source_pack_id": source.pack_id, "status": "diagnostic-only", "ground_truth": False, "source": { - "representation": "legacy-e10-vendor-map-with-pose", + "representation": source.representation, + "schema_version": source.schema_version, + "intensity_available": source.intensity_available, + "field_retention": source.field_retention, "immutable": True, "passive_processing_only": True, "firmware_or_device_commands_used": False, @@ -1725,7 +1758,7 @@ def _height_above_plane( def _anchors( - source: E10LidarFieldSource, + source: _LocalSurfaceSourceView, valid: npt.NDArray[np.bool_], ) -> list[dict[str, object]]: source_indices = source.arrays["source_frame_indices"] @@ -1737,6 +1770,18 @@ def _anchors( for window in RAVNOVES00_CENTRAL_WINDOWS: if candidates.size == 0: frame_index = 0 + elif source.representation == "lossless-lidar-replay-v2": + midpoint_seconds = (window.start_seconds + window.end_seconds) / 2.0 + frame_index = int( + candidates[ + np.argmin( + np.abs( + source.arrays["session_seconds"][candidates] + - midpoint_seconds + ) + ) + ] + ) else: frame_index = int( candidates[ @@ -1763,20 +1808,185 @@ def _anchors( def _validate_source_binding( model: K1LocalSurfaceV1, - source: E10LidarFieldSource, + source: _LocalSurfaceSourceView, ) -> None: if ( model.identity.get("source_pack_id") != source.pack_id or model.identity.get("source_pack_identity_sha256") - != source.manifest.get("identity_sha256") + != source.identity_sha256 or model.identity.get("source_artifact_sha256") - != source.manifest.get("artifact", {}).get("sha256") + != source.artifact_sha256 or model.identity.get("frame_count") != source.frame_count or model.identity.get("point_count") != source.point_count ): raise LidarGroundError("K1 local-surface source binding is invalid") +def _local_surface_source_view( + source: LocalSurfaceSource | _LocalSurfaceSourceView, +) -> _LocalSurfaceSourceView: + if isinstance(source, _LocalSurfaceSourceView): + return source + if isinstance(source, E10LidarFieldSource): + artifact = _object(source.manifest.get("artifact"), "E10 LiDAR artifact") + identity_sha256 = source.manifest.get("identity_sha256") + artifact_sha256 = artifact.get("sha256") + session_id = source.identity.get("session_id") + if ( + not isinstance(identity_sha256, str) + or _SHA256.fullmatch(identity_sha256) is None + or not isinstance(artifact_sha256, str) + or _SHA256.fullmatch(artifact_sha256) is None + or not isinstance(session_id, str) + or not session_id + ): + raise LidarGroundError("E10 local-surface source binding is invalid") + arrays = { + name: np.asarray(source.arrays[name]) + for name in ( + "source_frame_indices", + "session_seconds", + "sample_available", + "cloud_offsets", + "cloud_points_map", + "pose_positions_map", + "pose_quaternions_map_from_lidar", + "pose_point_delta_ms", + ) + } + return _LocalSurfaceSourceView( + pack_id=source.pack_id, + identity_sha256=identity_sha256, + artifact_sha256=artifact_sha256, + session_id=session_id, + representation="legacy-e10-vendor-map-with-pose", + schema_version=str(source.identity["schema_version"]), + intensity_available=False, + field_retention=None, + arrays=arrays, + frame_count=source.frame_count, + point_count=source.point_count, + ) + if not isinstance(source, LidarReplayPackV2): + raise LidarGroundError("K1 local-surface source type is unsupported") + identity_sha256 = source.manifest.get("identity_sha256") + session_id = source.identity.get("session_id") + artifact_sha256 = _lidar_replay_arrays_sha256(source) + if ( + source.identity.get("schema_version") != LIDAR_REPLAY_PACK_SCHEMA + or not isinstance(identity_sha256, str) + or _SHA256.fullmatch(identity_sha256) is None + or not isinstance(session_id, str) + or not session_id + ): + raise LidarGroundError("LiDAR replay v2 local-surface binding is invalid") + point_times = np.asarray( + source.arrays["point_received_monotonic_ns"], + dtype=" str: + artifacts = _list(source.manifest.get("artifacts"), "LiDAR replay artifacts") + for value in artifacts: + artifact = _object(value, "LiDAR replay artifact") + if artifact.get("kind") == "lidar-arrays": + sha256 = artifact.get("sha256") + if isinstance(sha256, str) and _SHA256.fullmatch(sha256) is not None: + return sha256 + raise LidarGroundError("LiDAR replay arrays artifact is missing") + + +def _nearest_pose_indices( + point_times: npt.NDArray[np.int64], + pose_times: npt.NDArray[np.int64], +) -> tuple[npt.NDArray[np.int64], npt.NDArray[np.float64]]: + if pose_times.size == 0: + return ( + np.zeros(point_times.shape[0], dtype=" str: digest.update(name.encode()) digest.update(array.dtype.str.encode()) digest.update(_canonical_json(list(array.shape))) - digest.update(memoryview(array).cast("B")) + if array.nbytes: + digest.update(memoryview(array).cast("B")) return digest.hexdigest() diff --git a/src/k1link/compute/lidar_replay.py b/src/k1link/compute/lidar_replay.py index d934382..394c0a7 100644 --- a/src/k1link/compute/lidar_replay.py +++ b/src/k1link/compute/lidar_replay.py @@ -21,12 +21,9 @@ from k1link.data_plane import ( DecodedPoseView, ) from k1link.device_plugins.xgrids_k1.protocol.streams import ( - LioPointCloudFrame, - LioPoseFrame, decode_lio_pcl, decode_lio_pose, ) -from k1link.device_plugins.xgrids_k1.viewer.messages import StreamMessage from k1link.device_plugins.xgrids_k1.viewer.replay import iter_replay_messages from .lidar_contract import ( @@ -86,6 +83,27 @@ class LidarReplayError(ValueError): """A replay pack or its source evidence violates the v2 contract.""" +class _MaterializedNpz: + """One-time decompression wrapper for bounded repeated array access.""" + + def __init__(self, path: Path) -> None: + archive = np.load(path, allow_pickle=False) + try: + self.files = list(archive.files) + self._arrays = { + name: np.asarray(archive[name]) + for name in self.files + } + finally: + archive.close() + + def __getitem__(self, name: str) -> npt.NDArray[Any]: + return self._arrays[name] + + def close(self) -> None: + self._arrays.clear() + + @dataclass(frozen=True, slots=True) class LidarReplayPointFrame: capture_sequence: int @@ -180,7 +198,7 @@ class LidarReplayPackV2: self.arrays_path = artifacts["lidar-arrays"] self.quality_path = artifacts["lidar-quality"] self.equivalence_path = artifacts["live-replay-equivalence"] - self.arrays = np.load(self.arrays_path, allow_pickle=False) + self.arrays = _MaterializedNpz(self.arrays_path) if set(self.arrays.files) != set(_ARRAY_DTYPES): self.close() raise LidarReplayError("LiDAR replay array set is incompatible") @@ -413,6 +431,7 @@ def build_lidar_replay_pack_v2( "artifacts": artifacts, } _write_json(staging / LIDAR_MANIFEST_NAME, manifest) + del arrays os.replace(staging, output) try: validation = LidarReplayPackV2(output) @@ -486,127 +505,159 @@ def lidar_pack_detail(pack: LidarReplayPackV2) -> dict[str, object]: def _capture_arrays(path: Path) -> dict[str, npt.NDArray[Any]]: - point_messages: list[tuple[StreamMessage, LioPointCloudFrame]] = [] - pose_messages: list[tuple[StreamMessage, LioPoseFrame]] = [] + point_capture_sequence: list[int] = [] + point_payload_bytes: list[int] = [] + point_received_at_epoch_ns: list[int] = [] + point_received_monotonic_ns: list[int] = [] + point_header_seq: list[int] = [] + point_header_stamp: list[int] = [] + point_scaler: list[int] = [] + point_counts: list[int] = [] + pose_capture_sequence: list[int] = [] + pose_payload_bytes: list[int] = [] + pose_received_at_epoch_ns: list[int] = [] + pose_received_monotonic_ns: list[int] = [] + pose_header_seq: list[int] = [] + pose_header_stamp: list[int] = [] + pose_header_scaler: list[int] = [] + pose_stamp: list[int] = [] + pose_positions_map: list[tuple[float, float, float]] = [] + pose_quaternions_map_from_lidar: list[tuple[float, float, float, float]] = [] + pose_distance: list[float] = [] + pose_accuracy: list[float] = [] + for message in iter_replay_messages(path): if message.source != "k1mqtt" or message.received_monotonic_ns is None: raise LidarReplayError("LiDAR v2 requires native capture with exact host time") if message.topic.endswith(_POINT_TOPIC_SUFFIX): - point_messages.append((message, decode_lio_pcl(message.payload))) + point_frame = decode_lio_pcl(message.payload) + point_capture_sequence.append(message.sequence) + point_payload_bytes.append(len(message.payload)) + point_received_at_epoch_ns.append(message.received_at_epoch_ns) + point_received_monotonic_ns.append(message.received_monotonic_ns) + point_header_seq.append(point_frame.header.seq) + point_header_stamp.append(point_frame.header.stamp) + point_scaler.append(point_frame.header.scaler) + point_counts.append(len(point_frame.points)) elif message.topic.endswith(_POSE_TOPIC_SUFFIX): - pose_messages.append((message, decode_lio_pose(message.payload))) - if not point_messages: + pose_frame = decode_lio_pose(message.payload) + pose_capture_sequence.append(message.sequence) + pose_payload_bytes.append(len(message.payload)) + pose_received_at_epoch_ns.append(message.received_at_epoch_ns) + pose_received_monotonic_ns.append(message.received_monotonic_ns) + pose_header_seq.append(pose_frame.header.seq) + pose_header_stamp.append(pose_frame.header.stamp) + pose_header_scaler.append(pose_frame.header.scaler) + pose_stamp.append(pose_frame.pose_stamp) + pose_positions_map.append(pose_frame.position_xyz) + pose_quaternions_map_from_lidar.append(pose_frame.orientation_xyzw) + pose_distance.append(pose_frame.distance) + pose_accuracy.append(pose_frame.pose_accuracy) + if not point_capture_sequence: raise LidarReplayError("LiDAR replay source contains no lio_pcl frames") + if any(count <= 0 for count in point_counts): + raise LidarReplayError("LiDAR replay contains an empty point frame") - point_offsets = [0] - point_raw: list[npt.NDArray[np.int64]] = [] - point_xyz: list[npt.NDArray[np.float64]] = [] - point_rgbi: list[npt.NDArray[np.uint32]] = [] - point_intensity: list[npt.NDArray[np.uint8]] = [] - for _, frame in point_messages: + point_offsets = np.empty(len(point_counts) + 1, dtype="= len(point_counts): + raise LidarReplayError("LiDAR source changed between bounded passes") + point_frame = decode_lio_pcl(message.payload) + if ( + message.source != "k1mqtt" + or message.received_monotonic_ns is None + or message.sequence != point_capture_sequence[point_index] + or message.received_at_epoch_ns + != point_received_at_epoch_ns[point_index] + or message.received_monotonic_ns + != point_received_monotonic_ns[point_index] + or len(message.payload) != point_payload_bytes[point_index] + or point_frame.header.seq != point_header_seq[point_index] + or point_frame.header.stamp != point_header_stamp[point_index] + or point_frame.header.scaler != point_scaler[point_index] + or len(point_frame.points) != point_counts[point_index] + ): + raise LidarReplayError("LiDAR source changed between bounded passes") + start = int(point_offsets[point_index]) + end = int(point_offsets[point_index + 1]) raw = np.asarray( - [(point.x_raw, point.y_raw, point.z_raw) for point in frame.points], + [ + (point.x_raw, point.y_raw, point.z_raw) + for point in point_frame.points + ], dtype=" npt.NDArray[np.int64]: - return np.asarray( - [getattr(message, attribute) for message, _ in messages], - dtype=" None: for name, dtype in _ARRAY_DTYPES.items(): if arrays[name].dtype != dtype: diff --git a/tests/test_e51_motion_semantic_qualification.py b/tests/test_e51_motion_semantic_qualification.py new file mode 100644 index 0000000..42f1888 --- /dev/null +++ b/tests/test_e51_motion_semantic_qualification.py @@ -0,0 +1,112 @@ +from __future__ import annotations + +from typing import Any, cast + +from k1link.compute import derive_motion_semantic_signal + +PROFILE: dict[str, float | int] = { + "minimum_motion_observations": 3, + "minimum_motion_span_seconds": 0.2, + "minimum_motion_displacement_m": 0.25, + "minimum_motion_speed_mps": 0.4, + "maximum_motion_speed_mps": 20.0, + "proximity_threshold_m": 2.0, +} + + +def _component(*, state: str = "current") -> dict[str, Any]: + return { + "state": state, + "temporal_id": 7, + "source_owner_key": "track:42", + "owner_kind": "camera-track", + "last_observed_age_seconds": 0.0 if state == "current" else 0.25, + "history_tail": [ + { + "session_seconds": 1.0, + "centroid_map_xyz_m": [0.0, 0.0, 0.0], + }, + { + "session_seconds": 1.5, + "centroid_map_xyz_m": [0.5, 0.0, 0.0], + }, + { + "session_seconds": 2.0, + "centroid_map_xyz_m": [1.0, 0.0, 0.0], + }, + ], + "semantic_provenance": { + "owner": "camera", + "labels": ["person"], + "track_ids": [42], + }, + } + + +def _geometry() -> dict[str, Any]: + return { + "evidence_state": "agree", + "range_m": 1.5, + "reason_codes": [ + "camera-semantic-with-connected-occupied-lidar-support" + ], + "semantic": { + "owner": "camera", + "label": "person", + "track_id": 42, + }, + } + + +def test_e51_derives_bounded_motion_proximity_and_semantic_evidence() -> None: + signal = derive_motion_semantic_signal( + _component(), + _geometry(), + map_frame_jump_candidate=False, + profile=PROFILE, + ) + assert signal is not None + motion = cast(dict[str, Any], signal["motion"]) + proximity = cast(dict[str, Any], signal["proximity"]) + semantic = cast(dict[str, Any], signal["semantic"]) + collision = cast(dict[str, Any], signal["collision"]) + assert motion["candidate"] is True + assert motion["speed_mps"] == 1.0 + assert motion["dynamic_class_available"] is False + assert proximity["candidate"] is True + assert semantic["available"] is True + assert semantic["confidence"]["available"] is False + assert collision["state"] == "unavailable" + assert signal["authority"]["navigation_or_safety_accepted"] is False + + +def test_e51_rejects_motion_on_map_frame_jump_without_losing_semantics() -> None: + signal = derive_motion_semantic_signal( + _component(), + _geometry(), + map_frame_jump_candidate=True, + profile=PROFILE, + ) + assert signal is not None + motion = cast(dict[str, Any], signal["motion"]) + semantic = cast(dict[str, Any], signal["semantic"]) + assert motion["candidate"] is False + assert motion["map_frame_jump_rejected"] is True + assert semantic["available"] is True + + +def test_e51_held_component_cannot_publish_current_proximity() -> None: + signal = derive_motion_semantic_signal( + _component(state="held"), + None, + map_frame_jump_candidate=False, + profile=PROFILE, + ) + assert signal is not None + freshness = cast(dict[str, Any], signal["freshness"]) + proximity = cast(dict[str, Any], signal["proximity"]) + semantic = cast(dict[str, Any], signal["semantic"]) + assert freshness["state"] == "held" + assert freshness["current_hit_backed"] is False + assert proximity["candidate"] is False + assert semantic["available"] is True diff --git a/tests/test_lidar_replay.py b/tests/test_lidar_replay.py index c098234..845f64b 100644 --- a/tests/test_lidar_replay.py +++ b/tests/test_lidar_replay.py @@ -3,6 +3,7 @@ from __future__ import annotations import json import struct from pathlib import Path +from typing import Any, cast import lz4.block import pytest @@ -11,11 +12,13 @@ from fastapi.routing import APIRoute from k1link.compute import ( K1_LIDAR_PACK_V2_PROFILE, + K1LocalSurfaceV1, LidarPipelineStage, LidarReadiness, LidarReplayError, LidarReplayPackV2, assess_lidar_profile, + build_k1_local_surface, build_lidar_replay_pack_v2, verify_lidar_replay_equivalence, ) @@ -163,7 +166,11 @@ def _capture(tmp_path: Path) -> Path: def _endpoint(router: APIRouter, path: str) -> object: for route in router.routes: - if isinstance(route, APIRoute) and route.path == path and "GET" in route.methods: + if ( + isinstance(route, APIRoute) + and route.path == path + and "GET" in (route.methods or set()) + ): return route.endpoint raise AssertionError(f"GET {path} route is missing") @@ -208,6 +215,38 @@ def test_lidar_replay_v2_retains_fields_and_passes_equivalence(tmp_path: Path) - assert pack.equivalence["array_mismatches"] == 0 rerun = verify_lidar_replay_equivalence(capture, pack) assert rerun["status"] == "passed" + + surface_output = build_k1_local_surface( + pack, + tmp_path / "local-surfaces", + display_name="synthetic replay v2 local surface", + ) + surface = K1LocalSurfaceV1(surface_output) + try: + assert surface.identity["source_pack_id"] == pack.pack_id + assert ( + surface.identity["source_pack_identity_sha256"] + == pack.manifest["identity_sha256"] + ) + assert ( + surface.identity["source_schema_version"] + == "missioncore.lidar-replay-pack/v2" + ) + assert ( + surface.identity["source_representation"] + == "lossless-lidar-replay-v2" + ) + assert surface.identity["frame_count"] == pack.point_frame_count + assert surface.identity["point_count"] == pack.point_count + surface_source = cast(dict[str, Any], surface.report["source"]) + assert surface_source["intensity_available"] is True + detail = surface.frame_detail(pack, 0) + assert detail["source_pack_id"] == pack.pack_id + assert detail["source_frame_index"] == 1 + detail_pose = cast(dict[str, Any], detail["pose"]) + assert detail_pose["binding_age_ms"] == pytest.approx(10.0) + finally: + surface.close() finally: pack.close()