SILVER
/
NODEDC_MISSION_CORE
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

feat(perception): qualify lidar evidence before models

This commit is contained in:
DCCONSTRUCTIONS 2026-07-25 01:00:26 +03:00
parent 37c24b5fa8
commit 3f549f91f1
8 changed files with 1185 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
docs/10_EXTERNAL_PERCEPTION_WORKER.md
View File

@ -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

5
docs/11_K1_CALIBRATED_PERCEPTION_ROADMAP.md
View File

@ -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

255
docs/13_LIDAR_WORKER_PRODUCT_AND_ROADMAP.md Normal file
View File

@ -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.

55
docs/adr/0018-lidar-evidence-before-models.md Normal file
View File

@ -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`.

14
experiments/perception/worker/run_e15_shadow_inference.py
View File

@ -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.",

38
src/k1link/compute/__init__.py
View File

@ -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",
]

623
src/k1link/compute/lidar_contract.py Normal file
View File

@ -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,
)

188
tests/test_lidar_contract.py Normal file
View File

@ -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