diff --git a/docs/10_EXTERNAL_PERCEPTION_WORKER.md b/docs/10_EXTERNAL_PERCEPTION_WORKER.md index ca433a0..22e5d08 100644 --- a/docs/10_EXTERNAL_PERCEPTION_WORKER.md +++ b/docs/10_EXTERNAL_PERCEPTION_WORKER.md @@ -1,5 +1,12 @@ # 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 ```text diff --git a/docs/11_K1_CALIBRATED_PERCEPTION_ROADMAP.md b/docs/11_K1_CALIBRATED_PERCEPTION_ROADMAP.md index e0cef76..06b89a5 100644 --- a/docs/11_K1_CALIBRATED_PERCEPTION_ROADMAP.md +++ b/docs/11_K1_CALIBRATED_PERCEPTION_ROADMAP.md @@ -2,6 +2,11 @@ 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 Mission Core will use the K1 factory camera/LiDAR calibration instead of diff --git a/docs/13_LIDAR_WORKER_PRODUCT_AND_ROADMAP.md b/docs/13_LIDAR_WORKER_PRODUCT_AND_ROADMAP.md new file mode 100644 index 0000000..972dc0e --- /dev/null +++ b/docs/13_LIDAR_WORKER_PRODUCT_AND_ROADMAP.md @@ -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. diff --git a/docs/adr/0018-lidar-evidence-before-models.md b/docs/adr/0018-lidar-evidence-before-models.md new file mode 100644 index 0000000..472556c --- /dev/null +++ b/docs/adr/0018-lidar-evidence-before-models.md @@ -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`. diff --git a/experiments/perception/worker/run_e15_shadow_inference.py b/experiments/perception/worker/run_e15_shadow_inference.py index cf91be8..f47703e 100644 --- a/experiments/perception/worker/run_e15_shadow_inference.py +++ b/experiments/perception/worker/run_e15_shadow_inference.py @@ -92,6 +92,11 @@ from k1link.compute.inline_temporal import ( read_inline_profile, stabilize_world_state, ) +from k1link.compute.lidar_contract import ( + K1_LIVE_LIDAR_PROFILE, + LidarQualityMonitor, + lidar_readiness_document, +) from k1link.compute.live_perception import ( LIVE_RESULT_MAX_PAYLOAD_BYTES, LiveSensorSynchronizer, @@ -563,6 +568,7 @@ def _receiver( max_duration_seconds: float, decoder: PersistentFmp4Decoder, synchronizer: LiveSensorSynchronizer, + lidar_quality: LidarQualityMonitor, state: _TransportState, sensor_decode_ms: dict[str, list[float]], result_queue: queue.Queue[bytes], @@ -690,6 +696,7 @@ def _receiver( ) sensor_decode_ms[modality].append((time.perf_counter() - decode_started) * 1000) if modality == "lidar" and isinstance(normalized, DecodedPointCloudView): + lidar_quality.observe(normalized) synchronizer.publish_point_cloud(normalized) elif modality == "pose" and isinstance(normalized, DecodedPoseView): 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"]), retention_seconds=float(temporal["retention_seconds"]), ) + lidar_quality = LidarQualityMonitor(K1_LIVE_LIDAR_PROFILE) first_camera_epoch_ns: list[int] = [] last_camera_epoch_ns: list[int] = [] decoded_frame_count = 0 @@ -1034,6 +1042,7 @@ def run(args: argparse.Namespace, loaded: _LoadedModels | None = None) -> int: "semantic": semantic_queue.snapshot, "decoder": decoder.snapshot, "synchronizer": synchronizer.snapshot, + "lidar_quality": lidar_quality.snapshot, "result": lambda: { "capacity": result_queue.maxsize, "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, "decoder": decoder, "synchronizer": synchronizer, + "lidar_quality": lidar_quality, "state": transport, "sensor_decode_ms": sensor_decode_ms, "result_queue": result_queue, @@ -1491,6 +1501,7 @@ def run(args: argparse.Namespace, loaded: _LoadedModels | None = None) -> int: "id": common["projection_manifest"]["pack_id"], "identity_sha256": common["projection_manifest"]["identity_sha256"], }, + "lidar_evidence": lidar_readiness_document(K1_LIVE_LIDAR_PROFILE), "worker_package": { "id": common["worker_package"]["package_id"], "identity_sha256": common["worker_package"]["identity_sha256"], @@ -1554,6 +1565,7 @@ def run(args: argparse.Namespace, loaded: _LoadedModels | None = None) -> int: "synchronizer": synchronizer.snapshot(), "sensor_decode_ms": sensor_decode_summary, }, + "lidar_quality": lidar_quality.snapshot(), "latency_ms": latency_summary, "temporal_stability": { "enabled": stability is not None, @@ -1602,6 +1614,8 @@ def run(args: argparse.Namespace, loaded: _LoadedModels | None = None) -> int: "limitations": [ "Shadow diagnostic authority only; no commands or navigation output.", "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.", "COCO and Cityscapes models are not forest-domain or safety validated.", "Amodal cuboids infer unobserved volume from class priors.", diff --git a/src/k1link/compute/__init__.py b/src/k1link/compute/__init__.py index 28f393f..98ce7ad 100644 --- a/src/k1link/compute/__init__.py +++ b/src/k1link/compute/__init__.py @@ -48,6 +48,26 @@ from .lab_instances import ( publish_e26_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 ( LIVE_INGRESS_SCHEMA, LIVE_INGRESS_WIRE_SCHEMA, @@ -118,10 +138,23 @@ __all__ = [ "EvaluationFrameRequest", "EvaluationPackFrame", "LatestWinsQueue", + "LIDAR_EVIDENCE_PROFILE_SCHEMA", + "LIDAR_READINESS_SCHEMA", "LIVE_INGRESS_SCHEMA", "LIVE_INGRESS_WIRE_SCHEMA", "LiveIngressEvent", "LivePerceptionIngress", + "LidarContractError", + "LidarCoordinateSpace", + "LidarEvidenceProfile", + "LidarPipelineStage", + "LidarPointField", + "LidarPoseStatus", + "LidarQualityMonitor", + "LidarReadiness", + "LidarRepresentation", + "LidarStageAssessment", + "LidarTimeBasis", "IntegratedPerceptionOverlayStore", "PublishedIntegratedLabInstance", "PublishedCameraEgoMotionLabInstance", @@ -133,6 +166,8 @@ __all__ = [ "MultiratePerceptionArtifact", "MultiratePerceptionQualificationResult", "QUALIFICATION_POLICY", + "K1_LAB_LIDAR_PACK_V1_PROFILE", + "K1_LIVE_LIDAR_PROFILE", "QueueSnapshot", "RecordedCalibratedFusion", "RecordedCalibratedFusionStore", @@ -165,6 +200,7 @@ __all__ = [ "publish_integrated_lab_instance", "validate_multirate_perception_qualification_result", "prepare_recorded_qualification_slice", + "assess_lidar_profile", "DetectionFrame", "ObjectDetection", "RecordedPerceptionOverlayError", @@ -184,4 +220,6 @@ __all__ = [ "validate_tracking_qualification_result", "validate_tracked_fusion_qualification_result", "validate_annotation_workspace", + "lidar_readiness_document", + "sensor_frame_xyzi", ] diff --git a/src/k1link/compute/lidar_contract.py b/src/k1link/compute/lidar_contract.py new file mode 100644 index 0000000..ed142c4 --- /dev/null +++ b/src/k1link/compute/lidar_contract.py @@ -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, +) diff --git a/tests/test_lidar_contract.py b/tests/test_lidar_contract.py new file mode 100644 index 0000000..588a444 --- /dev/null +++ b/tests/test_lidar_contract.py @@ -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