feat(perception): qualify lossless lidar observations

This commit is contained in:
DCCONSTRUCTIONS
2026-07-30 11:37:57 +03:00
parent e56fa0c074
commit 7d1a70d8e0
13 changed files with 1874 additions and 119 deletions
+5
View File
@@ -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 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 E37E40 into blind truth and does not block R2/R4 operational work. | | 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 E37E40 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`. | | 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. | | Product interface | DEFERRED — no new windows, page anatomy or design changes are part of this stabilization increment. |
The governing decision is The governing decision is
@@ -26,6 +28,9 @@ The governing decision is
Historical E37E40 artifacts remain immutable; only the claims made from them Historical E37E40 artifacts remain immutable; only the claims made from them
change. 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 ## Earlier checkpoint — 2026-07-24
| Stage | Result | | Stage | Result |
+52 -6
View File
@@ -10,7 +10,8 @@ parallel geometry-only replay implemented; E30E35 source-scoped qualification
accepted; RAVNOVES00 reference-source product maturation active; E36 transfer accepted; RAVNOVES00 reference-source product maturation active; E36 transfer
preregistered and deferred by ADR 0030/0032; E41 methodology boundary, E42 preregistered and deferred by ADR 0030/0032; E41 methodology boundary, E42
metamorphic checks, E44 amplification audit and E50 exact-content reference 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 Scope: passively received real-time K1 point/pose evidence, immutable replay and
future live shadow processing future live shadow processing
Explicitly out of scope: K1 firmware modification, a new onboard exporter, new 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 - [x] Bind the immutable `RAVNOVES00` E10 source by pack identity and artifact
hash without copying or rewriting the source generation. 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. preserving its separate identity and field-retention contract.
- [x] Reject stale pose binding and publish pose-binding age explicitly; keep - [x] Reject stale pose binding and publish pose-binding age explicitly; keep
map-native processing honest instead of claiming that pose inversion 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. Do not infer `free` merely because a mapped point is absent.
- [x] Leave the immutable persistent reconstruction untouched by the local - [x] Leave the immutable persistent reconstruction untouched by the local
derivative. derivative.
- [ ] Add recent-collision and dynamic-observation layers as separate - [x] Add a separate diagnostic motion-observation and near-occupied proximity
derivatives with independent decay and provenance. derivative with temporal freshness, source provenance and no persistent-map
- [ ] Reuse the accepted camera-to-LiDAR projection as an optional semantic 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. layer with source, confidence, freshness and conflict fields.
- [x] Qualify next-frame prediction and temporal stability with the evaluated - [x] Qualify next-frame prediction and temporal stability with the evaluated
frame excluded from prediction input. 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 - [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 evidence so a selected tail can be explained spatially instead of only by an
aggregate p95. 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. point age, obstacle preservation and memory growth.
- [x] Replay the same profile through a bounded latest-wins shadow queue at the - [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 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 These values describe this recording only; they are not calibration, ground
truth or a navigation gate. 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 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 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 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 or acquisition behavior is the test subject. Commands, free-space, navigation
and safety authority remain disabled. 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 Exit: one immutable K1 session yields both a persistent reconstruction and a
bounded local world state without hard-coded terrain height or scanner-side bounded local world state without hard-coded terrain height or scanner-side
changes. changes.
@@ -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.
@@ -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.
@@ -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
}
}
@@ -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())
+15 -1
View File
@@ -6,9 +6,12 @@ import json
from pathlib import Path from pathlib import Path
from k1link.compute import ( from k1link.compute import (
E10_LIDAR_PACK_SCHEMA,
LIDAR_REPLAY_PACK_SCHEMA,
E10LidarFieldSource, E10LidarFieldSource,
K1LocalSurfaceProfile, K1LocalSurfaceProfile,
K1LocalSurfaceV1, K1LocalSurfaceV1,
LidarReplayPackV2,
build_k1_local_surface, build_k1_local_surface,
) )
@@ -43,7 +46,18 @@ def main() -> int:
obstacle_min_height_m=arguments.obstacle_min_height_m, obstacle_min_height_m=arguments.obstacle_min_height_m,
obstacle_max_height_m=arguments.obstacle_max_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: try:
output = build_k1_local_surface( output = build_k1_local_surface(
source, source,
+22
View File
@@ -40,6 +40,18 @@ from .e33_worker_shadow import (
read_e33_worker_shadow_result, read_e33_worker_shadow_result,
run_e33_worker_shadow, 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 ( from .evaluation_pack import (
ANNOTATION_CONTRACT_SCHEMA, ANNOTATION_CONTRACT_SCHEMA,
EVALUATION_PACK_SCHEMA, EVALUATION_PACK_SCHEMA,
@@ -397,6 +409,16 @@ __all__ = [
"E33WorkerShadowResult", "E33WorkerShadowResult",
"read_e33_worker_shadow_result", "read_e33_worker_shadow_result",
"run_e33_worker_shadow", "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_annotation_template",
"build_lidar_ground_benchmark", "build_lidar_ground_benchmark",
"build_k1_local_surface", "build_k1_local_surface",
@@ -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)
+229 -18
View File
@@ -24,6 +24,7 @@ from .lidar_field_review import (
RAVNOVES00_CENTRAL_WINDOWS, RAVNOVES00_CENTRAL_WINDOWS,
E10LidarFieldSource, E10LidarFieldSource,
) )
from .lidar_replay import LIDAR_REPLAY_PACK_SCHEMA, LidarReplayPackV2
K1_LOCAL_SURFACE_SCHEMA: Final = "missioncore.k1-local-surface/v1" K1_LOCAL_SURFACE_SCHEMA: Final = "missioncore.k1-local-surface/v1"
K1_LOCAL_SURFACE_REPORT_SCHEMA: Final = "missioncore.k1-local-surface-report/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}$") _E10_PACK_ID = re.compile(r"^e10-lidar-pack-[a-f0-9]{64}$")
_SHA256 = re.compile(r"^[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) @dataclass(frozen=True, slots=True)
class K1LocalSurfaceProfile: class K1LocalSurfaceProfile:
@@ -511,9 +531,11 @@ class K1LocalSurfaceV1:
def frame_detail( def frame_detail(
self, self,
source: E10LidarFieldSource, source: LocalSurfaceSource,
frame_index: int, frame_index: int,
) -> dict[str, object]: ) -> dict[str, object]:
source_view = _local_surface_source_view(source)
source = source_view
_validate_source_binding(self, source) _validate_source_binding(self, source)
if not 0 <= frame_index < source.frame_count: if not 0 <= frame_index < source.frame_count:
raise IndexError(frame_index) raise IndexError(frame_index)
@@ -550,7 +572,7 @@ class K1LocalSurfaceV1:
"schema_version": K1_LOCAL_SURFACE_FRAME_SCHEMA, "schema_version": K1_LOCAL_SURFACE_FRAME_SCHEMA,
"model_id": self.model_id, "model_id": self.model_id,
"source_pack_id": source.pack_id, "source_pack_id": source.pack_id,
"session_id": source.identity["session_id"], "session_id": source.session_id,
"frame_index": frame_index, "frame_index": frame_index,
"frame_count": source.frame_count, "frame_count": source.frame_count,
"source_frame_index": int(source.arrays["source_frame_indices"][frame_index]), "source_frame_index": int(source.arrays["source_frame_indices"][frame_index]),
@@ -716,7 +738,9 @@ class K1LocalSurfaceV1:
"ground_truth": False, "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) _validate_source_binding(self, source)
frame_count = source.frame_count frame_count = source.frame_count
if self.has_temporal_qualification: if self.has_temporal_qualification:
@@ -745,7 +769,7 @@ class K1LocalSurfaceV1:
"schema_version": K1_LOCAL_SURFACE_TIMELINE_SCHEMA, "schema_version": K1_LOCAL_SURFACE_TIMELINE_SCHEMA,
"model_id": self.model_id, "model_id": self.model_id,
"source_pack_id": source.pack_id, "source_pack_id": source.pack_id,
"session_id": source.identity["session_id"], "session_id": source.session_id,
"frame_count": frame_count, "frame_count": frame_count,
"source_frame_index": source.arrays["source_frame_indices"] "source_frame_index": source.arrays["source_frame_indices"]
.astype(np.int64) .astype(np.int64)
@@ -772,7 +796,9 @@ class K1LocalSurfaceV1:
"authority": self.report["authority"], "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) _validate_source_binding(self, source)
criteria = self._review_criteria() criteria = self._review_criteria()
reason_counts = { reason_counts = {
@@ -788,7 +814,7 @@ class K1LocalSurfaceV1:
"review_profile_id": K1_LOCAL_SURFACE_REVIEW_PROFILE_ID, "review_profile_id": K1_LOCAL_SURFACE_REVIEW_PROFILE_ID,
"model_id": self.model_id, "model_id": self.model_id,
"source_pack_id": source.pack_id, "source_pack_id": source.pack_id,
"session_id": source.identity["session_id"], "session_id": source.session_id,
"available": False, "available": False,
"criteria": criteria, "criteria": criteria,
"summary": { "summary": {
@@ -918,7 +944,7 @@ class K1LocalSurfaceV1:
"review_profile_id": K1_LOCAL_SURFACE_REVIEW_PROFILE_ID, "review_profile_id": K1_LOCAL_SURFACE_REVIEW_PROFILE_ID,
"model_id": self.model_id, "model_id": self.model_id,
"source_pack_id": source.pack_id, "source_pack_id": source.pack_id,
"session_id": source.identity["session_id"], "session_id": source.session_id,
"available": True, "available": True,
"criteria": criteria, "criteria": criteria,
"summary": { "summary": {
@@ -985,7 +1011,7 @@ class K1LocalSurfaceV1:
def build_k1_local_surface( def build_k1_local_surface(
source: E10LidarFieldSource, source: LocalSurfaceSource,
output_root: Path, output_root: Path,
*, *,
profile: K1LocalSurfaceProfile = DEFAULT_K1_LOCAL_SURFACE_PROFILE, 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: if not display_name.strip() or len(display_name) > 200:
raise LidarGroundError("K1 local-surface display name is invalid") raise LidarGroundError("K1 local-surface display name is invalid")
source_view = _local_surface_source_view(source)
source = source_view
started = time.perf_counter() started = time.perf_counter()
frame_count = source.frame_count frame_count = source.frame_count
point_count = source.point_count point_count = source.point_count
@@ -1174,9 +1202,11 @@ def build_k1_local_surface(
identity = { identity = {
"schema_version": K1_LOCAL_SURFACE_SCHEMA, "schema_version": K1_LOCAL_SURFACE_SCHEMA,
"source_pack_id": source.pack_id, "source_pack_id": source.pack_id,
"source_pack_identity_sha256": source.manifest["identity_sha256"], "source_pack_identity_sha256": source.identity_sha256,
"source_artifact_sha256": source.manifest["artifact"]["sha256"], "source_artifact_sha256": source.artifact_sha256,
"session_id": source.identity["session_id"], "source_schema_version": source.schema_version,
"source_representation": source.representation,
"session_id": source.session_id,
"display_name": display_name, "display_name": display_name,
"frame_count": frame_count, "frame_count": frame_count,
"valid_frame_count": int(np.count_nonzero(valid)), "valid_frame_count": int(np.count_nonzero(valid)),
@@ -1204,12 +1234,15 @@ def build_k1_local_surface(
"schema_version": K1_LOCAL_SURFACE_REPORT_SCHEMA, "schema_version": K1_LOCAL_SURFACE_REPORT_SCHEMA,
"model_id": model_id, "model_id": model_id,
"display_name": display_name, "display_name": display_name,
"session_id": source.identity["session_id"], "session_id": source.session_id,
"source_pack_id": source.pack_id, "source_pack_id": source.pack_id,
"status": "diagnostic-only", "status": "diagnostic-only",
"ground_truth": False, "ground_truth": False,
"source": { "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, "immutable": True,
"passive_processing_only": True, "passive_processing_only": True,
"firmware_or_device_commands_used": False, "firmware_or_device_commands_used": False,
@@ -1725,7 +1758,7 @@ def _height_above_plane(
def _anchors( def _anchors(
source: E10LidarFieldSource, source: _LocalSurfaceSourceView,
valid: npt.NDArray[np.bool_], valid: npt.NDArray[np.bool_],
) -> list[dict[str, object]]: ) -> list[dict[str, object]]:
source_indices = source.arrays["source_frame_indices"] source_indices = source.arrays["source_frame_indices"]
@@ -1737,6 +1770,18 @@ def _anchors(
for window in RAVNOVES00_CENTRAL_WINDOWS: for window in RAVNOVES00_CENTRAL_WINDOWS:
if candidates.size == 0: if candidates.size == 0:
frame_index = 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: else:
frame_index = int( frame_index = int(
candidates[ candidates[
@@ -1763,20 +1808,185 @@ def _anchors(
def _validate_source_binding( def _validate_source_binding(
model: K1LocalSurfaceV1, model: K1LocalSurfaceV1,
source: E10LidarFieldSource, source: _LocalSurfaceSourceView,
) -> None: ) -> None:
if ( if (
model.identity.get("source_pack_id") != source.pack_id model.identity.get("source_pack_id") != source.pack_id
or model.identity.get("source_pack_identity_sha256") or model.identity.get("source_pack_identity_sha256")
!= source.manifest.get("identity_sha256") != source.identity_sha256
or model.identity.get("source_artifact_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("frame_count") != source.frame_count
or model.identity.get("point_count") != source.point_count or model.identity.get("point_count") != source.point_count
): ):
raise LidarGroundError("K1 local-surface source binding is invalid") 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="<i8",
)
pose_times = np.asarray(
source.arrays["pose_received_monotonic_ns"],
dtype="<i8",
)
if point_times.size < 1 or np.any(np.diff(point_times) <= 0):
raise LidarGroundError("LiDAR replay v2 point time is not strictly increasing")
pose_indices, pose_delta_ms = _nearest_pose_indices(point_times, pose_times)
if pose_times.size:
positions = np.asarray(
source.arrays["pose_positions_map"][pose_indices],
dtype="<f8",
)
quaternions = np.asarray(
source.arrays["pose_quaternions_map_from_lidar"][pose_indices],
dtype="<f8",
)
else:
positions = np.zeros((source.point_frame_count, 3), dtype="<f8")
quaternions = np.tile(
np.asarray([0.0, 0.0, 0.0, 1.0], dtype="<f8"),
(source.point_frame_count, 1),
)
origin_ns = int(
min(
int(point_times[0]),
int(pose_times[0]) if pose_times.size else int(point_times[0]),
)
)
session_seconds = (
point_times.astype(np.float64) - float(origin_ns)
) / 1_000_000_000.0
arrays = {
"source_frame_indices": np.asarray(
source.arrays["point_capture_sequence"],
dtype="<i8",
),
"session_seconds": np.asarray(session_seconds, dtype="<f8"),
"sample_available": np.ones(source.point_frame_count, dtype="?"),
"cloud_offsets": np.asarray(source.arrays["point_offsets"], dtype="<i8"),
"cloud_points_map": np.asarray(
source.arrays["point_xyz_map"],
dtype="<f8",
),
"pose_positions_map": positions,
"pose_quaternions_map_from_lidar": quaternions,
"pose_point_delta_ms": pose_delta_ms,
}
field_retention_value = source.identity.get("field_retention")
field_retention = (
cast(dict[str, object], field_retention_value)
if isinstance(field_retention_value, dict)
else None
)
return _LocalSurfaceSourceView(
pack_id=source.pack_id,
identity_sha256=identity_sha256,
artifact_sha256=artifact_sha256,
session_id=session_id,
representation="lossless-lidar-replay-v2",
schema_version=LIDAR_REPLAY_PACK_SCHEMA,
intensity_available=True,
field_retention=field_retention,
arrays=arrays,
frame_count=source.point_frame_count,
point_count=source.point_count,
)
def _lidar_replay_arrays_sha256(source: LidarReplayPackV2) -> 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="<i8"),
np.full(point_times.shape[0], np.inf, dtype="<f8"),
)
if np.any(np.diff(pose_times) < 0):
raise LidarGroundError("LiDAR replay v2 pose time is not monotonic")
right = np.searchsorted(pose_times, point_times, side="left")
right = np.clip(right, 0, pose_times.shape[0] - 1)
left = np.maximum(right - 1, 0)
right_delta = np.abs(pose_times[right] - point_times)
left_delta = np.abs(pose_times[left] - point_times)
indices = np.where(left_delta <= right_delta, left, right).astype("<i8")
delta_ms = (
np.abs(pose_times[indices] - point_times).astype(np.float64)
/ 1_000_000.0
)
return indices, np.asarray(delta_ms, dtype="<f8")
def _valid_distribution( def _valid_distribution(
values: npt.NDArray[np.float64], values: npt.NDArray[np.float64],
mask: npt.NDArray[np.bool_], mask: npt.NDArray[np.bool_],
@@ -1810,7 +2020,8 @@ def _logical_sha256(arrays: Mapping[str, npt.NDArray[Any]]) -> str:
digest.update(name.encode()) digest.update(name.encode())
digest.update(array.dtype.str.encode()) digest.update(array.dtype.str.encode())
digest.update(_canonical_json(list(array.shape))) 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() return digest.hexdigest()
+144 -93
View File
@@ -21,12 +21,9 @@ from k1link.data_plane import (
DecodedPoseView, DecodedPoseView,
) )
from k1link.device_plugins.xgrids_k1.protocol.streams import ( from k1link.device_plugins.xgrids_k1.protocol.streams import (
LioPointCloudFrame,
LioPoseFrame,
decode_lio_pcl, decode_lio_pcl,
decode_lio_pose, 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 k1link.device_plugins.xgrids_k1.viewer.replay import iter_replay_messages
from .lidar_contract import ( from .lidar_contract import (
@@ -86,6 +83,27 @@ class LidarReplayError(ValueError):
"""A replay pack or its source evidence violates the v2 contract.""" """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) @dataclass(frozen=True, slots=True)
class LidarReplayPointFrame: class LidarReplayPointFrame:
capture_sequence: int capture_sequence: int
@@ -180,7 +198,7 @@ class LidarReplayPackV2:
self.arrays_path = artifacts["lidar-arrays"] self.arrays_path = artifacts["lidar-arrays"]
self.quality_path = artifacts["lidar-quality"] self.quality_path = artifacts["lidar-quality"]
self.equivalence_path = artifacts["live-replay-equivalence"] 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): if set(self.arrays.files) != set(_ARRAY_DTYPES):
self.close() self.close()
raise LidarReplayError("LiDAR replay array set is incompatible") raise LidarReplayError("LiDAR replay array set is incompatible")
@@ -413,6 +431,7 @@ def build_lidar_replay_pack_v2(
"artifacts": artifacts, "artifacts": artifacts,
} }
_write_json(staging / LIDAR_MANIFEST_NAME, manifest) _write_json(staging / LIDAR_MANIFEST_NAME, manifest)
del arrays
os.replace(staging, output) os.replace(staging, output)
try: try:
validation = LidarReplayPackV2(output) 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]]: def _capture_arrays(path: Path) -> dict[str, npt.NDArray[Any]]:
point_messages: list[tuple[StreamMessage, LioPointCloudFrame]] = [] point_capture_sequence: list[int] = []
pose_messages: list[tuple[StreamMessage, LioPoseFrame]] = [] 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): for message in iter_replay_messages(path):
if message.source != "k1mqtt" or message.received_monotonic_ns is None: if message.source != "k1mqtt" or message.received_monotonic_ns is None:
raise LidarReplayError("LiDAR v2 requires native capture with exact host time") raise LidarReplayError("LiDAR v2 requires native capture with exact host time")
if message.topic.endswith(_POINT_TOPIC_SUFFIX): 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): elif message.topic.endswith(_POSE_TOPIC_SUFFIX):
pose_messages.append((message, decode_lio_pose(message.payload))) pose_frame = decode_lio_pose(message.payload)
if not point_messages: 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") 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_offsets = np.empty(len(point_counts) + 1, dtype="<i8")
point_raw: list[npt.NDArray[np.int64]] = [] point_offsets[0] = 0
point_xyz: list[npt.NDArray[np.float64]] = [] np.cumsum(np.asarray(point_counts, dtype="<i8"), out=point_offsets[1:])
point_rgbi: list[npt.NDArray[np.uint32]] = [] point_count = int(point_offsets[-1])
point_intensity: list[npt.NDArray[np.uint8]] = [] point_raw = np.empty((point_count, 3), dtype="<i8")
for _, frame in point_messages: point_xyz = np.empty((point_count, 3), dtype="<f8")
point_rgbi = np.empty(point_count, dtype="<u4")
point_intensity = np.empty(point_count, dtype="u1")
point_index = 0
for message in iter_replay_messages(path):
if not message.topic.endswith(_POINT_TOPIC_SUFFIX):
continue
if point_index >= 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( 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="<i8", dtype="<i8",
).reshape((-1, 3)) ).reshape((-1, 3))
rgbi = np.asarray([point.rgbi for point in frame.points], dtype="<u4") rgbi = np.asarray(
intensity = (rgbi & np.uint32(0xFF)).astype(np.uint8) [point.rgbi for point in point_frame.points],
xyz = raw.astype(np.float64) / float(frame.header.scaler) dtype="<u4",
point_raw.append(raw) )
point_xyz.append(xyz) point_raw[start:end] = raw
point_rgbi.append(rgbi) point_xyz[start:end] = raw.astype(np.float64) / float(
point_intensity.append(intensity) point_frame.header.scaler
point_offsets.append(point_offsets[-1] + raw.shape[0]) )
point_rgbi[start:end] = rgbi
point_intensity[start:end] = (rgbi & np.uint32(0xFF)).astype(np.uint8)
point_index += 1
if point_index != len(point_counts):
raise LidarReplayError("LiDAR source changed between bounded passes")
arrays: dict[str, npt.NDArray[Any]] = { arrays: dict[str, npt.NDArray[Any]] = {
"point_capture_sequence": _message_int_array(point_messages, "sequence"), "point_capture_sequence": np.asarray(point_capture_sequence, dtype="<i8"),
"point_payload_bytes": np.asarray( "point_payload_bytes": np.asarray(point_payload_bytes, dtype="<i8"),
[len(message.payload) for message, _ in point_messages], "point_received_at_epoch_ns": np.asarray(
point_received_at_epoch_ns,
dtype="<i8", dtype="<i8",
), ),
"point_received_at_epoch_ns": _message_int_array(
point_messages,
"received_at_epoch_ns",
),
"point_received_monotonic_ns": np.asarray( "point_received_monotonic_ns": np.asarray(
[message.received_monotonic_ns for message, _ in point_messages], point_received_monotonic_ns,
dtype="<i8", dtype="<i8",
), ),
"point_header_seq": np.asarray( "point_header_seq": np.asarray(point_header_seq, dtype="<u8"),
[frame.header.seq for _, frame in point_messages], "point_header_stamp": np.asarray(point_header_stamp, dtype="<i8"),
dtype="<u8", "point_scaler": np.asarray(point_scaler, dtype="<i8"),
), "point_offsets": point_offsets,
"point_header_stamp": np.asarray( "point_raw_xyz": point_raw,
[frame.header.stamp for _, frame in point_messages], "point_xyz_map": point_xyz,
"point_rgbi": point_rgbi,
"point_intensity": point_intensity,
"pose_capture_sequence": np.asarray(pose_capture_sequence, dtype="<i8"),
"pose_payload_bytes": np.asarray(pose_payload_bytes, dtype="<i8"),
"pose_received_at_epoch_ns": np.asarray(
pose_received_at_epoch_ns,
dtype="<i8", dtype="<i8",
), ),
"point_scaler": np.asarray(
[frame.header.scaler for _, frame in point_messages],
dtype="<i8",
),
"point_offsets": np.asarray(point_offsets, dtype="<i8"),
"point_raw_xyz": np.concatenate(point_raw),
"point_xyz_map": np.concatenate(point_xyz),
"point_rgbi": np.concatenate(point_rgbi),
"point_intensity": np.concatenate(point_intensity),
"pose_capture_sequence": _message_int_array(pose_messages, "sequence"),
"pose_payload_bytes": np.asarray(
[len(message.payload) for message, _ in pose_messages],
dtype="<i8",
),
"pose_received_at_epoch_ns": _message_int_array(
pose_messages,
"received_at_epoch_ns",
),
"pose_received_monotonic_ns": np.asarray( "pose_received_monotonic_ns": np.asarray(
[message.received_monotonic_ns for message, _ in pose_messages], pose_received_monotonic_ns,
dtype="<i8",
),
"pose_header_seq": np.asarray(
[frame.header.seq for _, frame in pose_messages],
dtype="<u8",
),
"pose_header_stamp": np.asarray(
[frame.header.stamp for _, frame in pose_messages],
dtype="<i8",
),
"pose_header_scaler": np.asarray(
[frame.header.scaler for _, frame in pose_messages],
dtype="<i8",
),
"pose_stamp": np.asarray(
[frame.pose_stamp for _, frame in pose_messages],
dtype="<i8", dtype="<i8",
), ),
"pose_header_seq": np.asarray(pose_header_seq, dtype="<u8"),
"pose_header_stamp": np.asarray(pose_header_stamp, dtype="<i8"),
"pose_header_scaler": np.asarray(pose_header_scaler, dtype="<i8"),
"pose_stamp": np.asarray(pose_stamp, dtype="<i8"),
"pose_positions_map": np.asarray( "pose_positions_map": np.asarray(
[frame.position_xyz for _, frame in pose_messages], pose_positions_map,
dtype="<f8", dtype="<f8",
).reshape((-1, 3)), ).reshape((-1, 3)),
"pose_quaternions_map_from_lidar": np.asarray( "pose_quaternions_map_from_lidar": np.asarray(
[frame.orientation_xyzw for _, frame in pose_messages], pose_quaternions_map_from_lidar,
dtype="<f8", dtype="<f8",
).reshape((-1, 4)), ).reshape((-1, 4)),
"pose_distance": np.asarray( "pose_distance": np.asarray(pose_distance, dtype="<f8"),
[frame.distance for _, frame in pose_messages], "pose_accuracy": np.asarray(pose_accuracy, dtype="<f8"),
dtype="<f8",
),
"pose_accuracy": np.asarray(
[frame.pose_accuracy for _, frame in pose_messages],
dtype="<f8",
),
} }
return arrays return arrays
def _message_int_array(
messages: list[tuple[StreamMessage, Any]],
attribute: str,
) -> npt.NDArray[np.int64]:
return np.asarray(
[getattr(message, attribute) for message, _ in messages],
dtype="<i8",
)
def _validate_arrays(arrays: Any, identity: dict[str, Any]) -> None: def _validate_arrays(arrays: Any, identity: dict[str, Any]) -> None:
for name, dtype in _ARRAY_DTYPES.items(): for name, dtype in _ARRAY_DTYPES.items():
if arrays[name].dtype != dtype: if arrays[name].dtype != dtype:
@@ -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
+40 -1
View File
@@ -3,6 +3,7 @@ from __future__ import annotations
import json import json
import struct import struct
from pathlib import Path from pathlib import Path
from typing import Any, cast
import lz4.block import lz4.block
import pytest import pytest
@@ -11,11 +12,13 @@ from fastapi.routing import APIRoute
from k1link.compute import ( from k1link.compute import (
K1_LIDAR_PACK_V2_PROFILE, K1_LIDAR_PACK_V2_PROFILE,
K1LocalSurfaceV1,
LidarPipelineStage, LidarPipelineStage,
LidarReadiness, LidarReadiness,
LidarReplayError, LidarReplayError,
LidarReplayPackV2, LidarReplayPackV2,
assess_lidar_profile, assess_lidar_profile,
build_k1_local_surface,
build_lidar_replay_pack_v2, build_lidar_replay_pack_v2,
verify_lidar_replay_equivalence, verify_lidar_replay_equivalence,
) )
@@ -163,7 +166,11 @@ def _capture(tmp_path: Path) -> Path:
def _endpoint(router: APIRouter, path: str) -> object: def _endpoint(router: APIRouter, path: str) -> object:
for route in router.routes: 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 return route.endpoint
raise AssertionError(f"GET {path} route is missing") 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 assert pack.equivalence["array_mismatches"] == 0
rerun = verify_lidar_replay_equivalence(capture, pack) rerun = verify_lidar_replay_equivalence(capture, pack)
assert rerun["status"] == "passed" 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: finally:
pack.close() pack.close()