feat(perception): qualify lidar evidence before models
This commit is contained in:
DCCONSTRUCTIONS
parent
37c24b5fa8
commit
3f549f91f1
|
|
@ -1,5 +1,12 @@
|
||||||
# External perception worker contract
|
# External perception worker contract
|
||||||
|
|
||||||
|
The LiDAR-native extension is governed by
|
||||||
|
`missioncore.lidar-evidence-profile/v1` and
|
||||||
|
`docs/13_LIDAR_WORKER_PRODUCT_AND_ROADMAP.md`. A worker process starting
|
||||||
|
successfully is not evidence that its LiDAR assumptions are satisfied. In
|
||||||
|
particular, the accepted E10 replay pack v1 has no intensity, and the current K1
|
||||||
|
stream is a vendor map increment rather than an unregistered sensor sweep.
|
||||||
|
|
||||||
## Boundary
|
## Boundary
|
||||||
|
|
||||||
```text
|
```text
|
||||||
|
|
|
||||||
|
|
@ -2,6 +2,11 @@
|
||||||
|
|
||||||
Status date: 2026-07-20.
|
Status date: 2026-07-20.
|
||||||
|
|
||||||
|
LiDAR-native work after E26 follows
|
||||||
|
`docs/13_LIDAR_WORKER_PRODUCT_AND_ROADMAP.md`: lossless replay and scanner
|
||||||
|
quality first, then ground segmentation and a PointPillars baseline. Alternative
|
||||||
|
odometry/SLAM is not admitted for the current vendor-mapped K1 stream.
|
||||||
|
|
||||||
## Outcome
|
## Outcome
|
||||||
|
|
||||||
Mission Core will use the K1 factory camera/LiDAR calibration instead of
|
Mission Core will use the K1 factory camera/LiDAR calibration instead of
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,255 @@
|
||||||
|
# LiDAR worker: product value, evidence boundary and implementation roadmap
|
||||||
|
|
||||||
|
Date: 2026-07-25
|
||||||
|
Status: accepted architecture plan; L0 contract implemented
|
||||||
|
Scope: real scanner records, replay and future live shadow processing
|
||||||
|
Explicitly out of scope: Unreal U0/U1, Gaussian assets and simulator rendering
|
||||||
|
|
||||||
|
## 1. Decision
|
||||||
|
|
||||||
|
Mission Core does not treat “LiDAR processing” as one model. It owns four
|
||||||
|
separate products:
|
||||||
|
|
||||||
|
1. scanner evidence and quality;
|
||||||
|
2. geometric preprocessing and map quality;
|
||||||
|
3. object/occupancy perception;
|
||||||
|
4. planner-facing local world state.
|
||||||
|
|
||||||
|
Simulation can exercise their contracts and generate synthetic regressions, but
|
||||||
|
it cannot prove real scanner timing, returns, reflectance, calibration, motion
|
||||||
|
distortion or environmental failure modes.
|
||||||
|
|
||||||
|
The AI worker remains external, Linux/NVIDIA-capable and replaceable. React is
|
||||||
|
the control and review surface. Mission Core owns immutable inputs, exact
|
||||||
|
profiles, result identities, acceptance gates and diagnostic-only authority.
|
||||||
|
Models do not gain scanner, navigation, command or safety authority.
|
||||||
|
|
||||||
|
## 2. What the current K1 source actually contains
|
||||||
|
|
||||||
|
The firmware-3 `lio_pcl` stream currently exposes:
|
||||||
|
|
||||||
|
- finite XYZ points already expressed in the canonical `map` frame;
|
||||||
|
- one verified uint8 intensity value in the low byte of `rgbi`;
|
||||||
|
- a frame header with sequence, stamp and scaler;
|
||||||
|
- a separate `T_map_from_lidar` pose stream.
|
||||||
|
|
||||||
|
It does not currently expose an admitted:
|
||||||
|
|
||||||
|
- raw sensor-frame sweep;
|
||||||
|
- per-point firing time;
|
||||||
|
- ring/channel number;
|
||||||
|
- IMU sample stream;
|
||||||
|
- LiDAR/IMU extrinsic;
|
||||||
|
- proven common hardware clock for LiDAR and cameras;
|
||||||
|
- LiDAR scan model required by projective integrations.
|
||||||
|
|
||||||
|
The accepted `missioncore.e10-lidar-replay-pack/v1` narrows this further: it
|
||||||
|
keeps XYZ, pose and best-effort camera binding, but drops intensity. It must not
|
||||||
|
be silently mutated because E10–E26 results are content-bound to that schema.
|
||||||
|
|
||||||
|
These facts have architectural consequences:
|
||||||
|
|
||||||
|
- current K1 points can support display, calibrated projection, persistent
|
||||||
|
support and bounded geometric analysis;
|
||||||
|
- the live stream can be converted back to a pose-relative sensor XYZI tensor;
|
||||||
|
- the v1 replay pack cannot feed the admitted NVIDIA PointPillars baseline;
|
||||||
|
- KISS-ICP, KISS-SLAM, FAST-LIO2, LIO-SAM or GLIM cannot honestly rebuild K1
|
||||||
|
odometry from points that are already vendor-mapped;
|
||||||
|
- deskew and LiDAR-inertial SLAM are blocked until the scanner or a future
|
||||||
|
vehicle LiDAR driver supplies raw scans, timing and IMU evidence.
|
||||||
|
|
||||||
|
The executable truth is
|
||||||
|
`missioncore.lidar-evidence-profile/v1` in
|
||||||
|
`src/k1link/compute/lidar_contract.py`. It assesses each stage as `ready`,
|
||||||
|
`degraded` or `blocked` and refuses to infer absent sensor fields.
|
||||||
|
|
||||||
|
## 3. Current system result
|
||||||
|
|
||||||
|
The existing camera-heavy E10–E26 line is valuable and remains in place:
|
||||||
|
|
||||||
|
- raw recording and content-addressed worker handoff;
|
||||||
|
- camera detection and semantic segmentation;
|
||||||
|
- factory KB4 camera/LiDAR projection;
|
||||||
|
- map-frame LiDAR support and ground-aware cuboids;
|
||||||
|
- bounded temporal stabilization;
|
||||||
|
- persistent support motion evidence;
|
||||||
|
- camera ego-motion evidence and conservative fusion.
|
||||||
|
|
||||||
|
E25 proved the LiDAR limit: missing current returns cannot be recovered by
|
||||||
|
threshold tuning. E26 passed its reviewed 11/11 diagnostic windows by adding
|
||||||
|
camera parallax, but it is not planner-ready: most camera observations remain
|
||||||
|
unknown, camera-only velocity is not metric, 374 conflicts remain, and the
|
||||||
|
benchmark is not independent ground truth.
|
||||||
|
|
||||||
|
The next work must therefore improve the LiDAR-native input and evaluation
|
||||||
|
surface, not add another visual smoothing pass.
|
||||||
|
|
||||||
|
## 4. Market and stack assessment
|
||||||
|
|
||||||
|
### 4.1 NVIDIA components worth retaining
|
||||||
|
|
||||||
|
| Component | Correct use in Mission Core | Decision |
|
||||||
|
| --- | --- | --- |
|
||||||
|
| [TAO PointPillars](https://docs.nvidia.com/tao/tao-toolkit/latest/text/cv_finetuning/pytorch/point_cloud/pointpillars.html) | Train/evaluate a LiDAR-native 3D detector over sensor-frame XYZI | First neural 3D baseline after replay v2 and labels |
|
||||||
|
| [DeepStream LiDAR 3D inference](https://docs.nvidia.com/metropolis/deepstream/7.1/text/DS_3D_Lidar_Inference.html) | Reference production pipeline for XYZI → Triton/TensorRT → 3D boxes | Reuse the inference pattern, not its file loader or UI |
|
||||||
|
| [TAO Deploy PointPillars](https://docs.nvidia.com/tao/tao-toolkit/latest/text/tao_deploy/pointpillars.html) | Build a pinned FP16/FP32 TensorRT engine and evaluate it | Worker optimization only after an accuracy baseline |
|
||||||
|
| [Isaac ROS nvblox](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_nvblox/isaac_ros_nvblox/index.html) | GPU local TSDF/occupancy/ESDF and Nav2 cost-map producer | Later; blocked on admitted scan geometry and better timing |
|
||||||
|
| DeepStream multimodal 3D fusion | Later camera/LiDAR BEV baseline | Deferred until LiDAR-only, sync and calibration gates pass |
|
||||||
|
|
||||||
|
NVIDIA does not provide a magic “clean the K1 map” stage. PointPillars produces
|
||||||
|
classified 3D objects. Nvblox produces reconstruction and distance fields. The
|
||||||
|
quality of both remains bounded by source timing, calibration, scan geometry
|
||||||
|
and training-domain fit.
|
||||||
|
|
||||||
|
### 4.2 Independent components worth benchmarking
|
||||||
|
|
||||||
|
| Component | Correct use | Decision |
|
||||||
|
| --- | --- | --- |
|
||||||
|
| [Patchwork++](https://github.com/url-kaist/patchwork-plusplus) | Fast adaptive ground segmentation, including reflection-noise handling | First non-neural geometry baseline |
|
||||||
|
| [Autoware CenterPoint](https://github.com/autowarefoundation/autoware_universe/tree/main/perception/autoware_lidar_centerpoint) | Mature ROS 2/TensorRT 3D detection and multi-frame reference | Second detector baseline after PointPillars |
|
||||||
|
| [MMDetection3D](https://github.com/open-mmlab/mmdetection3d) | Training/evaluation harness and dataset adapters | Laboratory only |
|
||||||
|
| [OpenPCDet](https://github.com/open-mmlab/OpenPCDet) | Alternative LiDAR detector benchmark/model zoo | Laboratory only; not the production runtime |
|
||||||
|
| [KISS-ICP](https://github.com/PRBonn/kiss-icp) | Simple LiDAR-only odometry baseline | Future raw sensor scans only |
|
||||||
|
| [KISS-SLAM](https://github.com/PRBonn/kiss-slam) | Global LiDAR SLAM/loop-closure baseline | Future raw sensor scans only |
|
||||||
|
| [FAST-LIO2](https://github.com/hku-mars/FAST_LIO) | Raw LiDAR + IMU odometry/mapping | Future vehicle sensor profile only |
|
||||||
|
| [LIO-SAM](https://github.com/TixiaoShan/LIO-SAM) | Deskewed LiDAR-inertial factor-graph reference | Future profile with ring/time/IMU only |
|
||||||
|
| [GLIM](https://github.com/koide3/glim) | GPU-accelerated range-inertial mapping, loop correction and map cleanup | Strong future mapping candidate; blocked for current K1 evidence |
|
||||||
|
|
||||||
|
Autoware is a useful architecture and component reference; importing the whole
|
||||||
|
autonomous-driving distribution into the worker would add a large operational
|
||||||
|
surface that Mission Core does not currently need.
|
||||||
|
|
||||||
|
## 5. Target worker shape
|
||||||
|
|
||||||
|
The worker exposes provider-neutral jobs rather than one growing camera script:
|
||||||
|
|
||||||
|
| Job profile | Input | Output | Authority |
|
||||||
|
| --- | --- | --- | --- |
|
||||||
|
| `lidar-quality/v1` | immutable LiDAR evidence | field/timing/density/intensity report | diagnostic |
|
||||||
|
| `lidar-ground/v1` | sensor-frame XYZI + pose | ground/non-ground points and metrics | diagnostic |
|
||||||
|
| `lidar-3d-detection/v1` | sensor-frame XYZI | classified 3D observations + uncertainty | shadow |
|
||||||
|
| `lidar-local-map/v1` | scans + synchronized pose | occupancy/TSDF/ESDF artifacts | shadow |
|
||||||
|
| `lidar-mapping-benchmark/v1` | raw scans + optional IMU | trajectory/map comparison | offline diagnostic |
|
||||||
|
|
||||||
|
Every result is content-addressed and binds:
|
||||||
|
|
||||||
|
- source evidence identity;
|
||||||
|
- exact evidence profile/readiness document;
|
||||||
|
- model, weights, runtime and preprocessing identities;
|
||||||
|
- coordinate transforms and timestamp basis;
|
||||||
|
- resource/queue/drop telemetry;
|
||||||
|
- acceptance policy and explicit authority.
|
||||||
|
|
||||||
|
Offline replay may batch work for throughput. Live shadow uses bounded
|
||||||
|
latest-wins queues. Accuracy is frozen before FP16/INT8, CUDA graph, pinned
|
||||||
|
memory, zero-copy or batching optimizations are accepted.
|
||||||
|
|
||||||
|
## 6. Implementation sequence
|
||||||
|
|
||||||
|
### L0 — evidence truth and detector adapter — complete
|
||||||
|
|
||||||
|
- [x] Add `missioncore.lidar-evidence-profile/v1`.
|
||||||
|
- [x] Encode current live K1 and replay-pack-v1 facts.
|
||||||
|
- [x] Produce deterministic stage readiness and blockers.
|
||||||
|
- [x] Add tested `map + T_map_from_lidar + intensity → sensor XYZI`.
|
||||||
|
- [x] Add bounded live point-count, cadence and intensity telemetry to the
|
||||||
|
existing external worker report.
|
||||||
|
- [x] Keep authority diagnostic-only.
|
||||||
|
|
||||||
|
### L1 — lossless replay v2 and scanner quality
|
||||||
|
|
||||||
|
- [ ] Create a new replay schema; do not rewrite pack v1.
|
||||||
|
- [ ] Preserve XYZ, intensity, source sequence/header stamp/scaler and exact
|
||||||
|
host capture/receive times.
|
||||||
|
- [ ] Record explicitly absent ring, per-point time and IMU fields.
|
||||||
|
- [ ] Add bounded reports for frame gaps, arrival jitter, points/frame, range
|
||||||
|
distribution, intensity distribution and pose coverage.
|
||||||
|
- [ ] Compare live-vs-replay decoding byte-for-byte on a frozen slice.
|
||||||
|
- [ ] Publish the report to the React observation view without bundling a
|
||||||
|
desktop renderer.
|
||||||
|
|
||||||
|
Exit: a recording cannot be called detector-ready when required fields were
|
||||||
|
dropped, and an operator can distinguish scanner dropout from model failure.
|
||||||
|
|
||||||
|
### L2 — geometric baseline
|
||||||
|
|
||||||
|
- [ ] Run Patchwork++ over the frozen XYZI slice.
|
||||||
|
- [ ] Retain ground and non-ground outputs separately; never delete raw points.
|
||||||
|
- [ ] Label a small independent ground/obstacle evaluation set in CVAT or an
|
||||||
|
equivalent accepted annotation workspace.
|
||||||
|
- [ ] Measure ground IoU, curb/low-obstacle recall, reflection-noise rejection
|
||||||
|
and CPU/GPU latency.
|
||||||
|
- [ ] Compare against the current heuristic ground proposal branch.
|
||||||
|
|
||||||
|
Exit: an evidence-backed decision to retain or reject Patchwork++.
|
||||||
|
|
||||||
|
### L3 — LiDAR-native 3D detection
|
||||||
|
|
||||||
|
- [ ] Freeze independent 3D annotations covering people, vehicles, cyclists,
|
||||||
|
stroller groups, vegetation and confusing static structures.
|
||||||
|
- [ ] Run NVIDIA PointPillars through the existing external worker/Triton seam.
|
||||||
|
- [ ] Treat pretrained output as a baseline, not an accepted product model.
|
||||||
|
- [ ] Measure class precision/recall, center/range/yaw error, distance-bucket
|
||||||
|
recall, false occupied objects and end-to-end latency.
|
||||||
|
- [ ] Compare Autoware CenterPoint only after the PointPillars harness is stable.
|
||||||
|
- [ ] Fine-tune only if the baseline demonstrates useful transfer and the
|
||||||
|
annotation budget is justified.
|
||||||
|
|
||||||
|
Exit: the selected detector beats the camera-derived cuboid baseline on the
|
||||||
|
independent gate without increasing unsafe false-free or false-dynamic output.
|
||||||
|
|
||||||
|
### L4 — live shadow integration
|
||||||
|
|
||||||
|
- [ ] Add a bounded LiDAR queue independent of camera cadence.
|
||||||
|
- [ ] Run the accepted detector profile on the NVIDIA worker.
|
||||||
|
- [ ] Fuse LiDAR-native objects with E26 camera evidence as independent sources.
|
||||||
|
- [ ] Publish `agree`, `single-source`, `conflict` and `unknown`; unknown remains
|
||||||
|
occupied.
|
||||||
|
- [ ] Measure sensor-to-result latency, deadline misses, drops, memory and GPU
|
||||||
|
headroom on a physical run.
|
||||||
|
|
||||||
|
Exit: repeatable shadow telemetry only. Navigation and safety acceptance remain
|
||||||
|
false.
|
||||||
|
|
||||||
|
### L5 — local occupancy and Nav2
|
||||||
|
|
||||||
|
- [ ] Obtain and validate the K1/future vehicle LiDAR scan model, or use a
|
||||||
|
different admitted source that supplies it.
|
||||||
|
- [ ] Prove pose and time behavior required by nvblox.
|
||||||
|
- [ ] Benchmark static occupancy/TSDF and ESDF output on real replay.
|
||||||
|
- [ ] Keep dynamic observations in a separate decaying layer.
|
||||||
|
- [ ] Connect the accepted 2D slice to Nav2 through the existing world-state
|
||||||
|
boundary.
|
||||||
|
|
||||||
|
Exit: local collision-space quality and deadline gates pass in replay and
|
||||||
|
shadow. This still does not authorize control.
|
||||||
|
|
||||||
|
### L6 — alternative odometry/mapping
|
||||||
|
|
||||||
|
This stage starts only when a source supplies unregistered sensor scans. Add
|
||||||
|
per-point time and IMU/extrinsic requirements before FAST-LIO2, LIO-SAM or GLIM.
|
||||||
|
Benchmark KISS-ICP first, then a global SLAM candidate using ATE/RPE, loop
|
||||||
|
closure residual, wall/surface thickness, map entropy, repeat-pass alignment
|
||||||
|
and compute cost.
|
||||||
|
|
||||||
|
The current K1 `lio_pcl` stream cannot satisfy this gate.
|
||||||
|
|
||||||
|
## 7. Product value
|
||||||
|
|
||||||
|
The near-term value is not a prettier point cloud:
|
||||||
|
|
||||||
|
- a trustworthy observation tells the operator whether the scanner, transport,
|
||||||
|
pose, calibration or model failed;
|
||||||
|
- lossless replay makes model and worker upgrades repeatable;
|
||||||
|
- LiDAR-native objects reduce dependence on camera visibility and provide
|
||||||
|
metric geometry;
|
||||||
|
- ground/non-ground and local occupancy become the bridge from archive review
|
||||||
|
to route validation and later collision checking;
|
||||||
|
- the same job/result contracts accept real, replayed or simulated sources
|
||||||
|
without moving heavy compute into React;
|
||||||
|
- hardware selection becomes evidence-driven: a future vehicle LiDAR is
|
||||||
|
accepted by its timing/fields/profile, not by vendor marketing.
|
||||||
|
|
||||||
|
The highest-value immediate work is L1, followed by L2 and L3. Nvblox and
|
||||||
|
alternative SLAM are useful, but starting them before their input gates would
|
||||||
|
produce attractive demos with unqualified geometry.
|
||||||
|
|
@ -0,0 +1,55 @@
|
||||||
|
# ADR 0018: qualify LiDAR evidence before selecting accelerated models
|
||||||
|
|
||||||
|
Date: 2026-07-25
|
||||||
|
Status: accepted
|
||||||
|
|
||||||
|
## Context
|
||||||
|
|
||||||
|
Mission Core preserves K1 point clouds, poses, cameras and factory calibration
|
||||||
|
and already runs an external NVIDIA worker for camera perception. The next
|
||||||
|
candidate components include PointPillars, CenterPoint, Patchwork++, nvblox and
|
||||||
|
several LiDAR SLAM systems.
|
||||||
|
|
||||||
|
Those components do not share the same input assumptions. The current K1
|
||||||
|
`lio_pcl` source is a vendor-mapped increment in the canonical `map` frame with intensity and a
|
||||||
|
separate best-effort pose. It is not an unregistered sweep and exposes no
|
||||||
|
admitted ring, per-point time or IMU stream. The accepted E10 replay pack also
|
||||||
|
drops intensity.
|
||||||
|
|
||||||
|
Installing a component without expressing those facts would let runtime success
|
||||||
|
be mistaken for geometric validity.
|
||||||
|
|
||||||
|
## Decision
|
||||||
|
|
||||||
|
1. Mission Core owns a versioned, provider-neutral
|
||||||
|
`missioncore.lidar-evidence-profile/v1`.
|
||||||
|
2. Each processing stage receives a deterministic readiness result with
|
||||||
|
explicit blockers.
|
||||||
|
3. Existing content-addressed replay schemas are immutable. A lossless replay
|
||||||
|
improvement is a new version.
|
||||||
|
4. The first detector seam consumes sensor-frame XYZI. Map-frame K1 points are
|
||||||
|
converted only with the bound `T_map_from_lidar` pose, and verified uint8
|
||||||
|
intensity is normalized to `[0, 1]`.
|
||||||
|
5. Missing ring, point time, scan geometry or IMU evidence remains missing. It
|
||||||
|
is never synthesized to satisfy a model.
|
||||||
|
6. Accelerated models remain external worker providers. React consumes reports,
|
||||||
|
overlays and world-state products; it does not host CUDA/TensorRT/ROS 2
|
||||||
|
compute.
|
||||||
|
7. All LiDAR results remain diagnostic/shadow until separate navigation and
|
||||||
|
safety gates pass.
|
||||||
|
|
||||||
|
## Consequences
|
||||||
|
|
||||||
|
- PointPillars integration is blocked for replay pack v1 and degraded for the
|
||||||
|
current live vendor-mapped source.
|
||||||
|
- Nvblox is blocked until the LiDAR scan geometry and timing/pose contract are
|
||||||
|
admitted.
|
||||||
|
- LiDAR odometry and LiDAR-inertial SLAM are blocked for current K1 evidence.
|
||||||
|
- L1 lossless replay and scanner-quality telemetry precede new model installs.
|
||||||
|
- Future scanners, simulation providers and datasets can use the same
|
||||||
|
readiness contract without being forced into K1-specific code.
|
||||||
|
|
||||||
|
## References
|
||||||
|
|
||||||
|
The detailed product rationale, market review, gates and sequence are in
|
||||||
|
`docs/13_LIDAR_WORKER_PRODUCT_AND_ROADMAP.md`.
|
||||||
|
|
@ -92,6 +92,11 @@ from k1link.compute.inline_temporal import (
|
||||||
read_inline_profile,
|
read_inline_profile,
|
||||||
stabilize_world_state,
|
stabilize_world_state,
|
||||||
)
|
)
|
||||||
|
from k1link.compute.lidar_contract import (
|
||||||
|
K1_LIVE_LIDAR_PROFILE,
|
||||||
|
LidarQualityMonitor,
|
||||||
|
lidar_readiness_document,
|
||||||
|
)
|
||||||
from k1link.compute.live_perception import (
|
from k1link.compute.live_perception import (
|
||||||
LIVE_RESULT_MAX_PAYLOAD_BYTES,
|
LIVE_RESULT_MAX_PAYLOAD_BYTES,
|
||||||
LiveSensorSynchronizer,
|
LiveSensorSynchronizer,
|
||||||
|
|
@ -563,6 +568,7 @@ def _receiver(
|
||||||
max_duration_seconds: float,
|
max_duration_seconds: float,
|
||||||
decoder: PersistentFmp4Decoder,
|
decoder: PersistentFmp4Decoder,
|
||||||
synchronizer: LiveSensorSynchronizer,
|
synchronizer: LiveSensorSynchronizer,
|
||||||
|
lidar_quality: LidarQualityMonitor,
|
||||||
state: _TransportState,
|
state: _TransportState,
|
||||||
sensor_decode_ms: dict[str, list[float]],
|
sensor_decode_ms: dict[str, list[float]],
|
||||||
result_queue: queue.Queue[bytes],
|
result_queue: queue.Queue[bytes],
|
||||||
|
|
@ -690,6 +696,7 @@ def _receiver(
|
||||||
)
|
)
|
||||||
sensor_decode_ms[modality].append((time.perf_counter() - decode_started) * 1000)
|
sensor_decode_ms[modality].append((time.perf_counter() - decode_started) * 1000)
|
||||||
if modality == "lidar" and isinstance(normalized, DecodedPointCloudView):
|
if modality == "lidar" and isinstance(normalized, DecodedPointCloudView):
|
||||||
|
lidar_quality.observe(normalized)
|
||||||
synchronizer.publish_point_cloud(normalized)
|
synchronizer.publish_point_cloud(normalized)
|
||||||
elif modality == "pose" and isinstance(normalized, DecodedPoseView):
|
elif modality == "pose" and isinstance(normalized, DecodedPoseView):
|
||||||
synchronizer.publish_pose(normalized)
|
synchronizer.publish_pose(normalized)
|
||||||
|
|
@ -919,6 +926,7 @@ def run(args: argparse.Namespace, loaded: _LoadedModels | None = None) -> int:
|
||||||
capacity_per_modality=int(temporal["buffer_capacity_per_modality"]),
|
capacity_per_modality=int(temporal["buffer_capacity_per_modality"]),
|
||||||
retention_seconds=float(temporal["retention_seconds"]),
|
retention_seconds=float(temporal["retention_seconds"]),
|
||||||
)
|
)
|
||||||
|
lidar_quality = LidarQualityMonitor(K1_LIVE_LIDAR_PROFILE)
|
||||||
first_camera_epoch_ns: list[int] = []
|
first_camera_epoch_ns: list[int] = []
|
||||||
last_camera_epoch_ns: list[int] = []
|
last_camera_epoch_ns: list[int] = []
|
||||||
decoded_frame_count = 0
|
decoded_frame_count = 0
|
||||||
|
|
@ -1034,6 +1042,7 @@ def run(args: argparse.Namespace, loaded: _LoadedModels | None = None) -> int:
|
||||||
"semantic": semantic_queue.snapshot,
|
"semantic": semantic_queue.snapshot,
|
||||||
"decoder": decoder.snapshot,
|
"decoder": decoder.snapshot,
|
||||||
"synchronizer": synchronizer.snapshot,
|
"synchronizer": synchronizer.snapshot,
|
||||||
|
"lidar_quality": lidar_quality.snapshot,
|
||||||
"result": lambda: {
|
"result": lambda: {
|
||||||
"capacity": result_queue.maxsize,
|
"capacity": result_queue.maxsize,
|
||||||
"depth": result_queue.qsize(),
|
"depth": result_queue.qsize(),
|
||||||
|
|
@ -1102,6 +1111,7 @@ def run(args: argparse.Namespace, loaded: _LoadedModels | None = None) -> int:
|
||||||
"max_duration_seconds": args.max_duration_seconds,
|
"max_duration_seconds": args.max_duration_seconds,
|
||||||
"decoder": decoder,
|
"decoder": decoder,
|
||||||
"synchronizer": synchronizer,
|
"synchronizer": synchronizer,
|
||||||
|
"lidar_quality": lidar_quality,
|
||||||
"state": transport,
|
"state": transport,
|
||||||
"sensor_decode_ms": sensor_decode_ms,
|
"sensor_decode_ms": sensor_decode_ms,
|
||||||
"result_queue": result_queue,
|
"result_queue": result_queue,
|
||||||
|
|
@ -1491,6 +1501,7 @@ def run(args: argparse.Namespace, loaded: _LoadedModels | None = None) -> int:
|
||||||
"id": common["projection_manifest"]["pack_id"],
|
"id": common["projection_manifest"]["pack_id"],
|
||||||
"identity_sha256": common["projection_manifest"]["identity_sha256"],
|
"identity_sha256": common["projection_manifest"]["identity_sha256"],
|
||||||
},
|
},
|
||||||
|
"lidar_evidence": lidar_readiness_document(K1_LIVE_LIDAR_PROFILE),
|
||||||
"worker_package": {
|
"worker_package": {
|
||||||
"id": common["worker_package"]["package_id"],
|
"id": common["worker_package"]["package_id"],
|
||||||
"identity_sha256": common["worker_package"]["identity_sha256"],
|
"identity_sha256": common["worker_package"]["identity_sha256"],
|
||||||
|
|
@ -1554,6 +1565,7 @@ def run(args: argparse.Namespace, loaded: _LoadedModels | None = None) -> int:
|
||||||
"synchronizer": synchronizer.snapshot(),
|
"synchronizer": synchronizer.snapshot(),
|
||||||
"sensor_decode_ms": sensor_decode_summary,
|
"sensor_decode_ms": sensor_decode_summary,
|
||||||
},
|
},
|
||||||
|
"lidar_quality": lidar_quality.snapshot(),
|
||||||
"latency_ms": latency_summary,
|
"latency_ms": latency_summary,
|
||||||
"temporal_stability": {
|
"temporal_stability": {
|
||||||
"enabled": stability is not None,
|
"enabled": stability is not None,
|
||||||
|
|
@ -1602,6 +1614,8 @@ def run(args: argparse.Namespace, loaded: _LoadedModels | None = None) -> int:
|
||||||
"limitations": [
|
"limitations": [
|
||||||
"Shadow diagnostic authority only; no commands or navigation output.",
|
"Shadow diagnostic authority only; no commands or navigation output.",
|
||||||
"Camera/LiDAR matching uses recorded host arrival time, not a hardware clock.",
|
"Camera/LiDAR matching uses recorded host arrival time, not a hardware clock.",
|
||||||
|
"K1 LiDAR is a vendor map increment, not an admitted raw sensor sweep.",
|
||||||
|
"K1 LiDAR has no admitted per-point time, ring, scan geometry or IMU stream.",
|
||||||
"Cross-host source epoch age is diagnostic and excluded from acceptance.",
|
"Cross-host source epoch age is diagnostic and excluded from acceptance.",
|
||||||
"COCO and Cityscapes models are not forest-domain or safety validated.",
|
"COCO and Cityscapes models are not forest-domain or safety validated.",
|
||||||
"Amodal cuboids infer unobserved volume from class priors.",
|
"Amodal cuboids infer unobserved volume from class priors.",
|
||||||
|
|
|
||||||
|
|
@ -48,6 +48,26 @@ from .lab_instances import (
|
||||||
publish_e26_lab_instance,
|
publish_e26_lab_instance,
|
||||||
publish_integrated_lab_instance,
|
publish_integrated_lab_instance,
|
||||||
)
|
)
|
||||||
|
from .lidar_contract import (
|
||||||
|
K1_LAB_LIDAR_PACK_V1_PROFILE,
|
||||||
|
K1_LIVE_LIDAR_PROFILE,
|
||||||
|
LIDAR_EVIDENCE_PROFILE_SCHEMA,
|
||||||
|
LIDAR_READINESS_SCHEMA,
|
||||||
|
LidarContractError,
|
||||||
|
LidarCoordinateSpace,
|
||||||
|
LidarEvidenceProfile,
|
||||||
|
LidarPipelineStage,
|
||||||
|
LidarPointField,
|
||||||
|
LidarPoseStatus,
|
||||||
|
LidarQualityMonitor,
|
||||||
|
LidarReadiness,
|
||||||
|
LidarRepresentation,
|
||||||
|
LidarStageAssessment,
|
||||||
|
LidarTimeBasis,
|
||||||
|
assess_lidar_profile,
|
||||||
|
lidar_readiness_document,
|
||||||
|
sensor_frame_xyzi,
|
||||||
|
)
|
||||||
from .live_perception import (
|
from .live_perception import (
|
||||||
LIVE_INGRESS_SCHEMA,
|
LIVE_INGRESS_SCHEMA,
|
||||||
LIVE_INGRESS_WIRE_SCHEMA,
|
LIVE_INGRESS_WIRE_SCHEMA,
|
||||||
|
|
@ -118,10 +138,23 @@ __all__ = [
|
||||||
"EvaluationFrameRequest",
|
"EvaluationFrameRequest",
|
||||||
"EvaluationPackFrame",
|
"EvaluationPackFrame",
|
||||||
"LatestWinsQueue",
|
"LatestWinsQueue",
|
||||||
|
"LIDAR_EVIDENCE_PROFILE_SCHEMA",
|
||||||
|
"LIDAR_READINESS_SCHEMA",
|
||||||
"LIVE_INGRESS_SCHEMA",
|
"LIVE_INGRESS_SCHEMA",
|
||||||
"LIVE_INGRESS_WIRE_SCHEMA",
|
"LIVE_INGRESS_WIRE_SCHEMA",
|
||||||
"LiveIngressEvent",
|
"LiveIngressEvent",
|
||||||
"LivePerceptionIngress",
|
"LivePerceptionIngress",
|
||||||
|
"LidarContractError",
|
||||||
|
"LidarCoordinateSpace",
|
||||||
|
"LidarEvidenceProfile",
|
||||||
|
"LidarPipelineStage",
|
||||||
|
"LidarPointField",
|
||||||
|
"LidarPoseStatus",
|
||||||
|
"LidarQualityMonitor",
|
||||||
|
"LidarReadiness",
|
||||||
|
"LidarRepresentation",
|
||||||
|
"LidarStageAssessment",
|
||||||
|
"LidarTimeBasis",
|
||||||
"IntegratedPerceptionOverlayStore",
|
"IntegratedPerceptionOverlayStore",
|
||||||
"PublishedIntegratedLabInstance",
|
"PublishedIntegratedLabInstance",
|
||||||
"PublishedCameraEgoMotionLabInstance",
|
"PublishedCameraEgoMotionLabInstance",
|
||||||
|
|
@ -133,6 +166,8 @@ __all__ = [
|
||||||
"MultiratePerceptionArtifact",
|
"MultiratePerceptionArtifact",
|
||||||
"MultiratePerceptionQualificationResult",
|
"MultiratePerceptionQualificationResult",
|
||||||
"QUALIFICATION_POLICY",
|
"QUALIFICATION_POLICY",
|
||||||
|
"K1_LAB_LIDAR_PACK_V1_PROFILE",
|
||||||
|
"K1_LIVE_LIDAR_PROFILE",
|
||||||
"QueueSnapshot",
|
"QueueSnapshot",
|
||||||
"RecordedCalibratedFusion",
|
"RecordedCalibratedFusion",
|
||||||
"RecordedCalibratedFusionStore",
|
"RecordedCalibratedFusionStore",
|
||||||
|
|
@ -165,6 +200,7 @@ __all__ = [
|
||||||
"publish_integrated_lab_instance",
|
"publish_integrated_lab_instance",
|
||||||
"validate_multirate_perception_qualification_result",
|
"validate_multirate_perception_qualification_result",
|
||||||
"prepare_recorded_qualification_slice",
|
"prepare_recorded_qualification_slice",
|
||||||
|
"assess_lidar_profile",
|
||||||
"DetectionFrame",
|
"DetectionFrame",
|
||||||
"ObjectDetection",
|
"ObjectDetection",
|
||||||
"RecordedPerceptionOverlayError",
|
"RecordedPerceptionOverlayError",
|
||||||
|
|
@ -184,4 +220,6 @@ __all__ = [
|
||||||
"validate_tracking_qualification_result",
|
"validate_tracking_qualification_result",
|
||||||
"validate_tracked_fusion_qualification_result",
|
"validate_tracked_fusion_qualification_result",
|
||||||
"validate_annotation_workspace",
|
"validate_annotation_workspace",
|
||||||
|
"lidar_readiness_document",
|
||||||
|
"sensor_frame_xyzi",
|
||||||
]
|
]
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,623 @@
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import re
|
||||||
|
import threading
|
||||||
|
from collections import deque
|
||||||
|
from dataclasses import dataclass
|
||||||
|
from enum import StrEnum
|
||||||
|
from typing import Any, Final
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
import numpy.typing as npt
|
||||||
|
|
||||||
|
from k1link.data_plane import DecodedPointCloudView, DecodedPoseView
|
||||||
|
from k1link.device_plugins.xgrids_k1.analyze.calibrated_projection import (
|
||||||
|
CalibratedProjectionError,
|
||||||
|
map_points_to_lidar,
|
||||||
|
)
|
||||||
|
|
||||||
|
LIDAR_EVIDENCE_PROFILE_SCHEMA: Final = "missioncore.lidar-evidence-profile/v1"
|
||||||
|
LIDAR_READINESS_SCHEMA: Final = "missioncore.lidar-readiness/v1"
|
||||||
|
_IDENTIFIER = re.compile(r"^[A-Za-z0-9][A-Za-z0-9._:/-]{0,159}$")
|
||||||
|
|
||||||
|
Float32Array = npt.NDArray[np.float32]
|
||||||
|
|
||||||
|
|
||||||
|
class LidarContractError(ValueError):
|
||||||
|
"""A LiDAR evidence profile or conversion violates the admitted contract."""
|
||||||
|
|
||||||
|
|
||||||
|
class LidarRepresentation(StrEnum):
|
||||||
|
SENSOR_SCAN = "sensor-scan"
|
||||||
|
VENDOR_MAP_INCREMENT = "vendor-map-increment"
|
||||||
|
ACCUMULATED_MAP = "accumulated-map"
|
||||||
|
|
||||||
|
|
||||||
|
class LidarCoordinateSpace(StrEnum):
|
||||||
|
SENSOR = "sensor"
|
||||||
|
MAP = "map"
|
||||||
|
|
||||||
|
|
||||||
|
class LidarTimeBasis(StrEnum):
|
||||||
|
SENSOR = "sensor"
|
||||||
|
HOST_ARRIVAL = "host-arrival"
|
||||||
|
CAMERA_BOUND_HOST_ARRIVAL = "camera-bound-host-arrival"
|
||||||
|
|
||||||
|
|
||||||
|
class LidarPointField(StrEnum):
|
||||||
|
XYZ = "xyz"
|
||||||
|
INTENSITY = "intensity"
|
||||||
|
RING = "ring"
|
||||||
|
RELATIVE_TIME = "relative-time"
|
||||||
|
|
||||||
|
|
||||||
|
class LidarPoseStatus(StrEnum):
|
||||||
|
NONE = "none"
|
||||||
|
BEST_EFFORT = "best-effort"
|
||||||
|
SENSOR_SYNCHRONIZED = "sensor-synchronized"
|
||||||
|
|
||||||
|
|
||||||
|
class LidarPipelineStage(StrEnum):
|
||||||
|
SCANNER_QUALITY = "scanner-quality"
|
||||||
|
GROUND_SEGMENTATION = "ground-segmentation"
|
||||||
|
LIDAR_3D_DETECTION = "lidar-3d-detection"
|
||||||
|
NVIDIA_NVBLOX = "nvidia-nvblox"
|
||||||
|
LIDAR_ODOMETRY = "lidar-odometry"
|
||||||
|
LIDAR_INERTIAL_SLAM = "lidar-inertial-slam"
|
||||||
|
CAMERA_LIDAR_FUSION = "camera-lidar-fusion"
|
||||||
|
|
||||||
|
|
||||||
|
class LidarReadiness(StrEnum):
|
||||||
|
READY = "ready"
|
||||||
|
DEGRADED = "degraded"
|
||||||
|
BLOCKED = "blocked"
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True, slots=True)
|
||||||
|
class LidarEvidenceProfile:
|
||||||
|
"""Describe what one LiDAR source actually provides before model selection."""
|
||||||
|
|
||||||
|
profile_id: str
|
||||||
|
representation: LidarRepresentation
|
||||||
|
coordinate_space: LidarCoordinateSpace
|
||||||
|
coordinate_frame: str
|
||||||
|
frame_time_basis: LidarTimeBasis
|
||||||
|
point_fields: tuple[LidarPointField, ...]
|
||||||
|
pose_status: LidarPoseStatus
|
||||||
|
scan_geometry_known: bool
|
||||||
|
imu_samples_available: bool
|
||||||
|
lidar_imu_extrinsic_available: bool
|
||||||
|
camera_extrinsic_available: bool
|
||||||
|
commands_enabled: bool = False
|
||||||
|
navigation_or_safety_accepted: bool = False
|
||||||
|
|
||||||
|
def __post_init__(self) -> None:
|
||||||
|
_safe_identifier(self.profile_id, "LiDAR profile id")
|
||||||
|
_safe_identifier(self.coordinate_frame, "LiDAR coordinate frame")
|
||||||
|
if not self.point_fields or LidarPointField.XYZ not in self.point_fields:
|
||||||
|
raise LidarContractError("LiDAR evidence must contain xyz")
|
||||||
|
if len(self.point_fields) != len(set(self.point_fields)):
|
||||||
|
raise LidarContractError("LiDAR point fields must be unique")
|
||||||
|
if (
|
||||||
|
self.coordinate_space is LidarCoordinateSpace.MAP
|
||||||
|
and self.pose_status is LidarPoseStatus.NONE
|
||||||
|
):
|
||||||
|
raise LidarContractError("map-frame LiDAR evidence requires a sensor pose")
|
||||||
|
if self.lidar_imu_extrinsic_available and not self.imu_samples_available:
|
||||||
|
raise LidarContractError("LiDAR/IMU extrinsic has no admitted IMU samples")
|
||||||
|
if self.commands_enabled or self.navigation_or_safety_accepted:
|
||||||
|
raise LidarContractError("v1 LiDAR evidence is diagnostic-only")
|
||||||
|
|
||||||
|
def to_dict(self) -> dict[str, object]:
|
||||||
|
return {
|
||||||
|
"schema_version": LIDAR_EVIDENCE_PROFILE_SCHEMA,
|
||||||
|
"profile_id": self.profile_id,
|
||||||
|
"representation": self.representation.value,
|
||||||
|
"coordinates": {
|
||||||
|
"space": self.coordinate_space.value,
|
||||||
|
"frame": self.coordinate_frame,
|
||||||
|
},
|
||||||
|
"time": {
|
||||||
|
"frame_basis": self.frame_time_basis.value,
|
||||||
|
"point_relative_time": LidarPointField.RELATIVE_TIME in self.point_fields,
|
||||||
|
},
|
||||||
|
"point_fields": [field.value for field in self.point_fields],
|
||||||
|
"pose_status": self.pose_status.value,
|
||||||
|
"scan_geometry_known": self.scan_geometry_known,
|
||||||
|
"imu": {
|
||||||
|
"samples_available": self.imu_samples_available,
|
||||||
|
"lidar_extrinsic_available": self.lidar_imu_extrinsic_available,
|
||||||
|
},
|
||||||
|
"camera_extrinsic_available": self.camera_extrinsic_available,
|
||||||
|
"authority": {
|
||||||
|
"commands_enabled": self.commands_enabled,
|
||||||
|
"navigation_or_safety_accepted": self.navigation_or_safety_accepted,
|
||||||
|
},
|
||||||
|
}
|
||||||
|
|
||||||
|
@classmethod
|
||||||
|
def from_dict(cls, value: object) -> LidarEvidenceProfile:
|
||||||
|
document = _object(value, "LiDAR evidence profile")
|
||||||
|
_exact_keys(
|
||||||
|
document,
|
||||||
|
{
|
||||||
|
"schema_version",
|
||||||
|
"profile_id",
|
||||||
|
"representation",
|
||||||
|
"coordinates",
|
||||||
|
"time",
|
||||||
|
"point_fields",
|
||||||
|
"pose_status",
|
||||||
|
"scan_geometry_known",
|
||||||
|
"imu",
|
||||||
|
"camera_extrinsic_available",
|
||||||
|
"authority",
|
||||||
|
},
|
||||||
|
"LiDAR evidence profile",
|
||||||
|
)
|
||||||
|
if document.get("schema_version") != LIDAR_EVIDENCE_PROFILE_SCHEMA:
|
||||||
|
raise LidarContractError("LiDAR evidence profile schema is incompatible")
|
||||||
|
coordinates = _object(document.get("coordinates"), "LiDAR coordinates")
|
||||||
|
time = _object(document.get("time"), "LiDAR time")
|
||||||
|
imu = _object(document.get("imu"), "LiDAR IMU evidence")
|
||||||
|
authority = _object(document.get("authority"), "LiDAR authority")
|
||||||
|
_exact_keys(coordinates, {"space", "frame"}, "LiDAR coordinates")
|
||||||
|
_exact_keys(time, {"frame_basis", "point_relative_time"}, "LiDAR time")
|
||||||
|
_exact_keys(
|
||||||
|
imu,
|
||||||
|
{"samples_available", "lidar_extrinsic_available"},
|
||||||
|
"LiDAR IMU evidence",
|
||||||
|
)
|
||||||
|
_exact_keys(
|
||||||
|
authority,
|
||||||
|
{"commands_enabled", "navigation_or_safety_accepted"},
|
||||||
|
"LiDAR authority",
|
||||||
|
)
|
||||||
|
point_fields = _array(document, "point_fields")
|
||||||
|
try:
|
||||||
|
parsed_fields = tuple(
|
||||||
|
LidarPointField(_string_value(field, "LiDAR point field"))
|
||||||
|
for field in point_fields
|
||||||
|
)
|
||||||
|
profile = cls(
|
||||||
|
profile_id=_string(document, "profile_id"),
|
||||||
|
representation=LidarRepresentation(
|
||||||
|
_string(document, "representation")
|
||||||
|
),
|
||||||
|
coordinate_space=LidarCoordinateSpace(_string(coordinates, "space")),
|
||||||
|
coordinate_frame=_string(coordinates, "frame"),
|
||||||
|
frame_time_basis=LidarTimeBasis(_string(time, "frame_basis")),
|
||||||
|
point_fields=parsed_fields,
|
||||||
|
pose_status=LidarPoseStatus(_string(document, "pose_status")),
|
||||||
|
scan_geometry_known=_bool(document, "scan_geometry_known"),
|
||||||
|
imu_samples_available=_bool(imu, "samples_available"),
|
||||||
|
lidar_imu_extrinsic_available=_bool(imu, "lidar_extrinsic_available"),
|
||||||
|
camera_extrinsic_available=_bool(
|
||||||
|
document,
|
||||||
|
"camera_extrinsic_available",
|
||||||
|
),
|
||||||
|
commands_enabled=_bool(authority, "commands_enabled"),
|
||||||
|
navigation_or_safety_accepted=_bool(
|
||||||
|
authority,
|
||||||
|
"navigation_or_safety_accepted",
|
||||||
|
),
|
||||||
|
)
|
||||||
|
except ValueError as exc:
|
||||||
|
raise LidarContractError("LiDAR evidence profile enum is unknown") from exc
|
||||||
|
if (
|
||||||
|
time.get("point_relative_time")
|
||||||
|
is not (LidarPointField.RELATIVE_TIME in parsed_fields)
|
||||||
|
):
|
||||||
|
raise LidarContractError("LiDAR point-time declarations disagree")
|
||||||
|
return profile
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True, slots=True)
|
||||||
|
class LidarStageAssessment:
|
||||||
|
stage: LidarPipelineStage
|
||||||
|
readiness: LidarReadiness
|
||||||
|
reasons: tuple[str, ...]
|
||||||
|
|
||||||
|
def to_dict(self) -> dict[str, object]:
|
||||||
|
return {
|
||||||
|
"stage": self.stage.value,
|
||||||
|
"readiness": self.readiness.value,
|
||||||
|
"reasons": list(self.reasons),
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
class LidarQualityMonitor:
|
||||||
|
"""Bounded scanner telemetry over decoded point frames.
|
||||||
|
|
||||||
|
The monitor reports observed distributions and field coverage. It does not
|
||||||
|
turn those measurements into navigation or safety acceptance.
|
||||||
|
"""
|
||||||
|
|
||||||
|
def __init__(
|
||||||
|
self,
|
||||||
|
profile: LidarEvidenceProfile,
|
||||||
|
*,
|
||||||
|
frame_sample_capacity: int = 512,
|
||||||
|
point_sample_capacity: int = 32_768,
|
||||||
|
points_sampled_per_frame: int = 256,
|
||||||
|
) -> None:
|
||||||
|
if (
|
||||||
|
not 2 <= frame_sample_capacity <= 16_384
|
||||||
|
or not 256 <= point_sample_capacity <= 1_048_576
|
||||||
|
or not 1 <= points_sampled_per_frame <= 4096
|
||||||
|
):
|
||||||
|
raise LidarContractError("LiDAR quality monitor bounds are invalid")
|
||||||
|
self.profile = profile
|
||||||
|
self._frame_points: deque[int] = deque(maxlen=frame_sample_capacity)
|
||||||
|
self._frame_intervals_ms: deque[float] = deque(maxlen=frame_sample_capacity)
|
||||||
|
self._range_samples_m: deque[float] = deque(maxlen=point_sample_capacity)
|
||||||
|
self._intensity_samples: deque[float] = deque(maxlen=point_sample_capacity)
|
||||||
|
self._points_sampled_per_frame = points_sampled_per_frame
|
||||||
|
self._frames = 0
|
||||||
|
self._points = 0
|
||||||
|
self._intensity_frames = 0
|
||||||
|
self._range_frames = 0
|
||||||
|
self._range_unavailable_frames = 0
|
||||||
|
self._nonincreasing_frame_times = 0
|
||||||
|
self._last_frame_time_ns: int | None = None
|
||||||
|
self._lock = threading.Lock()
|
||||||
|
|
||||||
|
def observe(
|
||||||
|
self,
|
||||||
|
point_cloud: DecodedPointCloudView,
|
||||||
|
*,
|
||||||
|
pose: DecodedPoseView | None = None,
|
||||||
|
) -> None:
|
||||||
|
if point_cloud.frame_id != self.profile.coordinate_frame:
|
||||||
|
raise LidarContractError("LiDAR quality frame differs from its evidence profile")
|
||||||
|
points = np.asarray(point_cloud.positions_xyz, dtype=np.float64).reshape((-1, 3))
|
||||||
|
sample_indices = _uniform_sample_indices(
|
||||||
|
point_cloud.point_count,
|
||||||
|
self._points_sampled_per_frame,
|
||||||
|
)
|
||||||
|
sampled_ranges: npt.NDArray[np.float64] | None = None
|
||||||
|
if self.profile.coordinate_space is LidarCoordinateSpace.SENSOR:
|
||||||
|
sampled_ranges = np.linalg.norm(points[sample_indices], axis=1)
|
||||||
|
elif pose is not None:
|
||||||
|
if pose.frame_id != point_cloud.frame_id:
|
||||||
|
raise LidarContractError("LiDAR quality pose uses another map frame")
|
||||||
|
try:
|
||||||
|
points_sensor = map_points_to_lidar(
|
||||||
|
points[sample_indices],
|
||||||
|
position_map_xyz=pose.position_xyz,
|
||||||
|
orientation_map_from_lidar_xyzw=pose.orientation_xyzw,
|
||||||
|
)
|
||||||
|
except CalibratedProjectionError as exc:
|
||||||
|
raise LidarContractError("LiDAR quality pose is invalid") from exc
|
||||||
|
sampled_ranges = np.linalg.norm(points_sensor, axis=1)
|
||||||
|
|
||||||
|
intensity_samples: npt.NDArray[np.float64] | None = None
|
||||||
|
if point_cloud.intensities is not None:
|
||||||
|
intensities = np.frombuffer(point_cloud.intensities, dtype=np.uint8)
|
||||||
|
intensity_samples = intensities[sample_indices].astype(np.float64) / 255.0
|
||||||
|
elif LidarPointField.INTENSITY in self.profile.point_fields:
|
||||||
|
raise LidarContractError("LiDAR frame dropped profile-required intensity")
|
||||||
|
|
||||||
|
frame_time_ns = point_cloud.context.captured_at_epoch_ns
|
||||||
|
with self._lock:
|
||||||
|
if self._last_frame_time_ns is not None:
|
||||||
|
delta_ns = frame_time_ns - self._last_frame_time_ns
|
||||||
|
if delta_ns <= 0:
|
||||||
|
self._nonincreasing_frame_times += 1
|
||||||
|
else:
|
||||||
|
self._frame_intervals_ms.append(delta_ns / 1_000_000)
|
||||||
|
self._last_frame_time_ns = frame_time_ns
|
||||||
|
self._frames += 1
|
||||||
|
self._points += point_cloud.point_count
|
||||||
|
self._frame_points.append(point_cloud.point_count)
|
||||||
|
if intensity_samples is not None:
|
||||||
|
self._intensity_frames += 1
|
||||||
|
self._intensity_samples.extend(float(value) for value in intensity_samples)
|
||||||
|
if sampled_ranges is None:
|
||||||
|
self._range_unavailable_frames += 1
|
||||||
|
else:
|
||||||
|
self._range_frames += 1
|
||||||
|
self._range_samples_m.extend(float(value) for value in sampled_ranges)
|
||||||
|
|
||||||
|
def snapshot(self) -> dict[str, object]:
|
||||||
|
with self._lock:
|
||||||
|
return {
|
||||||
|
"schema_version": "missioncore.lidar-quality-report/v1",
|
||||||
|
"profile_id": self.profile.profile_id,
|
||||||
|
"frames_observed": self._frames,
|
||||||
|
"points_observed": self._points,
|
||||||
|
"intensity_frames": self._intensity_frames,
|
||||||
|
"range_frames": self._range_frames,
|
||||||
|
"sensor_range_unavailable_frames": self._range_unavailable_frames,
|
||||||
|
"nonincreasing_frame_times": self._nonincreasing_frame_times,
|
||||||
|
"sample_bounds": {
|
||||||
|
"frame_capacity": self._frame_points.maxlen,
|
||||||
|
"point_capacity": self._range_samples_m.maxlen,
|
||||||
|
"points_sampled_per_frame": self._points_sampled_per_frame,
|
||||||
|
},
|
||||||
|
"point_count_per_frame": _distribution(self._frame_points),
|
||||||
|
"frame_interval_ms": _distribution(self._frame_intervals_ms),
|
||||||
|
"sensor_range_m": _distribution(self._range_samples_m),
|
||||||
|
"intensity_0_1": _distribution(self._intensity_samples),
|
||||||
|
"authority": {
|
||||||
|
"commands_enabled": False,
|
||||||
|
"navigation_or_safety_accepted": False,
|
||||||
|
},
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
def assess_lidar_profile(
|
||||||
|
profile: LidarEvidenceProfile,
|
||||||
|
) -> tuple[LidarStageAssessment, ...]:
|
||||||
|
"""Return deterministic readiness without inferring missing sensor evidence."""
|
||||||
|
|
||||||
|
assessments = [
|
||||||
|
_quality_assessment(profile),
|
||||||
|
_ground_assessment(profile),
|
||||||
|
_detector_assessment(profile),
|
||||||
|
_nvblox_assessment(profile),
|
||||||
|
_odometry_assessment(profile),
|
||||||
|
_lio_assessment(profile),
|
||||||
|
_fusion_assessment(profile),
|
||||||
|
]
|
||||||
|
return tuple(assessments)
|
||||||
|
|
||||||
|
|
||||||
|
def lidar_readiness_document(profile: LidarEvidenceProfile) -> dict[str, object]:
|
||||||
|
return {
|
||||||
|
"schema_version": LIDAR_READINESS_SCHEMA,
|
||||||
|
"profile": profile.to_dict(),
|
||||||
|
"stages": [assessment.to_dict() for assessment in assess_lidar_profile(profile)],
|
||||||
|
"authority": {
|
||||||
|
"commands_enabled": False,
|
||||||
|
"navigation_or_safety_accepted": False,
|
||||||
|
},
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
def sensor_frame_xyzi(
|
||||||
|
point_cloud: DecodedPointCloudView,
|
||||||
|
pose: DecodedPoseView | None = None,
|
||||||
|
) -> Float32Array:
|
||||||
|
"""Build the finite sensor-frame XYZI tensor expected by LiDAR detectors.
|
||||||
|
|
||||||
|
K1 `lio_pcl` positions are already in the canonical `map` frame; they are not raw
|
||||||
|
sensor-frame sweeps. The matching `T_map_from_lidar` pose is therefore
|
||||||
|
required to invert them. Intensity is normalized from the verified uint8
|
||||||
|
low byte to the [0, 1] reflectance interval used by the admitted
|
||||||
|
PointPillars baseline.
|
||||||
|
"""
|
||||||
|
|
||||||
|
if point_cloud.intensities is None:
|
||||||
|
raise LidarContractError("sensor-frame XYZI requires intensity")
|
||||||
|
points = np.asarray(point_cloud.positions_xyz, dtype=np.float64).reshape((-1, 3))
|
||||||
|
if point_cloud.frame_id == (pose.child_frame_id if pose is not None else None):
|
||||||
|
points_sensor = points
|
||||||
|
elif pose is not None and point_cloud.frame_id == pose.frame_id:
|
||||||
|
try:
|
||||||
|
points_sensor = map_points_to_lidar(
|
||||||
|
points,
|
||||||
|
position_map_xyz=pose.position_xyz,
|
||||||
|
orientation_map_from_lidar_xyzw=pose.orientation_xyzw,
|
||||||
|
)
|
||||||
|
except CalibratedProjectionError as exc:
|
||||||
|
raise LidarContractError("map-frame LiDAR pose is invalid") from exc
|
||||||
|
else:
|
||||||
|
raise LidarContractError(
|
||||||
|
"LiDAR coordinates cannot be bound to the supplied sensor pose"
|
||||||
|
)
|
||||||
|
intensity = np.frombuffer(point_cloud.intensities, dtype=np.uint8).astype(np.float32)
|
||||||
|
xyzi = np.empty((point_cloud.point_count, 4), dtype=np.float32)
|
||||||
|
xyzi[:, :3] = points_sensor.astype(np.float32)
|
||||||
|
xyzi[:, 3] = intensity / 255.0
|
||||||
|
if not np.isfinite(xyzi).all():
|
||||||
|
raise LidarContractError("sensor-frame XYZI contains non-finite values")
|
||||||
|
return xyzi
|
||||||
|
|
||||||
|
|
||||||
|
def _quality_assessment(profile: LidarEvidenceProfile) -> LidarStageAssessment:
|
||||||
|
reasons: list[str] = []
|
||||||
|
if LidarPointField.INTENSITY not in profile.point_fields:
|
||||||
|
reasons.append("intensity-unavailable")
|
||||||
|
if profile.frame_time_basis is not LidarTimeBasis.SENSOR:
|
||||||
|
reasons.append("sensor-clock-unproven")
|
||||||
|
return _assessment(LidarPipelineStage.SCANNER_QUALITY, reasons, blocked=False)
|
||||||
|
|
||||||
|
|
||||||
|
def _ground_assessment(profile: LidarEvidenceProfile) -> LidarStageAssessment:
|
||||||
|
reasons: list[str] = []
|
||||||
|
if profile.representation is not LidarRepresentation.SENSOR_SCAN:
|
||||||
|
reasons.append("vendor-mapped-points-are-not-raw-returns")
|
||||||
|
if profile.coordinate_space is LidarCoordinateSpace.MAP:
|
||||||
|
reasons.append("sensor-frame-conversion-required")
|
||||||
|
return _assessment(LidarPipelineStage.GROUND_SEGMENTATION, reasons, blocked=False)
|
||||||
|
|
||||||
|
|
||||||
|
def _detector_assessment(profile: LidarEvidenceProfile) -> LidarStageAssessment:
|
||||||
|
reasons: list[str] = []
|
||||||
|
blocked = False
|
||||||
|
if LidarPointField.INTENSITY not in profile.point_fields:
|
||||||
|
reasons.append("admitted-pointpillars-baseline-requires-intensity")
|
||||||
|
blocked = True
|
||||||
|
if (
|
||||||
|
profile.coordinate_space is LidarCoordinateSpace.MAP
|
||||||
|
and profile.pose_status is LidarPoseStatus.NONE
|
||||||
|
):
|
||||||
|
reasons.append("sensor-frame-conversion-has-no-pose")
|
||||||
|
blocked = True
|
||||||
|
elif profile.coordinate_space is LidarCoordinateSpace.MAP:
|
||||||
|
reasons.append("sensor-frame-conversion-required")
|
||||||
|
if profile.representation is not LidarRepresentation.SENSOR_SCAN:
|
||||||
|
reasons.append("pretrained-domain-expects-sensor-scan")
|
||||||
|
return _assessment(LidarPipelineStage.LIDAR_3D_DETECTION, reasons, blocked)
|
||||||
|
|
||||||
|
|
||||||
|
def _nvblox_assessment(profile: LidarEvidenceProfile) -> LidarStageAssessment:
|
||||||
|
reasons: list[str] = []
|
||||||
|
blocked = False
|
||||||
|
if not profile.scan_geometry_known:
|
||||||
|
reasons.append("lidar-intrinsics-or-scan-geometry-unknown")
|
||||||
|
blocked = True
|
||||||
|
if profile.pose_status is LidarPoseStatus.NONE:
|
||||||
|
reasons.append("pose-unavailable")
|
||||||
|
blocked = True
|
||||||
|
elif profile.pose_status is not LidarPoseStatus.SENSOR_SYNCHRONIZED:
|
||||||
|
reasons.append("pose-is-best-effort")
|
||||||
|
if profile.frame_time_basis is not LidarTimeBasis.SENSOR:
|
||||||
|
reasons.append("sensor-clock-unproven")
|
||||||
|
return _assessment(LidarPipelineStage.NVIDIA_NVBLOX, reasons, blocked)
|
||||||
|
|
||||||
|
|
||||||
|
def _odometry_assessment(profile: LidarEvidenceProfile) -> LidarStageAssessment:
|
||||||
|
reasons: list[str] = []
|
||||||
|
blocked = False
|
||||||
|
if profile.representation is not LidarRepresentation.SENSOR_SCAN:
|
||||||
|
reasons.append("odometry-requires-unregistered-sensor-scans")
|
||||||
|
blocked = True
|
||||||
|
if profile.frame_time_basis is not LidarTimeBasis.SENSOR:
|
||||||
|
reasons.append("sensor-clock-unproven")
|
||||||
|
return _assessment(LidarPipelineStage.LIDAR_ODOMETRY, reasons, blocked)
|
||||||
|
|
||||||
|
|
||||||
|
def _lio_assessment(profile: LidarEvidenceProfile) -> LidarStageAssessment:
|
||||||
|
reasons: list[str] = []
|
||||||
|
if profile.representation is not LidarRepresentation.SENSOR_SCAN:
|
||||||
|
reasons.append("lio-requires-unregistered-sensor-scans")
|
||||||
|
if LidarPointField.RELATIVE_TIME not in profile.point_fields:
|
||||||
|
reasons.append("per-point-time-unavailable")
|
||||||
|
if not profile.imu_samples_available:
|
||||||
|
reasons.append("imu-samples-unavailable")
|
||||||
|
if not profile.lidar_imu_extrinsic_available:
|
||||||
|
reasons.append("lidar-imu-extrinsic-unavailable")
|
||||||
|
if profile.frame_time_basis is not LidarTimeBasis.SENSOR:
|
||||||
|
reasons.append("sensor-clock-unproven")
|
||||||
|
return _assessment(
|
||||||
|
LidarPipelineStage.LIDAR_INERTIAL_SLAM,
|
||||||
|
reasons,
|
||||||
|
blocked=bool(reasons),
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def _fusion_assessment(profile: LidarEvidenceProfile) -> LidarStageAssessment:
|
||||||
|
reasons: list[str] = []
|
||||||
|
blocked = False
|
||||||
|
if not profile.camera_extrinsic_available:
|
||||||
|
reasons.append("camera-extrinsic-unavailable")
|
||||||
|
blocked = True
|
||||||
|
if profile.pose_status is LidarPoseStatus.NONE:
|
||||||
|
reasons.append("pose-unavailable")
|
||||||
|
blocked = True
|
||||||
|
if profile.frame_time_basis is not LidarTimeBasis.SENSOR:
|
||||||
|
reasons.append("camera-lidar-synchronization-is-best-effort")
|
||||||
|
return _assessment(LidarPipelineStage.CAMERA_LIDAR_FUSION, reasons, blocked)
|
||||||
|
|
||||||
|
|
||||||
|
def _assessment(
|
||||||
|
stage: LidarPipelineStage,
|
||||||
|
reasons: list[str],
|
||||||
|
blocked: bool,
|
||||||
|
) -> LidarStageAssessment:
|
||||||
|
if blocked:
|
||||||
|
readiness = LidarReadiness.BLOCKED
|
||||||
|
elif reasons:
|
||||||
|
readiness = LidarReadiness.DEGRADED
|
||||||
|
else:
|
||||||
|
readiness = LidarReadiness.READY
|
||||||
|
return LidarStageAssessment(stage, readiness, tuple(reasons))
|
||||||
|
|
||||||
|
|
||||||
|
def _safe_identifier(value: str, label: str) -> str:
|
||||||
|
if not isinstance(value, str) or _IDENTIFIER.fullmatch(value) is None:
|
||||||
|
raise LidarContractError(f"{label} is not a safe identifier")
|
||||||
|
return value
|
||||||
|
|
||||||
|
|
||||||
|
def _uniform_sample_indices(point_count: int, maximum: int) -> npt.NDArray[np.int64]:
|
||||||
|
if point_count <= maximum:
|
||||||
|
return np.arange(point_count, dtype=np.int64)
|
||||||
|
return np.linspace(0, point_count - 1, maximum, dtype=np.int64)
|
||||||
|
|
||||||
|
|
||||||
|
def _distribution(values: deque[int] | deque[float]) -> dict[str, float | int | None]:
|
||||||
|
if not values:
|
||||||
|
return {
|
||||||
|
"sample_count": 0,
|
||||||
|
"minimum": None,
|
||||||
|
"mean": None,
|
||||||
|
"p50": None,
|
||||||
|
"p95": None,
|
||||||
|
"maximum": None,
|
||||||
|
}
|
||||||
|
array = np.asarray(values, dtype=np.float64)
|
||||||
|
return {
|
||||||
|
"sample_count": int(array.size),
|
||||||
|
"minimum": float(np.min(array)),
|
||||||
|
"mean": float(np.mean(array)),
|
||||||
|
"p50": float(np.percentile(array, 50)),
|
||||||
|
"p95": float(np.percentile(array, 95)),
|
||||||
|
"maximum": float(np.max(array)),
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
def _object(value: object, label: str) -> dict[str, Any]:
|
||||||
|
if not isinstance(value, dict) or any(not isinstance(key, str) for key in value):
|
||||||
|
raise LidarContractError(f"{label} must be an object")
|
||||||
|
return value
|
||||||
|
|
||||||
|
|
||||||
|
def _exact_keys(document: dict[str, Any], expected: set[str], label: str) -> None:
|
||||||
|
if set(document) != expected:
|
||||||
|
raise LidarContractError(f"{label} fields are incompatible")
|
||||||
|
|
||||||
|
|
||||||
|
def _array(document: dict[str, Any], key: str) -> list[object]:
|
||||||
|
value = document.get(key)
|
||||||
|
if not isinstance(value, list):
|
||||||
|
raise LidarContractError(f"{key} must be an array")
|
||||||
|
return value
|
||||||
|
|
||||||
|
|
||||||
|
def _string(document: dict[str, Any], key: str) -> str:
|
||||||
|
return _string_value(document.get(key), key)
|
||||||
|
|
||||||
|
|
||||||
|
def _string_value(value: object, label: str) -> str:
|
||||||
|
if not isinstance(value, str) or not value:
|
||||||
|
raise LidarContractError(f"{label} must be a nonempty string")
|
||||||
|
return value
|
||||||
|
|
||||||
|
|
||||||
|
def _bool(document: dict[str, Any], key: str) -> bool:
|
||||||
|
value = document.get(key)
|
||||||
|
if not isinstance(value, bool):
|
||||||
|
raise LidarContractError(f"{key} must be a boolean")
|
||||||
|
return value
|
||||||
|
|
||||||
|
|
||||||
|
K1_LIVE_LIDAR_PROFILE: Final = LidarEvidenceProfile(
|
||||||
|
profile_id="xgrids-k1-live-lio-pcl/v1",
|
||||||
|
representation=LidarRepresentation.VENDOR_MAP_INCREMENT,
|
||||||
|
coordinate_space=LidarCoordinateSpace.MAP,
|
||||||
|
coordinate_frame="map",
|
||||||
|
frame_time_basis=LidarTimeBasis.HOST_ARRIVAL,
|
||||||
|
point_fields=(LidarPointField.XYZ, LidarPointField.INTENSITY),
|
||||||
|
pose_status=LidarPoseStatus.BEST_EFFORT,
|
||||||
|
scan_geometry_known=False,
|
||||||
|
imu_samples_available=False,
|
||||||
|
lidar_imu_extrinsic_available=False,
|
||||||
|
camera_extrinsic_available=True,
|
||||||
|
)
|
||||||
|
|
||||||
|
K1_LAB_LIDAR_PACK_V1_PROFILE: Final = LidarEvidenceProfile(
|
||||||
|
profile_id="xgrids-k1-e10-lidar-replay-pack/v1",
|
||||||
|
representation=LidarRepresentation.VENDOR_MAP_INCREMENT,
|
||||||
|
coordinate_space=LidarCoordinateSpace.MAP,
|
||||||
|
coordinate_frame="map",
|
||||||
|
frame_time_basis=LidarTimeBasis.CAMERA_BOUND_HOST_ARRIVAL,
|
||||||
|
point_fields=(LidarPointField.XYZ,),
|
||||||
|
pose_status=LidarPoseStatus.BEST_EFFORT,
|
||||||
|
scan_geometry_known=False,
|
||||||
|
imu_samples_available=False,
|
||||||
|
lidar_imu_extrinsic_available=False,
|
||||||
|
camera_extrinsic_available=True,
|
||||||
|
)
|
||||||
|
|
@ -0,0 +1,188 @@
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from dataclasses import replace
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
import pytest
|
||||||
|
|
||||||
|
from k1link.compute.lidar_contract import (
|
||||||
|
K1_LAB_LIDAR_PACK_V1_PROFILE,
|
||||||
|
K1_LIVE_LIDAR_PROFILE,
|
||||||
|
LidarContractError,
|
||||||
|
LidarCoordinateSpace,
|
||||||
|
LidarEvidenceProfile,
|
||||||
|
LidarPipelineStage,
|
||||||
|
LidarPointField,
|
||||||
|
LidarPoseStatus,
|
||||||
|
LidarQualityMonitor,
|
||||||
|
LidarReadiness,
|
||||||
|
LidarRepresentation,
|
||||||
|
LidarTimeBasis,
|
||||||
|
assess_lidar_profile,
|
||||||
|
lidar_readiness_document,
|
||||||
|
sensor_frame_xyzi,
|
||||||
|
)
|
||||||
|
from k1link.data_plane import (
|
||||||
|
ConsumerFrameContext,
|
||||||
|
DecodedPointCloudView,
|
||||||
|
DecodedPoseView,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def _context(sequence: int = 1) -> ConsumerFrameContext:
|
||||||
|
return ConsumerFrameContext(
|
||||||
|
sequence=sequence,
|
||||||
|
captured_at_epoch_ns=10,
|
||||||
|
received_monotonic_ns=20,
|
||||||
|
processing_started_monotonic_ns=21,
|
||||||
|
encoded_size_bytes=100,
|
||||||
|
live=True,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def _assessment(stage: LidarPipelineStage, *, lab_pack: bool = False) -> object:
|
||||||
|
profile = K1_LAB_LIDAR_PACK_V1_PROFILE if lab_pack else K1_LIVE_LIDAR_PROFILE
|
||||||
|
return next(item for item in assess_lidar_profile(profile) if item.stage is stage)
|
||||||
|
|
||||||
|
|
||||||
|
def test_k1_profiles_round_trip_and_do_not_claim_navigation_authority() -> None:
|
||||||
|
restored = type(K1_LIVE_LIDAR_PROFILE).from_dict(K1_LIVE_LIDAR_PROFILE.to_dict())
|
||||||
|
|
||||||
|
assert restored == K1_LIVE_LIDAR_PROFILE
|
||||||
|
document = lidar_readiness_document(restored)
|
||||||
|
assert document["authority"] == {
|
||||||
|
"commands_enabled": False,
|
||||||
|
"navigation_or_safety_accepted": False,
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
def test_current_k1_live_source_is_detector_degraded_but_odometry_blocked() -> None:
|
||||||
|
detector = _assessment(LidarPipelineStage.LIDAR_3D_DETECTION)
|
||||||
|
odometry = _assessment(LidarPipelineStage.LIDAR_ODOMETRY)
|
||||||
|
lio = _assessment(LidarPipelineStage.LIDAR_INERTIAL_SLAM)
|
||||||
|
|
||||||
|
assert detector.readiness is LidarReadiness.DEGRADED
|
||||||
|
assert "pretrained-domain-expects-sensor-scan" in detector.reasons
|
||||||
|
assert odometry.readiness is LidarReadiness.BLOCKED
|
||||||
|
assert lio.readiness is LidarReadiness.BLOCKED
|
||||||
|
assert "per-point-time-unavailable" in lio.reasons
|
||||||
|
|
||||||
|
|
||||||
|
def test_lidar_pack_v1_is_blocked_for_pointpillars_because_it_dropped_intensity() -> None:
|
||||||
|
detector = _assessment(LidarPipelineStage.LIDAR_3D_DETECTION, lab_pack=True)
|
||||||
|
|
||||||
|
assert detector.readiness is LidarReadiness.BLOCKED
|
||||||
|
assert "admitted-pointpillars-baseline-requires-intensity" in detector.reasons
|
||||||
|
|
||||||
|
|
||||||
|
def test_nvblox_is_blocked_until_k1_scan_geometry_is_known() -> None:
|
||||||
|
assessment = _assessment(LidarPipelineStage.NVIDIA_NVBLOX)
|
||||||
|
|
||||||
|
assert assessment.readiness is LidarReadiness.BLOCKED
|
||||||
|
assert "lidar-intrinsics-or-scan-geometry-unknown" in assessment.reasons
|
||||||
|
|
||||||
|
|
||||||
|
def test_complete_sensor_profile_is_ready_for_all_admitted_stages() -> None:
|
||||||
|
profile = LidarEvidenceProfile(
|
||||||
|
profile_id="reference-raw-lidar-imu/v1",
|
||||||
|
representation=LidarRepresentation.SENSOR_SCAN,
|
||||||
|
coordinate_space=LidarCoordinateSpace.SENSOR,
|
||||||
|
coordinate_frame="lidar",
|
||||||
|
frame_time_basis=LidarTimeBasis.SENSOR,
|
||||||
|
point_fields=(
|
||||||
|
LidarPointField.XYZ,
|
||||||
|
LidarPointField.INTENSITY,
|
||||||
|
LidarPointField.RING,
|
||||||
|
LidarPointField.RELATIVE_TIME,
|
||||||
|
),
|
||||||
|
pose_status=LidarPoseStatus.SENSOR_SYNCHRONIZED,
|
||||||
|
scan_geometry_known=True,
|
||||||
|
imu_samples_available=True,
|
||||||
|
lidar_imu_extrinsic_available=True,
|
||||||
|
camera_extrinsic_available=True,
|
||||||
|
)
|
||||||
|
|
||||||
|
assert {
|
||||||
|
assessment.readiness for assessment in assess_lidar_profile(profile)
|
||||||
|
} == {LidarReadiness.READY}
|
||||||
|
assert len(assess_lidar_profile(profile)) == len(LidarPipelineStage)
|
||||||
|
|
||||||
|
|
||||||
|
def test_sensor_frame_xyzi_inverts_map_pose_and_normalizes_intensity() -> None:
|
||||||
|
cloud = DecodedPointCloudView(
|
||||||
|
context=_context(),
|
||||||
|
frame_id="map",
|
||||||
|
positions_xyz=((11.0, 2.0, 3.0), (10.0, 3.0, 3.0)),
|
||||||
|
intensities=bytes((0, 255)),
|
||||||
|
)
|
||||||
|
pose = DecodedPoseView(
|
||||||
|
context=_context(2),
|
||||||
|
frame_id="map",
|
||||||
|
child_frame_id="k1-lidar",
|
||||||
|
position_xyz=(10.0, 2.0, 3.0),
|
||||||
|
orientation_xyzw=(0.0, 0.0, 0.0, 1.0),
|
||||||
|
)
|
||||||
|
|
||||||
|
xyzi = sensor_frame_xyzi(cloud, pose)
|
||||||
|
|
||||||
|
np.testing.assert_allclose(
|
||||||
|
xyzi,
|
||||||
|
np.asarray(
|
||||||
|
[
|
||||||
|
[1.0, 0.0, 0.0, 0.0],
|
||||||
|
[0.0, 1.0, 0.0, 1.0],
|
||||||
|
],
|
||||||
|
dtype=np.float32,
|
||||||
|
),
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def test_sensor_frame_xyzi_rejects_missing_intensity_and_unbound_frames() -> None:
|
||||||
|
cloud = DecodedPointCloudView(
|
||||||
|
context=_context(),
|
||||||
|
frame_id="map",
|
||||||
|
positions_xyz=((1.0, 2.0, 3.0),),
|
||||||
|
)
|
||||||
|
with pytest.raises(LidarContractError, match="requires intensity"):
|
||||||
|
sensor_frame_xyzi(cloud)
|
||||||
|
|
||||||
|
with pytest.raises(LidarContractError, match="diagnostic-only"):
|
||||||
|
replace(K1_LIVE_LIDAR_PROFILE, navigation_or_safety_accepted=True)
|
||||||
|
|
||||||
|
|
||||||
|
def test_quality_monitor_is_bounded_and_reports_sensor_relative_range() -> None:
|
||||||
|
monitor = LidarQualityMonitor(
|
||||||
|
K1_LIVE_LIDAR_PROFILE,
|
||||||
|
frame_sample_capacity=2,
|
||||||
|
point_sample_capacity=256,
|
||||||
|
points_sampled_per_frame=2,
|
||||||
|
)
|
||||||
|
pose = DecodedPoseView(
|
||||||
|
context=_context(20),
|
||||||
|
frame_id="map",
|
||||||
|
child_frame_id="k1-lidar",
|
||||||
|
position_xyz=(10.0, 2.0, 3.0),
|
||||||
|
orientation_xyzw=(0.0, 0.0, 0.0, 1.0),
|
||||||
|
)
|
||||||
|
for sequence, capture_ns in enumerate((10, 110_000_010, 210_000_010), start=1):
|
||||||
|
context = replace(_context(sequence), captured_at_epoch_ns=capture_ns)
|
||||||
|
monitor.observe(
|
||||||
|
DecodedPointCloudView(
|
||||||
|
context=context,
|
||||||
|
frame_id="map",
|
||||||
|
positions_xyz=((11.0, 2.0, 3.0), (10.0, 4.0, 3.0)),
|
||||||
|
intensities=bytes((0, 255)),
|
||||||
|
),
|
||||||
|
pose=pose,
|
||||||
|
)
|
||||||
|
|
||||||
|
report = monitor.snapshot()
|
||||||
|
|
||||||
|
assert report["frames_observed"] == 3
|
||||||
|
assert report["points_observed"] == 6
|
||||||
|
assert report["point_count_per_frame"]["sample_count"] == 2
|
||||||
|
assert report["sensor_range_m"]["minimum"] == pytest.approx(1.0)
|
||||||
|
assert report["sensor_range_m"]["maximum"] == pytest.approx(2.0)
|
||||||
|
assert report["intensity_0_1"]["minimum"] == pytest.approx(0.0)
|
||||||
|
assert report["intensity_0_1"]["maximum"] == pytest.approx(1.0)
|
||||||
|
assert report["authority"]["navigation_or_safety_accepted"] is False
|
||||||
Loading…
Reference in New Issue