feat(perception): qualify lossless lidar observations
This commit is contained in:
DCCONSTRUCTIONS
@@ -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 |
|
||||
|
||||
@@ -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.
|
||||
|
||||
@@ -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())
|
||||
@@ -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,
|
||||
|
||||
@@ -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",
|
||||
|
||||
@@ -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)
|
||||
@@ -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="<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(
|
||||
values: npt.NDArray[np.float64],
|
||||
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(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()
|
||||
|
||||
|
||||
|
||||
@@ -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="<i8")
|
||||
point_offsets[0] = 0
|
||||
np.cumsum(np.asarray(point_counts, dtype="<i8"), out=point_offsets[1:])
|
||||
point_count = int(point_offsets[-1])
|
||||
point_raw = np.empty((point_count, 3), dtype="<i8")
|
||||
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(
|
||||
[(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",
|
||||
).reshape((-1, 3))
|
||||
rgbi = np.asarray([point.rgbi for point in frame.points], dtype="<u4")
|
||||
intensity = (rgbi & np.uint32(0xFF)).astype(np.uint8)
|
||||
xyz = raw.astype(np.float64) / float(frame.header.scaler)
|
||||
point_raw.append(raw)
|
||||
point_xyz.append(xyz)
|
||||
point_rgbi.append(rgbi)
|
||||
point_intensity.append(intensity)
|
||||
point_offsets.append(point_offsets[-1] + raw.shape[0])
|
||||
rgbi = np.asarray(
|
||||
[point.rgbi for point in point_frame.points],
|
||||
dtype="<u4",
|
||||
)
|
||||
point_raw[start:end] = raw
|
||||
point_xyz[start:end] = raw.astype(np.float64) / float(
|
||||
point_frame.header.scaler
|
||||
)
|
||||
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]] = {
|
||||
"point_capture_sequence": _message_int_array(point_messages, "sequence"),
|
||||
"point_payload_bytes": np.asarray(
|
||||
[len(message.payload) for message, _ in point_messages],
|
||||
"point_capture_sequence": np.asarray(point_capture_sequence, dtype="<i8"),
|
||||
"point_payload_bytes": np.asarray(point_payload_bytes, dtype="<i8"),
|
||||
"point_received_at_epoch_ns": np.asarray(
|
||||
point_received_at_epoch_ns,
|
||||
dtype="<i8",
|
||||
),
|
||||
"point_received_at_epoch_ns": _message_int_array(
|
||||
point_messages,
|
||||
"received_at_epoch_ns",
|
||||
),
|
||||
"point_received_monotonic_ns": np.asarray(
|
||||
[message.received_monotonic_ns for message, _ in point_messages],
|
||||
point_received_monotonic_ns,
|
||||
dtype="<i8",
|
||||
),
|
||||
"point_header_seq": np.asarray(
|
||||
[frame.header.seq for _, frame in point_messages],
|
||||
dtype="<u8",
|
||||
),
|
||||
"point_header_stamp": np.asarray(
|
||||
[frame.header.stamp for _, frame in point_messages],
|
||||
"point_header_seq": np.asarray(point_header_seq, dtype="<u8"),
|
||||
"point_header_stamp": np.asarray(point_header_stamp, dtype="<i8"),
|
||||
"point_scaler": np.asarray(point_scaler, dtype="<i8"),
|
||||
"point_offsets": point_offsets,
|
||||
"point_raw_xyz": point_raw,
|
||||
"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",
|
||||
),
|
||||
"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(
|
||||
[message.received_monotonic_ns for message, _ in pose_messages],
|
||||
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],
|
||||
pose_received_monotonic_ns,
|
||||
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(
|
||||
[frame.position_xyz for _, frame in pose_messages],
|
||||
pose_positions_map,
|
||||
dtype="<f8",
|
||||
).reshape((-1, 3)),
|
||||
"pose_quaternions_map_from_lidar": np.asarray(
|
||||
[frame.orientation_xyzw for _, frame in pose_messages],
|
||||
pose_quaternions_map_from_lidar,
|
||||
dtype="<f8",
|
||||
).reshape((-1, 4)),
|
||||
"pose_distance": np.asarray(
|
||||
[frame.distance for _, frame in pose_messages],
|
||||
dtype="<f8",
|
||||
),
|
||||
"pose_accuracy": np.asarray(
|
||||
[frame.pose_accuracy for _, frame in pose_messages],
|
||||
dtype="<f8",
|
||||
),
|
||||
"pose_distance": np.asarray(pose_distance, dtype="<f8"),
|
||||
"pose_accuracy": np.asarray(pose_accuracy, dtype="<f8"),
|
||||
}
|
||||
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:
|
||||
for name, dtype in _ARRAY_DTYPES.items():
|
||||
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
|
||||
@@ -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()
|
||||
|
||||
|
||||
Reference in New Issue
Block a user