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, ) K1_LIDAR_PACK_V2_PROFILE: Final = LidarEvidenceProfile( profile_id="xgrids-k1-lidar-replay-pack/v2", 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, )