diff --git a/README.md b/README.md index 070a847..53974f5 100644 --- a/README.md +++ b/README.md @@ -139,6 +139,14 @@ private replay dataset for this work. No K1 firmware change, onboard exporter or new device command is part of the LiDAR roadmap, and the historical `1.27 m` handheld-height experiment is not a runtime constant. +The first passive replay derivative is now implemented as +`missioncore.k1-local-surface/v1`. It binds the immutable `RAVNOVES00` pack, +estimates a rolling local surface from map points plus compatible pose, and +publishes height, slope, roughness, confidence and conservative observed +surface/occupied/unknown evidence. All `526/526` available samples produced a +diagnostic result; no free-space, command, navigation or safety authority is +inferred. The selected scene is visible in **Парк → Диагностика LiDAR**. + The complete RELLIS-3D v1.1 release is now admitted there and its full `2,413`-frame validation split is available in **Полигон → Датасеты**. The sealed Current/Patchwork++ comparison rejected Patchwork++ for navigation: diff --git a/apps/control-station/src/core/lidar/localSurface.ts b/apps/control-station/src/core/lidar/localSurface.ts new file mode 100644 index 0000000..2d9bb76 --- /dev/null +++ b/apps/control-station/src/core/lidar/localSurface.ts @@ -0,0 +1,602 @@ +export interface LidarLocalSurfaceDistribution { + sampleCount: number; + minimum: number | null; + mean: number | null; + p50: number | null; + p95: number | null; + maximum: number | null; +} + +export interface LidarLocalSurfaceAnchor { + key: string; + label: string; + frameIndex: number; + sourceFrameIndex: number; + sessionSeconds: number; + valid: boolean; +} + +export interface LidarLocalSurfaceModel { + modelId: string; + displayName: string; + sessionId: string; + sourcePackId: string; + status: "diagnostic-only"; + source: { + frameCount: number; + availableLidarFrames: number; + pointCount: number; + timelineStartSeconds: number; + timelineEndSeconds: number; + immutable: true; + passiveProcessingOnly: true; + firmwareOrDeviceCommandsUsed: false; + }; + metrics: { + frames: { + total: number; + sourceAvailable: number; + valid: number; + sourceUnavailable: number; + poseStale: number; + insufficientSurface: number; + fitFailed: number; + }; + sensorHeightM: LidarLocalSurfaceDistribution; + slopeDeg: LidarLocalSurfaceDistribution; + roughnessM: LidarLocalSurfaceDistribution; + confidence: LidarLocalSurfaceDistribution; + poseBindingAgeMs: LidarLocalSurfaceDistribution; + surfaceMaxAgeMs: LidarLocalSurfaceDistribution; + }; + anchors: LidarLocalSurfaceAnchor[]; + occupancyPolicy: { + absenceOfPointsMeansFree: false; + unknownIsTraversable: false; + persistentReconstructionMutated: false; + dynamicObjectLayerAvailable: false; + }; + createdAtUtc: string | null; + groundTruth: false; + authority: { + commandsEnabled: false; + navigationOrSafetyAccepted: false; + }; +} + +export interface LidarLocalSurfaceCatalog { + configured: boolean; + validTotal: number; + invalidTotal: number; + items: LidarLocalSurfaceModel[]; +} + +export interface LidarLocalSurfaceFrame { + modelId: string; + sourcePackId: string; + sessionId: string; + frameIndex: number; + frameCount: number; + sourceFrameIndex: number; + sessionSeconds: number; + sourceAvailable: boolean; + valid: boolean; + failureCode: number; + pointCount: number; + coordinateFrame: "map"; + distanceUnit: "m"; + pointsXyzM: Array<[number, number, number]>; + pointClass: number[]; + pointHeightM: number[]; + pose: { + positionXyzM: [number, number, number]; + orientationXyzw: [number, number, number, number]; + bindingAgeMs: number; + }; + surface: { + planeCoefficientsMap: [number, number, number, number]; + sensorHeightM: number; + slopeDeg: number; + roughnessM: number; + confidence: number; + surfaceMaxAgeMs: number; + cellCount: number; + inlierCellCount: number; + }; + counts: { + classified: number; + surface: number; + occupied: number; + belowSurface: number; + }; + occupancyPolicy: LidarLocalSurfaceModel["occupancyPolicy"]; + groundTruth: false; + authority: LidarLocalSurfaceModel["authority"]; +} + +export class LidarLocalSurfaceContractError extends Error {} + +export class LidarLocalSurfaceApiError extends Error { + constructor(message: string, readonly status: number | null = null) { + super(message); + } +} + +type LidarFetch = ( + input: RequestInfo | URL, + init?: RequestInit, +) => Promise; + +const LOCAL_SURFACE_SCHEMA_PREFIX = ["missioncore.", "k", "1", "-local-surface"].join(""); +const LOCAL_SURFACE_MODEL_PREFIX = ["k", "1", "-local-surface-"].join(""); +const SAFE_MODEL_ID = new RegExp(`^${LOCAL_SURFACE_MODEL_PREFIX}[a-f0-9]{64}$`); +const SAFE_PACK_ID = /^e10-lidar-pack-[a-f0-9]{64}$/; +const SAFE_ID = /^[A-Za-z0-9][A-Za-z0-9._:/-]{0,159}$/; +const SAFE_KEY = /^[a-z0-9][a-z0-9-]{0,63}$/; + +function record(value: unknown, label: string): Record { + if (!value || typeof value !== "object" || Array.isArray(value)) { + throw new LidarLocalSurfaceContractError(`${label}: ожидался объект`); + } + return value as Record; +} + +function array(value: unknown, label: string): unknown[] { + if (!Array.isArray(value)) { + throw new LidarLocalSurfaceContractError(`${label}: ожидался массив`); + } + return value; +} + +function text( + value: unknown, + label: string, + pattern?: RegExp, +): string { + if ( + typeof value !== "string" + || !value.trim() + || value.length > 240 + || (pattern && !pattern.test(value)) + ) { + throw new LidarLocalSurfaceContractError(`${label}: некорректная строка`); + } + return value; +} + +function integer(value: unknown, label: string): number { + if (typeof value !== "number" || !Number.isSafeInteger(value) || value < 0) { + throw new LidarLocalSurfaceContractError(`${label}: ожидалось целое число`); + } + return value; +} + +function finite(value: unknown, label: string): number { + if (typeof value !== "number" || !Number.isFinite(value)) { + throw new LidarLocalSurfaceContractError(`${label}: ожидалось конечное число`); + } + return value; +} + +function boolean(value: unknown, label: string): boolean { + if (typeof value !== "boolean") { + throw new LidarLocalSurfaceContractError(`${label}: ожидался boolean`); + } + return value; +} + +function tuple( + value: unknown, + length: number, + label: string, +): number[] { + const values = array(value, label).map((item, index) => + finite(item, `${label}[${index}]`) + ); + if (values.length !== length) { + throw new LidarLocalSurfaceContractError(`${label}: неверная длина`); + } + return values; +} + +function distribution( + value: unknown, + label: string, +): LidarLocalSurfaceDistribution { + const source = record(value, label); + const sampleCount = integer(source.sample_count, `${label}.sample_count`); + const metric = (key: string): number | null => { + const item = source[key]; + if (item === null) return null; + return finite(item, `${label}.${key}`); + }; + const result = { + sampleCount, + minimum: metric("minimum"), + mean: metric("mean"), + p50: metric("p50"), + p95: metric("p95"), + maximum: metric("maximum"), + }; + if ( + (sampleCount === 0 + && Object.entries(result).some( + ([key, item]) => key !== "sampleCount" && item !== null, + )) + || (sampleCount > 0 + && Object.entries(result).some( + ([key, item]) => key !== "sampleCount" && item === null, + )) + ) { + throw new LidarLocalSurfaceContractError(`${label}: несовместимая выборка`); + } + return result; +} + +function occupancyPolicy( + value: unknown, +): LidarLocalSurfaceModel["occupancyPolicy"] { + const source = record(value, "occupancy_policy"); + if ( + source.absence_of_points_means_free !== false + || source.unknown_is_traversable !== false + || source.persistent_reconstruction_mutated !== false + || source.dynamic_object_layer_available !== false + ) { + throw new LidarLocalSurfaceContractError( + "Local-surface policy завышает доступное знание", + ); + } + return { + absenceOfPointsMeansFree: false, + unknownIsTraversable: false, + persistentReconstructionMutated: false, + dynamicObjectLayerAvailable: false, + }; +} + +function authority( + value: unknown, +): LidarLocalSurfaceModel["authority"] { + const source = record(value, "authority"); + if ( + source.commands_enabled !== false + || source.navigation_or_safety_accepted !== false + ) { + throw new LidarLocalSurfaceContractError( + "Local-surface authority несовместим", + ); + } + return { + commandsEnabled: false, + navigationOrSafetyAccepted: false, + }; +} + +function anchor(value: unknown): LidarLocalSurfaceAnchor { + const source = record(value, "anchor"); + return { + key: text(source.key, "anchor.key", SAFE_KEY), + label: text(source.label, "anchor.label"), + frameIndex: integer(source.frame_index, "anchor.frame_index"), + sourceFrameIndex: integer( + source.source_frame_index, + "anchor.source_frame_index", + ), + sessionSeconds: finite(source.session_seconds, "anchor.session_seconds"), + valid: boolean(source.valid, "anchor.valid"), + }; +} + +function model(value: unknown): LidarLocalSurfaceModel { + const source = record(value, "LiDAR local-surface model"); + const sourceEvidence = record(source.source, "source"); + const metrics = record(source.metrics, "metrics"); + const frames = record(metrics.frames, "metrics.frames"); + if ( + source.status !== "diagnostic-only" + || source.ground_truth !== false + || sourceEvidence.immutable !== true + || sourceEvidence.passive_processing_only !== true + || sourceEvidence.firmware_or_device_commands_used !== false + ) { + throw new LidarLocalSurfaceContractError( + "LiDAR local-surface меняет источник или завышает статус", + ); + } + const frameMetrics = { + total: integer(frames.total, "frames.total"), + sourceAvailable: integer(frames.source_available, "frames.source_available"), + valid: integer(frames.valid, "frames.valid"), + sourceUnavailable: integer( + frames.source_unavailable, + "frames.source_unavailable", + ), + poseStale: integer(frames.pose_stale, "frames.pose_stale"), + insufficientSurface: integer( + frames.insufficient_surface, + "frames.insufficient_surface", + ), + fitFailed: integer(frames.fit_failed, "frames.fit_failed"), + }; + if ( + frameMetrics.sourceAvailable + frameMetrics.sourceUnavailable + !== frameMetrics.total + || frameMetrics.valid > frameMetrics.sourceAvailable + ) { + throw new LidarLocalSurfaceContractError( + "LiDAR local-surface frame totals расходятся", + ); + } + const anchors = array(source.anchors, "anchors").map(anchor); + if (!anchors.length || anchors.some((item) => item.frameIndex >= frameMetrics.total)) { + throw new LidarLocalSurfaceContractError( + "LiDAR local-surface anchors несовместимы", + ); + } + return { + modelId: text(source.model_id, "model_id", SAFE_MODEL_ID), + displayName: text(source.display_name, "display_name"), + sessionId: text(source.session_id, "session_id", SAFE_ID), + sourcePackId: text(source.source_pack_id, "source_pack_id", SAFE_PACK_ID), + status: "diagnostic-only", + source: { + frameCount: integer(sourceEvidence.frame_count, "source.frame_count"), + availableLidarFrames: integer( + sourceEvidence.available_lidar_frames, + "source.available_lidar_frames", + ), + pointCount: integer(sourceEvidence.point_count, "source.point_count"), + timelineStartSeconds: finite( + sourceEvidence.timeline_start_seconds, + "source.timeline_start_seconds", + ), + timelineEndSeconds: finite( + sourceEvidence.timeline_end_seconds, + "source.timeline_end_seconds", + ), + immutable: true, + passiveProcessingOnly: true, + firmwareOrDeviceCommandsUsed: false, + }, + metrics: { + frames: frameMetrics, + sensorHeightM: distribution(metrics.sensor_height_m, "sensor_height_m"), + slopeDeg: distribution(metrics.slope_deg, "slope_deg"), + roughnessM: distribution(metrics.roughness_m, "roughness_m"), + confidence: distribution(metrics.confidence, "confidence"), + poseBindingAgeMs: distribution( + metrics.pose_binding_age_ms, + "pose_binding_age_ms", + ), + surfaceMaxAgeMs: distribution( + metrics.surface_max_age_ms, + "surface_max_age_ms", + ), + }, + anchors, + occupancyPolicy: occupancyPolicy(source.occupancy_policy), + createdAtUtc: + source.created_at_utc === null || source.created_at_utc === undefined + ? null + : text(source.created_at_utc, "created_at_utc"), + groundTruth: false, + authority: authority(source.authority), + }; +} + +export function parseLidarLocalSurfaceCatalog( + value: unknown, +): LidarLocalSurfaceCatalog { + const source = record(value, "LiDAR local-surface catalog"); + if ( + source.schema_version !== `${LOCAL_SURFACE_SCHEMA_PREFIX}-catalog/v1` + || source.access !== "read-only" + ) { + throw new LidarLocalSurfaceContractError( + "LiDAR local-surface catalog несовместим", + ); + } + return { + configured: boolean(source.configured, "configured"), + validTotal: integer(source.valid_total, "valid_total"), + invalidTotal: integer(source.invalid_total, "invalid_total"), + items: array(source.items, "items").map(model), + }; +} + +export function parseLidarLocalSurfaceFrame( + value: unknown, +): LidarLocalSurfaceFrame { + const source = record(value, "LiDAR local-surface frame"); + if ( + source.schema_version !== `${LOCAL_SURFACE_SCHEMA_PREFIX}-frame/v1` + || source.access !== "read-only" + || source.ground_truth !== false + || source.coordinate_frame !== "map" + || source.distance_unit !== "m" + ) { + throw new LidarLocalSurfaceContractError( + "LiDAR local-surface frame несовместим", + ); + } + const pointCount = integer(source.point_count, "point_count"); + if (pointCount > 200_000) { + throw new LidarLocalSurfaceContractError( + "LiDAR local-surface frame слишком большой", + ); + } + const pointsXyzM = array(source.points_xyz_m, "points_xyz_m").map( + (item, index): [number, number, number] => { + const values = tuple(item, 3, `points_xyz_m[${index}]`); + return [values[0], values[1], values[2]]; + }, + ); + const pointClass = array(source.point_class, "point_class").map( + (item, index) => { + const value = integer(item, `point_class[${index}]`); + if (value > 3) { + throw new LidarLocalSurfaceContractError( + "Неизвестный local-surface class", + ); + } + return value; + }, + ); + const pointHeightM = array(source.point_height_m, "point_height_m").map( + (item, index) => finite(item, `point_height_m[${index}]`), + ); + if ( + pointsXyzM.length !== pointCount + || pointClass.length !== pointCount + || pointHeightM.length !== pointCount + ) { + throw new LidarLocalSurfaceContractError( + "LiDAR local-surface point arrays расходятся", + ); + } + const pose = record(source.pose, "pose"); + const surface = record(source.surface, "surface"); + const counts = record(source.counts, "counts"); + const parsedCounts = { + classified: integer(counts.classified, "counts.classified"), + surface: integer(counts.surface, "counts.surface"), + occupied: integer(counts.occupied, "counts.occupied"), + belowSurface: integer(counts.below_surface, "counts.below_surface"), + }; + if ( + parsedCounts.classified !== pointClass.filter((item) => item !== 0).length + || parsedCounts.surface !== pointClass.filter((item) => item === 1).length + || parsedCounts.occupied !== pointClass.filter((item) => item === 2).length + || parsedCounts.belowSurface + !== pointClass.filter((item) => item === 3).length + ) { + throw new LidarLocalSurfaceContractError( + "LiDAR local-surface counts расходятся", + ); + } + const position = tuple(pose.position_xyz_m, 3, "pose.position_xyz_m"); + const orientation = tuple( + pose.orientation_xyzw, + 4, + "pose.orientation_xyzw", + ); + const plane = tuple( + surface.plane_coefficients_map, + 4, + "surface.plane_coefficients_map", + ); + return { + modelId: text(source.model_id, "model_id", SAFE_MODEL_ID), + sourcePackId: text(source.source_pack_id, "source_pack_id", SAFE_PACK_ID), + sessionId: text(source.session_id, "session_id", SAFE_ID), + frameIndex: integer(source.frame_index, "frame_index"), + frameCount: integer(source.frame_count, "frame_count"), + sourceFrameIndex: integer(source.source_frame_index, "source_frame_index"), + sessionSeconds: finite(source.session_seconds, "session_seconds"), + sourceAvailable: boolean(source.source_available, "source_available"), + valid: boolean(source.valid, "valid"), + failureCode: integer(source.failure_code, "failure_code"), + pointCount, + coordinateFrame: "map", + distanceUnit: "m", + pointsXyzM, + pointClass, + pointHeightM, + pose: { + positionXyzM: [position[0], position[1], position[2]], + orientationXyzw: [ + orientation[0], + orientation[1], + orientation[2], + orientation[3], + ], + bindingAgeMs: finite(pose.binding_age_ms, "pose.binding_age_ms"), + }, + surface: { + planeCoefficientsMap: [plane[0], plane[1], plane[2], plane[3]], + sensorHeightM: finite(surface.sensor_height_m, "surface.sensor_height_m"), + slopeDeg: finite(surface.slope_deg, "surface.slope_deg"), + roughnessM: finite(surface.roughness_m, "surface.roughness_m"), + confidence: finite(surface.confidence, "surface.confidence"), + surfaceMaxAgeMs: finite( + surface.surface_max_age_ms, + "surface.surface_max_age_ms", + ), + cellCount: integer(surface.cell_count, "surface.cell_count"), + inlierCellCount: integer( + surface.inlier_cell_count, + "surface.inlier_cell_count", + ), + }, + counts: parsedCounts, + occupancyPolicy: occupancyPolicy(source.occupancy_policy), + groundTruth: false, + authority: authority(source.authority), + }; +} + +async function responseJson( + response: Response, + fallback: string, +): Promise { + let payload: unknown = null; + try { + payload = await response.json(); + } catch { + // Preserve the status-aware fallback below. + } + if (!response.ok) { + const detail = + payload && typeof payload === "object" && "detail" in payload + ? String((payload as { detail?: unknown }).detail) + : fallback; + throw new LidarLocalSurfaceApiError(detail, response.status); + } + return payload; +} + +export async function fetchLidarLocalSurfaces( + options: { signal?: AbortSignal; fetcher?: LidarFetch } = {}, +): Promise { + const fetcher = options.fetcher ?? fetch; + const response = await fetcher("/api/v1/lidar/local-surfaces?limit=10", { + method: "GET", + headers: { Accept: "application/json" }, + signal: options.signal, + }); + return parseLidarLocalSurfaceCatalog( + await responseJson(response, "Не удалось получить LiDAR local-surface."), + ); +} + +export async function fetchLidarLocalSurfaceFrame( + modelId: string, + frameIndex: number, + options: { signal?: AbortSignal; fetcher?: LidarFetch } = {}, +): Promise { + if ( + !SAFE_MODEL_ID.test(modelId) + || !Number.isSafeInteger(frameIndex) + || frameIndex < 0 + ) { + throw new LidarLocalSurfaceContractError( + "Некорректный LiDAR local-surface frame", + ); + } + const fetcher = options.fetcher ?? fetch; + const response = await fetcher( + `/api/v1/lidar/local-surfaces/${modelId}/frames/${frameIndex}`, + { + method: "GET", + headers: { Accept: "application/json" }, + signal: options.signal, + }, + ); + return parseLidarLocalSurfaceFrame( + await responseJson( + response, + "Не удалось получить LiDAR local-surface frame.", + ), + ); +} diff --git a/apps/control-station/src/styles/responsive.css b/apps/control-station/src/styles/responsive.css index 04bdc55..febc29b 100644 --- a/apps/control-station/src/styles/responsive.css +++ b/apps/control-station/src/styles/responsive.css @@ -30,6 +30,14 @@ grid-template-columns: 1fr; } + .lidar-local-surface__summary { + grid-template-columns: repeat(2, minmax(0, 1fr)); + } + + .lidar-local-surface__stage { + grid-template-columns: minmax(0, 1fr) minmax(13rem, 0.32fr); + } + .lidar-field-cloud { grid-column: 1; grid-row: 1; @@ -412,6 +420,20 @@ flex-direction: column; } + .lidar-local-surface__heading { + flex-direction: column; + } + + .lidar-local-surface__summary, + .lidar-local-surface__stage { + grid-template-columns: 1fr; + } + + .lidar-local-surface__stage .lidar-ground-scene, + .lidar-local-surface__stage .lidar-ground-scene-placeholder { + min-height: 22rem; + } + .polygon-run-identity dl { grid-template-columns: 1fr; } diff --git a/apps/control-station/src/styles/workspaces.css b/apps/control-station/src/styles/workspaces.css index 69e8e09..218cd8e 100644 --- a/apps/control-station/src/styles/workspaces.css +++ b/apps/control-station/src/styles/workspaces.css @@ -2821,6 +2821,146 @@ margin: 0; } +.lidar-local-surface { + display: grid; + gap: 0.72rem; + margin-top: 1.2rem; + padding-top: 1rem; +} + +.lidar-local-surface__heading { + display: flex; + align-items: flex-start; + justify-content: space-between; + gap: 1rem; +} + +.lidar-local-surface__heading h3, +.lidar-local-surface__heading p { + margin: 0; +} + +.lidar-local-surface__heading h3 { + margin-top: 0.22rem; + color: var(--nodedc-text-primary); + font-size: 0.92rem; +} + +.lidar-local-surface__heading p, +.lidar-local-surface__frame p { + max-width: 48rem; + margin-top: 0.28rem; + color: var(--nodedc-text-muted); + font-size: 0.62rem; + line-height: 1.5; +} + +.lidar-local-surface__summary { + display: grid; + grid-template-columns: repeat(4, minmax(0, 1fr)); + gap: 0.5rem; +} + +.lidar-local-surface__summary > div { + display: grid; + gap: 0.24rem; + background: rgb(255 255 255 / 0.025); + padding: 0.62rem 0.68rem; +} + +.lidar-local-surface__summary span, +.lidar-local-surface__frame span, +.lidar-local-surface__frame small, +.lidar-local-surface__frame dt, +.lidar-local-surface__legend { + color: var(--nodedc-text-muted); + font-size: 0.58rem; +} + +.lidar-local-surface__summary strong, +.lidar-local-surface__frame strong, +.lidar-local-surface__frame dd { + color: var(--nodedc-text-primary); + font-size: 0.68rem; +} + +.lidar-local-surface__stage { + display: grid; + overflow: hidden; + grid-template-columns: minmax(0, 1fr) minmax(14rem, 0.24fr); + min-height: 30rem; + border-radius: 0.9rem; + background: #06070a; +} + +.lidar-local-surface__stage .lidar-ground-scene, +.lidar-local-surface__stage .lidar-ground-scene-placeholder { + min-height: 30rem; + border: 0; + border-radius: 0; +} + +.lidar-local-surface__frame { + display: grid; + align-content: start; + gap: 0.85rem; + background: rgb(255 255 255 / 0.025); + padding: 0.9rem; +} + +.lidar-local-surface__frame > div:first-child { + display: grid; + gap: 0.2rem; +} + +.lidar-local-surface__frame dl { + display: grid; + gap: 0.55rem; + margin: 0; +} + +.lidar-local-surface__frame dl > div { + display: flex; + align-items: baseline; + justify-content: space-between; + gap: 0.6rem; +} + +.lidar-local-surface__frame dt, +.lidar-local-surface__frame dd { + margin: 0; +} + +.lidar-local-surface__legend { + display: grid; + gap: 0.42rem; +} + +.lidar-local-surface__legend span { + display: flex; + align-items: center; + gap: 0.45rem; +} + +.lidar-local-surface__legend i { + width: 0.55rem; + height: 0.55rem; + border-radius: 50%; + background: #454a47; +} + +.lidar-local-surface__legend i[data-class="surface"] { + background: #a1b87d; +} + +.lidar-local-surface__legend i[data-class="occupied"] { + background: #f0783d; +} + +.lidar-local-surface__legend i[data-class="below"] { + background: #a85061; +} + .lidar-fallback-review { display: grid; overflow: hidden; diff --git a/apps/control-station/src/workspaces/LidarGroundPointCloud.tsx b/apps/control-station/src/workspaces/LidarGroundPointCloud.tsx index 458deb3..8a74eb2 100644 --- a/apps/control-station/src/workspaces/LidarGroundPointCloud.tsx +++ b/apps/control-station/src/workspaces/LidarGroundPointCloud.tsx @@ -9,7 +9,8 @@ export type LidarGroundViewMode = | "disagreement" | "candidate-disagreement" | "semantic" - | "ground-truth"; + | "ground-truth" + | "local-surface"; export interface LidarGroundPointCloudFrame { pointCount: number; @@ -25,6 +26,7 @@ export interface LidarGroundPointCloudFrame { candidateDisagreement?: number[]; groundTruthGround?: number[]; evaluationMask?: number[]; + localSurfaceClass?: number[]; }; } @@ -67,7 +69,18 @@ function frameColors( const current = frame.masks.currentGround[index] === 1; const candidate = frame.masks.candidateGround[index] === 1; const candidateAssigned = frame.masks.candidateAssigned[index] === 1; - if (mode === "intensity") { + if (mode === "local-surface") { + const localClass = frame.masks.localSurfaceClass?.[index] ?? 0; + if (localClass === 1) { + setRgb(colors, offset, 0.63, 0.72, 0.49); + } else if (localClass === 2) { + setRgb(colors, offset, 0.94, 0.48, 0.24); + } else if (localClass === 3) { + setRgb(colors, offset, 0.66, 0.32, 0.38); + } else { + setRgb(colors, offset, 0.27, 0.29, 0.28); + } + } else if (mode === "intensity") { const intensity = (frame.intensity0To255?.[index] ?? 96) / 255; const neutral = 0.16 + intensity * 0.8; setRgb( diff --git a/apps/control-station/src/workspaces/LidarLocalSurfacePanel.tsx b/apps/control-station/src/workspaces/LidarLocalSurfacePanel.tsx new file mode 100644 index 0000000..de79255 --- /dev/null +++ b/apps/control-station/src/workspaces/LidarLocalSurfacePanel.tsx @@ -0,0 +1,219 @@ +import { useEffect, useMemo, useState } from "react"; +import { StatusBadge } from "@nodedc/ui-react"; + +import { + fetchLidarLocalSurfaceFrame, + fetchLidarLocalSurfaces, + type LidarLocalSurfaceFrame, + type LidarLocalSurfaceModel, +} from "../core/lidar/localSurface"; +import { LidarGroundPointCloud } from "./LidarGroundPointCloud"; + +function formatNumber(value: number | null, digits = 2): string { + if (value === null) return "—"; + return value.toLocaleString("ru-RU", { maximumFractionDigits: digits }); +} + +function errorMessage(error: unknown): string { + return error instanceof Error && error.message.trim() + ? error.message + : "Не удалось открыть локальную модель LiDAR."; +} + +export function LidarLocalSurfacePanel({ + selectedWindowKey, + reloadGeneration, +}: { + selectedWindowKey: string | null; + reloadGeneration: number; +}) { + const [model, setModel] = useState(null); + const [frame, setFrame] = useState(null); + const [loading, setLoading] = useState(true); + const [error, setError] = useState(null); + + const anchor = useMemo(() => { + if (!model) return null; + return ( + model.anchors.find((item) => item.key === selectedWindowKey) + ?? model.anchors[0] + ?? null + ); + }, [model, selectedWindowKey]); + + useEffect(() => { + const controller = new AbortController(); + setLoading(true); + setError(null); + void fetchLidarLocalSurfaces({ signal: controller.signal }) + .then((catalog) => { + if (controller.signal.aborted) return; + setModel(catalog.items[0] ?? null); + }) + .catch((loadError) => { + if (controller.signal.aborted) return; + setModel(null); + setFrame(null); + setError(errorMessage(loadError)); + }) + .finally(() => { + if (!controller.signal.aborted) setLoading(false); + }); + return () => controller.abort(); + }, [reloadGeneration]); + + useEffect(() => { + if (!model || !anchor) { + setFrame(null); + return; + } + const controller = new AbortController(); + setLoading(true); + setError(null); + void fetchLidarLocalSurfaceFrame(model.modelId, anchor.frameIndex, { + signal: controller.signal, + }) + .then((nextFrame) => { + if (!controller.signal.aborted) setFrame(nextFrame); + }) + .catch((loadError) => { + if (controller.signal.aborted) return; + setFrame(null); + setError(errorMessage(loadError)); + }) + .finally(() => { + if (!controller.signal.aborted) setLoading(false); + }); + return () => controller.abort(); + }, [anchor, model]); + + const cloudFrame = useMemo(() => { + if (!frame) return null; + const emptyMask = new Array(frame.pointCount).fill(0); + return { + pointCount: frame.pointCount, + pointsXyzM: frame.pointsXyzM, + intensity0To255: null, + masks: { + currentGround: emptyMask, + currentAssigned: emptyMask, + candidateGround: emptyMask, + candidateAssigned: emptyMask, + disagreement: emptyMask, + localSurfaceClass: frame.pointClass, + }, + }; + }, [frame]); + + if (!model && !loading && !error) { + return null; + } + + return ( +
+
+
+ ЛОКАЛЬНАЯ МОДЕЛЬ LIDAR · L2.6 +

Поверхность и наблюдаемые препятствия

+

+ Производная от неизменяемого RAVNOVES00: высота и уклон + вычисляются из текущей позы и локальной поверхности, без константы + 1,27 м и без команд в сканер. +

+
+ + {error + ? "Недоступно" + : frame?.valid + ? "Кадр рассчитан" + : "Диагностический режим"} + +
+ + {model ? ( +
+
+ Покрытие записи + + {model.metrics.frames.valid.toLocaleString("ru-RU")} /{" "} + {model.metrics.frames.sourceAvailable.toLocaleString("ru-RU")} + +
+
+ Высота над поверхностью · p50 + {formatNumber(model.metrics.sensorHeightM.p50)} м +
+
+ Шероховатость · p95 + {formatNumber(model.metrics.roughnessM.p95, 3)} м +
+
+ Pose binding · p95 + {formatNumber(model.metrics.poseBindingAgeMs.p95)} мс +
+
+ ) : null} + +
+ {cloudFrame && frame ? ( + + ) : ( +
+ + {error ? "Ошибка" : "Загрузка"} + + {error ?? "Читаем производную выбранной сцены…"} +
+ )} + {frame ? ( + + ) : null} +
+
+ ); +} diff --git a/apps/control-station/src/workspaces/LidarQualityWorkspace.tsx b/apps/control-station/src/workspaces/LidarQualityWorkspace.tsx index ed68405..9567897 100644 --- a/apps/control-station/src/workspaces/LidarQualityWorkspace.tsx +++ b/apps/control-station/src/workspaces/LidarQualityWorkspace.tsx @@ -25,6 +25,7 @@ import { LidarGroundPointCloud, type LidarGroundViewMode, } from "./LidarGroundPointCloud"; +import { LidarLocalSurfacePanel } from "./LidarLocalSurfacePanel"; function formatNumber(value: number | null, digits = 1): string { if (value === null) return "—"; @@ -460,6 +461,11 @@ export function LidarQualityWorkspace({ Оба non-ground + + ) : groundBenchmark ? (
{ + server = await createServer({ + appType: "custom", + logLevel: "silent", + server: { middlewareMode: true }, + }); + ({ + parseLidarLocalSurfaceCatalog, + parseLidarLocalSurfaceFrame, + LidarLocalSurfaceContractError, + } = await server.ssrLoadModule("/src/core/lidar/localSurface.ts")); +}); + +after(async () => { + await server?.close(); +}); + +test("decodes passive local-surface evidence", () => { + const decoded = parseLidarLocalSurfaceCatalog(catalog()); + assert.equal(decoded.items[0].source.passiveProcessingOnly, true); + assert.equal(decoded.items[0].metrics.sensorHeightM.p50, 1.3); + assert.equal(decoded.items[0].anchors[0].sourceFrameIndex, 1350); + + const decodedFrame = parseLidarLocalSurfaceFrame(frame()); + assert.equal(decodedFrame.counts.occupied, 1); + assert.equal(decodedFrame.surface.sensorHeightM, 1.3); +}); + +test("rejects inferred free space", () => { + assert.throws( + () => parseLidarLocalSurfaceCatalog(catalog({ + items: [model({ + occupancy_policy: policy({ absence_of_points_means_free: true }), + })], + })), + LidarLocalSurfaceContractError, + ); +}); + +test("rejects command authority", () => { + assert.throws( + () => parseLidarLocalSurfaceFrame(frame({ + authority: { + commands_enabled: true, + navigation_or_safety_accepted: false, + }, + })), + LidarLocalSurfaceContractError, + ); +}); diff --git a/docs/13_LIDAR_WORKER_PRODUCT_AND_ROADMAP.md b/docs/13_LIDAR_WORKER_PRODUCT_AND_ROADMAP.md index 0dc9e0e..6d69481 100644 --- a/docs/13_LIDAR_WORKER_PRODUCT_AND_ROADMAP.md +++ b/docs/13_LIDAR_WORKER_PRODUCT_AND_ROADMAP.md @@ -2,8 +2,8 @@ Date: 2026-07-25 Status: accepted architecture plan; L0/L1 implemented; L2 diagnostic A/B -complete; full GOOSE and RELLIS qualification complete; K1 local-world-model -gate next +complete; full GOOSE and RELLIS qualification complete; L2.6a K1 replay +local-surface slice implemented; operator review and live shadow next Scope: passively received real-time K1 point/pose evidence, immutable replay and future live shadow processing Explicitly out of scope: K1 firmware modification, a new onboard exporter, new @@ -384,28 +384,51 @@ showed that Patchwork++ slightly improved Ground IoU while reducing obstacle non-ground recall from `80.46%` to `69.70%`; the candidate was rejected. Dataset expansion is no longer the next gate. -### L2.6 — K1 local world model — next +### L2.6 — K1 local world model — in progress -- [ ] Bind the immutable `RAVNOVES00` source and replay-pack-v2 evidence without - rewriting either generation. -- [ ] Transform each admitted map increment through the nearest compatible - `T_map_from_lidar` and publish pose-binding age explicitly. -- [ ] Estimate a time-varying local surface for ground-vehicle profiles using +- [x] Bind the immutable `RAVNOVES00` E10 source by pack identity and artifact + hash without copying or rewriting the source generation. +- [ ] Mirror the same accepted profile over replay-pack-v2 evidence while + preserving its separate identity and field-retention contract. +- [x] Reject stale pose binding and publish pose-binding age explicitly; keep + map-native processing honest instead of claiming that pose inversion + recreates an original sensor sweep. +- [x] Estimate a time-varying local surface for ground-vehicle profiles using robust spatial cells and temporal support; do not use the historical `1.27 m` value. -- [ ] Publish surface height, slope, roughness, step/curb candidates and - confidence separately from semantic classes. -- [ ] Produce short-TTL `occupied` and `unknown` layers from current evidence. +- [x] Publish surface-relative height, slope, roughness and confidence + separately from semantic classes. +- [ ] Add independently reviewable step/curb candidates; do not derive them + from a single global height threshold. +- [x] Produce short-TTL observed-surface, `occupied` and `unknown` evidence. Do not infer `free` merely because a mapped point is absent. -- [ ] Keep persistent reconstruction, recent collision evidence and dynamic - observations as separate derivatives of the same source. +- [x] Leave the immutable persistent reconstruction untouched by the local + derivative. +- [ ] Add recent-collision and dynamic-observation layers as separate + derivatives with independent decay and provenance. - [ ] Reuse the accepted camera-to-LiDAR projection as an optional semantic layer with source, confidence, freshness and conflict fields. -- [ ] Measure latency, point age, pose-binding age, temporal stability, obstacle - preservation and memory growth over the complete recording. +- [ ] Complete the qualification report with per-frame latency, point age, + temporal stability, obstacle preservation and memory growth. - [ ] Replay the same profiles through a bounded latest-wins live-shadow queue; no K1 command, navigation or safety authority is added. +The implemented `missioncore.k1-local-surface/v1` derivative is reproducible +through `experiments/perception/run_k1_local_surface.py` and is exposed +read-only through `GET /api/v1/lidar/local-surfaces` plus the bound frame +endpoint. **Парк → Диагностика LiDAR** reuses the five RAVNOVES00 scene +selectors and shows the selected source frame as observed surface, observed +occupied-above-surface, negative outlier and unclassified evidence. The React +contract is provider-neutral; K1 remains a bound backend source rather than UI +implementation knowledge. + +The complete RAVNOVES00 run covered all `526/526` available LiDAR samples. +There were zero stale-pose, insufficient-surface or failed-fit frames. Observed +diagnostic distributions are: derived sensor-to-surface height `1.295 m` p50, +roughness `0.054 m` p95, slope `3.172°` p95 and pose-binding age `19.113 ms` +p95. These values describe this recording only; they are not calibration, +ground truth or a navigation gate. Free space remains unavailable. + Exit: one immutable K1 session yields both a persistent reconstruction and a bounded local world state without hard-coded terrain height or scanner-side changes. @@ -487,7 +510,10 @@ natural-ground recall from `51.76%` to `76.22%` and stays below `23.41 ms` p95 across 961 frames. Full RELLIS qualification covers `2,413` validation frames and rejects Patchwork++ because the small Ground-IoU gain came with unacceptable obstacle loss. Public-dataset ground qualification is therefore complete -enough for the current decision. The highest-value immediate work is the K1 -local world model over `RAVNOVES00`, followed by bounded live shadow. Nvblox, -raw-scan detectors and alternative SLAM remain optional later gates because the -current report contract does not carry their required ray/timing semantics. +enough for the current decision. The first K1 local-surface replay slice now +covers all available `RAVNOVES00` samples and is visible in the operator +interface. The highest-value immediate work is operator review, explicit +step/curb and temporal-stability qualification, followed by bounded live +shadow. Nvblox, raw-scan detectors and alternative SLAM remain optional later +gates because the current report contract does not carry their required +ray/timing semantics. diff --git a/docs/14_LIDAR_DATASET_GATEWAY.md b/docs/14_LIDAR_DATASET_GATEWAY.md index 2ac8521..0c52994 100644 --- a/docs/14_LIDAR_DATASET_GATEWAY.md +++ b/docs/14_LIDAR_DATASET_GATEWAY.md @@ -46,6 +46,12 @@ Implemented now: - count, finite-value and maximum-point safety gates; - immutable point-aligned arrays; - a fail-closed K1 `lio_pcl` boundary; +- a content-addressed `missioncore.k1-local-surface/v1` replay derivative over + immutable `RAVNOVES00`, with dynamic height/slope/roughness/confidence, + explicit pose-binding age and conservative observed occupied/unknown policy; +- a provider-neutral read-only local-surface view in + **Парк → Диагностика LiDAR**, synchronized to the five existing + RAVNOVES00 scene selectors; - worker storage admission for `D:\NDC_MISSIONCORE\datasets` and `/mnt/d/NDC_MISSIONCORE/datasets`; - a dedicated, honest dataset catalog in **Полигон → Датасеты**; diff --git a/experiments/perception/run_k1_local_surface.py b/experiments/perception/run_k1_local_surface.py new file mode 100644 index 0000000..bb34ba6 --- /dev/null +++ b/experiments/perception/run_k1_local_surface.py @@ -0,0 +1,79 @@ +#!/usr/bin/env python3 +from __future__ import annotations + +import argparse +import json +from pathlib import Path + +from k1link.compute import ( + E10LidarFieldSource, + K1LocalSurfaceProfile, + K1LocalSurfaceV1, + build_k1_local_surface, +) + + +def _arguments() -> argparse.Namespace: + parser = argparse.ArgumentParser( + description=( + "Build a passive, source-bound K1 rolling local-surface derivative " + "without firmware commands or a hardcoded sensor height." + ) + ) + parser.add_argument("source_pack", type=Path) + parser.add_argument("output_root", type=Path) + parser.add_argument("--local-radius-m", type=float, default=10.0) + parser.add_argument("--cell-size-m", type=float, default=0.45) + parser.add_argument("--surface-ttl-s", type=float, default=1.25) + parser.add_argument("--minimum-surface-cells", type=int, default=18) + parser.add_argument("--surface-band-m", type=float, default=0.16) + parser.add_argument("--obstacle-min-height-m", type=float, default=0.20) + parser.add_argument("--obstacle-max-height-m", type=float, default=3.5) + return parser.parse_args() + + +def main() -> int: + arguments = _arguments() + profile = K1LocalSurfaceProfile( + local_radius_m=arguments.local_radius_m, + cell_size_m=arguments.cell_size_m, + surface_ttl_s=arguments.surface_ttl_s, + minimum_surface_cells=arguments.minimum_surface_cells, + surface_band_m=arguments.surface_band_m, + obstacle_min_height_m=arguments.obstacle_min_height_m, + obstacle_max_height_m=arguments.obstacle_max_height_m, + ) + source = E10LidarFieldSource(arguments.source_pack) + try: + output = build_k1_local_surface( + source, + arguments.output_root, + profile=profile, + ) + finally: + source.close() + model = K1LocalSurfaceV1(output) + try: + print( + json.dumps( + { + "model_id": model.model_id, + "display_name": model.report["display_name"], + "source": model.report["source"], + "surface_model": model.report["surface_model"], + "occupancy_policy": model.report["occupancy_policy"], + "metrics": model.report["metrics"], + "anchors": model.report["anchors"], + "decision": model.report["decision"], + }, + ensure_ascii=False, + indent=2, + ) + ) + finally: + model.close() + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/src/k1link/compute/__init__.py b/src/k1link/compute/__init__.py index 4a74317..868a2f4 100644 --- a/src/k1link/compute/__init__.py +++ b/src/k1link/compute/__init__.py @@ -106,6 +106,16 @@ from .lidar_ground import ( lidar_ground_frame_detail, score_ground_labels, ) +from .lidar_local_surface import ( + DEFAULT_K1_LOCAL_SURFACE_PROFILE, + K1_LOCAL_SURFACE_FRAME_SCHEMA, + K1_LOCAL_SURFACE_REPORT_SCHEMA, + K1_LOCAL_SURFACE_SCHEMA, + K1LocalSurfaceProfile, + K1LocalSurfaceV1, + build_k1_local_surface, + k1_local_surface_catalog_item, +) from .lidar_replay import ( LIDAR_EQUIVALENCE_REPORT_SCHEMA, LIDAR_QUALITY_REPORT_SCHEMA, @@ -185,6 +195,7 @@ __all__ = [ "AnnotationWorkspaceError", "COMPUTE_JOB_SCHEMA", "DEFAULT_QUALIFICATION_FRAME_COUNT", + "DEFAULT_K1_LOCAL_SURFACE_PROFILE", "EVALUATION_PACK_SCHEMA", "EvaluationFrameRequest", "EvaluationPackFrame", @@ -195,6 +206,9 @@ __all__ = [ "LIDAR_GROUND_BENCHMARK_REPORT_SCHEMA", "LIDAR_GROUND_BENCHMARK_SCHEMA", "LIDAR_GROUND_FRAME_SCHEMA", + "K1_LOCAL_SURFACE_FRAME_SCHEMA", + "K1_LOCAL_SURFACE_REPORT_SCHEMA", + "K1_LOCAL_SURFACE_SCHEMA", "LIDAR_FIELD_REVIEW_REPORT_SCHEMA", "LIDAR_FIELD_REVIEW_SCHEMA", "LIDAR_FIELD_REVIEW_WINDOW_SCHEMA", @@ -217,6 +231,8 @@ __all__ = [ "LidarReadiness", "LidarGroundBenchmarkV1", "LidarGroundError", + "K1LocalSurfaceProfile", + "K1LocalSurfaceV1", "LidarReplayError", "LidarReplayPackV2", "LidarReplayPointFrame", @@ -280,7 +296,9 @@ __all__ = [ "build_lidar_replay_pack_v2", "build_lidar_ground_annotation_template", "build_lidar_ground_benchmark", + "build_k1_local_surface", "lidar_ground_frame_detail", + "k1_local_surface_catalog_item", "E10_LIDAR_PACK_SCHEMA", "FIELD_REVIEW_ARRAYS_NAME", "FIELD_REVIEW_MANIFEST_NAME", diff --git a/src/k1link/compute/lidar_local_surface.py b/src/k1link/compute/lidar_local_surface.py new file mode 100644 index 0000000..d4609bf --- /dev/null +++ b/src/k1link/compute/lidar_local_surface.py @@ -0,0 +1,1002 @@ +from __future__ import annotations + +import hashlib +import json +import math +import os +import re +import shutil +import time +from collections.abc import Mapping +from dataclasses import dataclass +from datetime import UTC, datetime +from pathlib import Path +from typing import Any, Final + +import numpy as np +import numpy.typing as npt + +from k1link.ground_segmentation import ( + GroundSegmentationError as LidarGroundError, +) + +from .lidar_field_review import ( + RAVNOVES00_CENTRAL_WINDOWS, + E10LidarFieldSource, +) + +K1_LOCAL_SURFACE_SCHEMA: Final = "missioncore.k1-local-surface/v1" +K1_LOCAL_SURFACE_REPORT_SCHEMA: Final = "missioncore.k1-local-surface-report/v1" +K1_LOCAL_SURFACE_FRAME_SCHEMA: Final = "missioncore.k1-local-surface-frame/v1" +K1_LOCAL_SURFACE_ARRAYS_NAME: Final = "local-surface.npz" +K1_LOCAL_SURFACE_REPORT_NAME: Final = "local-surface.json" +K1_LOCAL_SURFACE_MANIFEST_NAME: Final = "manifest.json" + +POINT_UNCLASSIFIED: Final = 0 +POINT_SURFACE: Final = 1 +POINT_OCCUPIED: Final = 2 +POINT_BELOW_SURFACE: Final = 3 + +FRAME_VALID: Final = 0 +FRAME_SOURCE_UNAVAILABLE: Final = 1 +FRAME_POSE_STALE: Final = 2 +FRAME_INSUFFICIENT_SURFACE: Final = 3 +FRAME_FIT_FAILED: Final = 4 + +_LOCAL_SURFACE_ID = re.compile(r"^k1-local-surface-[a-f0-9]{64}$") +_E10_PACK_ID = re.compile(r"^e10-lidar-pack-[a-f0-9]{64}$") +_SHA256 = re.compile(r"^[a-f0-9]{64}$") + + +@dataclass(frozen=True, slots=True) +class K1LocalSurfaceProfile: + """Content-bound parameters for the passive K1 local-surface experiment.""" + + profile_id: str = "k1-vendor-map-dynamic-local-surface/v1" + local_radius_m: float = 10.0 + cell_size_m: float = 0.45 + surface_ttl_s: float = 1.25 + cell_lower_percentile: float = 20.0 + initial_lower_fraction: float = 0.55 + minimum_surface_cells: int = 18 + robust_iterations: int = 5 + robust_mad_scale: float = 2.8 + minimum_inlier_band_m: float = 0.10 + surface_band_m: float = 0.16 + obstacle_min_height_m: float = 0.20 + obstacle_max_height_m: float = 3.5 + maximum_pose_binding_ms: float = 100.0 + maximum_slope_deg: float = 40.0 + + def __post_init__(self) -> None: + numeric = ( + self.local_radius_m, + self.cell_size_m, + self.surface_ttl_s, + self.cell_lower_percentile, + self.initial_lower_fraction, + self.robust_mad_scale, + self.minimum_inlier_band_m, + self.surface_band_m, + self.obstacle_min_height_m, + self.obstacle_max_height_m, + self.maximum_pose_binding_ms, + self.maximum_slope_deg, + ) + if ( + not self.profile_id.strip() + or len(self.profile_id) > 160 + or not np.isfinite(numeric).all() + or not 1.0 <= self.local_radius_m <= 100.0 + or not 0.05 <= self.cell_size_m <= 5.0 + or not 0.05 <= self.surface_ttl_s <= 30.0 + or not 0.0 <= self.cell_lower_percentile <= 50.0 + or not 0.05 <= self.initial_lower_fraction <= 0.95 + or not 3 <= self.minimum_surface_cells <= 100_000 + or not 1 <= self.robust_iterations <= 20 + or not 1.0 <= self.robust_mad_scale <= 10.0 + or not 0.01 <= self.minimum_inlier_band_m <= 1.0 + or not 0.01 <= self.surface_band_m <= 1.0 + or not self.surface_band_m <= self.obstacle_min_height_m + or not self.obstacle_min_height_m < self.obstacle_max_height_m <= 20.0 + or not 1.0 <= self.maximum_pose_binding_ms <= 10_000.0 + or not 1.0 <= self.maximum_slope_deg < 90.0 + ): + raise LidarGroundError("K1 local-surface profile is invalid") + + def to_dict(self) -> dict[str, object]: + return { + "schema_version": "missioncore.k1-local-surface-profile/v1", + "profile_id": self.profile_id, + "input": { + "representation": "legacy-e10-vendor-map-with-pose", + "gravity_alignment": "vendor-map-z-assumed-diagnostic", + "physical_sensor_height_required": False, + "hardcoded_height_m": None, + }, + "rolling_surface": { + "local_radius_m": self.local_radius_m, + "cell_size_m": self.cell_size_m, + "surface_ttl_s": self.surface_ttl_s, + "cell_lower_percentile": self.cell_lower_percentile, + "initial_lower_fraction": self.initial_lower_fraction, + "minimum_surface_cells": self.minimum_surface_cells, + "robust_iterations": self.robust_iterations, + "robust_mad_scale": self.robust_mad_scale, + "minimum_inlier_band_m": self.minimum_inlier_band_m, + "maximum_slope_deg": self.maximum_slope_deg, + }, + "classification": { + "surface_band_m": self.surface_band_m, + "obstacle_min_height_m": self.obstacle_min_height_m, + "obstacle_max_height_m": self.obstacle_max_height_m, + "absence_of_points_means_free": False, + "unknown_is_traversable": False, + }, + "pose_binding": { + "basis": "recorded-nearest-host-monotonic-arrival", + "maximum_age_ms": self.maximum_pose_binding_ms, + }, + "authority": { + "commands_enabled": False, + "navigation_or_safety_accepted": False, + }, + } + + +DEFAULT_K1_LOCAL_SURFACE_PROFILE: Final = K1LocalSurfaceProfile() + + +class K1LocalSurfaceV1: + """Strict reader for source-aligned, passive K1 local-surface evidence.""" + + def __init__(self, root: Path) -> None: + candidate = root.expanduser().absolute() + if candidate.is_symlink(): + raise LidarGroundError("K1 local-surface artifact cannot be a symlink") + self.root = candidate.resolve(strict=True) + if not self.root.is_dir() or _LOCAL_SURFACE_ID.fullmatch(self.root.name) is None: + raise LidarGroundError("K1 local-surface id is invalid") + self.manifest = _read_json(self.root / K1_LOCAL_SURFACE_MANIFEST_NAME) + self.identity = _object(self.manifest.get("identity"), "K1 local-surface identity") + identity_sha256 = self.manifest.get("identity_sha256") + if ( + self.manifest.get("schema_version") != K1_LOCAL_SURFACE_SCHEMA + or self.identity.get("schema_version") != K1_LOCAL_SURFACE_SCHEMA + or not isinstance(identity_sha256, str) + or _SHA256.fullmatch(identity_sha256) is None + or hashlib.sha256(_canonical_json(self.identity)).hexdigest() != identity_sha256 + or self.root.name != f"k1-local-surface-{identity_sha256}" + or self.manifest.get("model_id") != self.root.name + ): + raise LidarGroundError("K1 local-surface identity is invalid") + artifacts = _validate_artifacts(self.root, self.manifest.get("artifacts")) + self.arrays = np.load(artifacts["local-surface"], allow_pickle=False) + self.report = _read_json(artifacts["local-surface-report"]) + try: + self._validate() + except BaseException: + self.close() + raise + self.model_id = self.root.name + + def close(self) -> None: + self.arrays.close() + + def _validate(self) -> None: + frame_count = _nonnegative_int(self.identity.get("frame_count"), "frame count") + point_count = _nonnegative_int(self.identity.get("point_count"), "point count") + required = { + "frame_valid", + "frame_failure_code", + "plane_coefficients_map", + "sensor_height_m", + "slope_deg", + "roughness_m", + "confidence", + "pose_binding_age_ms", + "surface_max_age_ms", + "surface_cell_count", + "surface_inlier_cell_count", + "classified_point_count", + "surface_point_count", + "occupied_point_count", + "below_surface_point_count", + "point_class", + "point_height_m", + } + if set(self.arrays.files) != required: + raise LidarGroundError("K1 local-surface arrays are incomplete") + vector_f64 = ( + "sensor_height_m", + "slope_deg", + "roughness_m", + "confidence", + "pose_binding_age_ms", + "surface_max_age_ms", + ) + vector_i64 = ( + "surface_cell_count", + "surface_inlier_cell_count", + "classified_point_count", + "surface_point_count", + "occupied_point_count", + "below_surface_point_count", + ) + if ( + self.report.get("schema_version") != K1_LOCAL_SURFACE_REPORT_SCHEMA + or self.report.get("model_id") != self.root.name + or self.report.get("status") != "diagnostic-only" + or self.report.get("ground_truth") is not False + or self.arrays["frame_valid"].shape != (frame_count,) + or self.arrays["frame_valid"].dtype != np.dtype("?") + or self.arrays["frame_failure_code"].shape != (frame_count,) + or self.arrays["frame_failure_code"].dtype != np.dtype("u1") + or np.any(self.arrays["frame_failure_code"] > FRAME_FIT_FAILED) + or self.arrays["plane_coefficients_map"].shape != (frame_count, 4) + or self.arrays["plane_coefficients_map"].dtype != np.dtype(" POINT_BELOW_SURFACE) + or self.arrays["point_height_m"].shape != (point_count,) + or self.arrays["point_height_m"].dtype != np.dtype(" 1) + or self.identity.get("valid_frame_count") != int(np.count_nonzero(valid)) + ): + raise LidarGroundError("K1 local-surface frame validity is inconsistent") + authority = _object(self.report.get("authority"), "K1 local-surface authority") + policy = _object(self.report.get("occupancy_policy"), "K1 local-surface policy") + if ( + authority.get("commands_enabled") is not False + or authority.get("navigation_or_safety_accepted") is not False + or policy.get("absence_of_points_means_free") is not False + or policy.get("unknown_is_traversable") is not False + ): + raise LidarGroundError("K1 local-surface authority is invalid") + + def frame_detail( + self, + source: E10LidarFieldSource, + frame_index: int, + ) -> dict[str, object]: + _validate_source_binding(self, source) + if not 0 <= frame_index < source.frame_count: + raise IndexError(frame_index) + offsets = source.arrays["cloud_offsets"] + start = int(offsets[frame_index]) + end = int(offsets[frame_index + 1]) + point_class = self.arrays["point_class"][start:end] + counts = { + "classified": int(np.count_nonzero(point_class)), + "surface": int(np.count_nonzero(point_class == POINT_SURFACE)), + "occupied": int(np.count_nonzero(point_class == POINT_OCCUPIED)), + "below_surface": int(np.count_nonzero(point_class == POINT_BELOW_SURFACE)), + } + expected = { + "classified": int(self.arrays["classified_point_count"][frame_index]), + "surface": int(self.arrays["surface_point_count"][frame_index]), + "occupied": int(self.arrays["occupied_point_count"][frame_index]), + "below_surface": int(self.arrays["below_surface_point_count"][frame_index]), + } + if counts != expected: + raise LidarGroundError("K1 local-surface frame counts are inconsistent") + return { + "schema_version": K1_LOCAL_SURFACE_FRAME_SCHEMA, + "model_id": self.model_id, + "source_pack_id": source.pack_id, + "session_id": source.identity["session_id"], + "frame_index": frame_index, + "frame_count": source.frame_count, + "source_frame_index": int(source.arrays["source_frame_indices"][frame_index]), + "session_seconds": float(source.arrays["session_seconds"][frame_index]), + "source_available": bool(source.arrays["sample_available"][frame_index]), + "valid": bool(self.arrays["frame_valid"][frame_index]), + "failure_code": int(self.arrays["frame_failure_code"][frame_index]), + "point_count": end - start, + "coordinate_frame": "map", + "distance_unit": "m", + "points_xyz_m": source.arrays["cloud_points_map"][start:end] + .astype(np.float64) + .tolist(), + "point_class": point_class.astype(np.int64).tolist(), + "point_height_m": self.arrays["point_height_m"][start:end] + .astype(np.float64) + .tolist(), + "pose": { + "position_xyz_m": source.arrays["pose_positions_map"][frame_index] + .astype(np.float64) + .tolist(), + "orientation_xyzw": source.arrays[ + "pose_quaternions_map_from_lidar" + ][frame_index] + .astype(np.float64) + .tolist(), + "binding_age_ms": float(self.arrays["pose_binding_age_ms"][frame_index]), + }, + "surface": { + "plane_coefficients_map": self.arrays["plane_coefficients_map"][frame_index] + .astype(np.float64) + .tolist(), + "sensor_height_m": float(self.arrays["sensor_height_m"][frame_index]), + "slope_deg": float(self.arrays["slope_deg"][frame_index]), + "roughness_m": float(self.arrays["roughness_m"][frame_index]), + "confidence": float(self.arrays["confidence"][frame_index]), + "surface_max_age_ms": float( + self.arrays["surface_max_age_ms"][frame_index] + ), + "cell_count": int(self.arrays["surface_cell_count"][frame_index]), + "inlier_cell_count": int( + self.arrays["surface_inlier_cell_count"][frame_index] + ), + }, + "counts": counts, + "classes": { + "0": "unclassified-or-outside-local-radius", + "1": "observed-surface", + "2": "observed-occupied-above-surface", + "3": "below-surface-or-negative-outlier", + }, + "occupancy_policy": self.report["occupancy_policy"], + "ground_truth": False, + "access": "read-only", + "authority": self.report["authority"], + } + + +def build_k1_local_surface( + source: E10LidarFieldSource, + output_root: Path, + *, + profile: K1LocalSurfaceProfile = DEFAULT_K1_LOCAL_SURFACE_PROFILE, + display_name: str = "RAVNOVES00 · динамическая локальная поверхность K1", +) -> Path: + """Build a deterministic local-surface derivative without mutating the K1 source.""" + + if not display_name.strip() or len(display_name) > 200: + raise LidarGroundError("K1 local-surface display name is invalid") + started = time.perf_counter() + frame_count = source.frame_count + point_count = source.point_count + arrays = _empty_arrays(frame_count, point_count) + source_arrays = source.arrays + available = source_arrays["sample_available"] + offsets = source_arrays["cloud_offsets"] + points_map = source_arrays["cloud_points_map"] + positions = source_arrays["pose_positions_map"] + times = source_arrays["session_seconds"] + pose_delta = np.abs(source_arrays["pose_point_delta_ms"]) + cache: dict[tuple[int, int], tuple[float, float]] = {} + + for frame_index in range(frame_count): + start = int(offsets[frame_index]) + end = int(offsets[frame_index + 1]) + arrays["pose_binding_age_ms"][frame_index] = ( + float(pose_delta[frame_index]) if np.isfinite(pose_delta[frame_index]) else 0.0 + ) + if not bool(available[frame_index]): + arrays["frame_failure_code"][frame_index] = FRAME_SOURCE_UNAVAILABLE + continue + if ( + not np.isfinite(pose_delta[frame_index]) + or float(pose_delta[frame_index]) > profile.maximum_pose_binding_ms + ): + arrays["frame_failure_code"][frame_index] = FRAME_POSE_STALE + continue + cloud = np.asarray(points_map[start:end], dtype=np.float64) + position = np.asarray(positions[frame_index], dtype=np.float64) + session_seconds = float(times[frame_index]) + local = np.sum((cloud[:, :2] - position[:2]) ** 2, axis=1) <= ( + profile.local_radius_m**2 + ) + local_cloud = cloud[local] + _expire_cache(cache, session_seconds, position, profile) + _update_cache(cache, local_cloud, session_seconds, profile) + cell_points, cell_times = _local_cache_points(cache, position, profile) + arrays["surface_cell_count"][frame_index] = cell_points.shape[0] + if cell_points.shape[0] < profile.minimum_surface_cells: + arrays["frame_failure_code"][frame_index] = FRAME_INSUFFICIENT_SURFACE + continue + fit = _fit_surface(cell_points, position, profile) + if fit is None: + arrays["frame_failure_code"][frame_index] = FRAME_FIT_FAILED + continue + plane, inliers, residuals = fit + slope_deg = math.degrees( + math.atan2(math.hypot(float(plane[0]), float(plane[1])), float(plane[2])) + ) + if not np.isfinite(slope_deg) or slope_deg > profile.maximum_slope_deg: + arrays["frame_failure_code"][frame_index] = FRAME_FIT_FAILED + continue + heights = _height_above_plane(cloud, plane) + local_classes = np.zeros(cloud.shape[0], dtype=np.uint8) + local_classes[local & (np.abs(heights) <= profile.surface_band_m)] = POINT_SURFACE + local_classes[ + local + & (heights >= profile.obstacle_min_height_m) + & (heights <= profile.obstacle_max_height_m) + ] = POINT_OCCUPIED + local_classes[local & (heights < -profile.surface_band_m)] = POINT_BELOW_SURFACE + point_heights = np.zeros(cloud.shape[0], dtype=np.float32) + point_heights[local] = heights[local].astype(np.float32) + sensor_height = float(_height_above_plane(position.reshape(1, 3), plane)[0]) + roughness = float(np.median(np.abs(residuals[inliers]))) + coverage = min(1.0, int(np.count_nonzero(inliers)) / (profile.minimum_surface_cells * 3)) + roughness_confidence = math.exp(-roughness / max(profile.surface_band_m, 1e-6)) + pose_confidence = max( + 0.0, + 1.0 - float(pose_delta[frame_index]) / profile.maximum_pose_binding_ms, + ) + confidence = float(np.clip(coverage * roughness_confidence * pose_confidence, 0.0, 1.0)) + arrays["frame_valid"][frame_index] = True + arrays["frame_failure_code"][frame_index] = FRAME_VALID + arrays["plane_coefficients_map"][frame_index] = plane + arrays["sensor_height_m"][frame_index] = sensor_height + arrays["slope_deg"][frame_index] = slope_deg + arrays["roughness_m"][frame_index] = roughness + arrays["confidence"][frame_index] = confidence + arrays["surface_max_age_ms"][frame_index] = max( + 0.0, + (session_seconds - float(np.min(cell_times[inliers]))) * 1_000.0, + ) + arrays["surface_inlier_cell_count"][frame_index] = int(np.count_nonzero(inliers)) + arrays["point_class"][start:end] = local_classes + arrays["point_height_m"][start:end] = point_heights + arrays["classified_point_count"][frame_index] = int( + np.count_nonzero(local_classes) + ) + arrays["surface_point_count"][frame_index] = int( + np.count_nonzero(local_classes == POINT_SURFACE) + ) + arrays["occupied_point_count"][frame_index] = int( + np.count_nonzero(local_classes == POINT_OCCUPIED) + ) + arrays["below_surface_point_count"][frame_index] = int( + np.count_nonzero(local_classes == POINT_BELOW_SURFACE) + ) + + logical_content_sha256 = _logical_sha256(arrays) + valid = arrays["frame_valid"] + identity = { + "schema_version": K1_LOCAL_SURFACE_SCHEMA, + "source_pack_id": source.pack_id, + "source_pack_identity_sha256": source.manifest["identity_sha256"], + "source_artifact_sha256": source.manifest["artifact"]["sha256"], + "session_id": source.identity["session_id"], + "display_name": display_name, + "frame_count": frame_count, + "valid_frame_count": int(np.count_nonzero(valid)), + "point_count": point_count, + "profile": profile.to_dict(), + "logical_content_sha256": logical_content_sha256, + "producer_sha256": _sha256(Path(__file__).resolve(strict=True)), + "ground_truth": False, + "authority": { + "commands_enabled": False, + "navigation_or_safety_accepted": False, + }, + } + identity_sha256 = hashlib.sha256(_canonical_json(identity)).hexdigest() + model_id = f"k1-local-surface-{identity_sha256}" + output_parent = output_root.expanduser().resolve() + output_parent.mkdir(mode=0o700, parents=True, exist_ok=True) + output = output_parent / model_id + if output.exists(): + existing = K1LocalSurfaceV1(output) + existing.close() + return output + + report = { + "schema_version": K1_LOCAL_SURFACE_REPORT_SCHEMA, + "model_id": model_id, + "display_name": display_name, + "session_id": source.identity["session_id"], + "source_pack_id": source.pack_id, + "status": "diagnostic-only", + "ground_truth": False, + "source": { + "representation": "legacy-e10-vendor-map-with-pose", + "immutable": True, + "passive_processing_only": True, + "firmware_or_device_commands_used": False, + "frame_count": frame_count, + "available_lidar_frames": int(np.count_nonzero(available)), + "point_count": point_count, + "timeline_start_seconds": float(times[0]), + "timeline_end_seconds": float(times[-1]), + }, + "surface_model": { + "kind": "time-varying-rolling-local-plane", + "coordinate_frame": "map", + "gravity_alignment": "vendor-map-z-assumed-diagnostic", + "physical_sensor_height_required": False, + "hardcoded_height_m": None, + "profile": profile.to_dict(), + }, + "occupancy_policy": { + "observed_surface_class": POINT_SURFACE, + "observed_occupied_class": POINT_OCCUPIED, + "below_surface_or_negative_outlier_class": POINT_BELOW_SURFACE, + "absence_of_points_means_free": False, + "unknown_is_traversable": False, + "persistent_reconstruction_mutated": False, + "dynamic_object_layer_available": False, + }, + "metrics": { + "frames": { + "total": frame_count, + "source_available": int(np.count_nonzero(available)), + "valid": int(np.count_nonzero(valid)), + "source_unavailable": int( + np.count_nonzero( + arrays["frame_failure_code"] == FRAME_SOURCE_UNAVAILABLE + ) + ), + "pose_stale": int( + np.count_nonzero(arrays["frame_failure_code"] == FRAME_POSE_STALE) + ), + "insufficient_surface": int( + np.count_nonzero( + arrays["frame_failure_code"] == FRAME_INSUFFICIENT_SURFACE + ) + ), + "fit_failed": int( + np.count_nonzero(arrays["frame_failure_code"] == FRAME_FIT_FAILED) + ), + }, + "sensor_height_m": _valid_distribution(arrays["sensor_height_m"], valid), + "slope_deg": _valid_distribution(arrays["slope_deg"], valid), + "roughness_m": _valid_distribution(arrays["roughness_m"], valid), + "confidence": _valid_distribution(arrays["confidence"], valid), + "pose_binding_age_ms": _valid_distribution( + arrays["pose_binding_age_ms"], available + ), + "surface_max_age_ms": _valid_distribution( + arrays["surface_max_age_ms"], valid + ), + "build_elapsed_ms": (time.perf_counter() - started) * 1_000.0, + }, + "anchors": _anchors(source, valid), + "decision": { + "status": "replay-experiment-only", + "production_promotion": False, + "next_gate": ( + "review the full recording, qualify local-surface stability, then run " + "bounded latest-wins live shadow without commands" + ), + }, + "authority": { + "commands_enabled": False, + "navigation_or_safety_accepted": False, + }, + } + staging = output_parent / f".{model_id}.{os.getpid()}.incomplete" + staging.mkdir(mode=0o700, exist_ok=False) + try: + arrays_path = staging / K1_LOCAL_SURFACE_ARRAYS_NAME + np.savez_compressed(arrays_path, **arrays) # type: ignore[arg-type] + report_path = staging / K1_LOCAL_SURFACE_REPORT_NAME + _write_json(report_path, report) + manifest = { + "schema_version": K1_LOCAL_SURFACE_SCHEMA, + "model_id": model_id, + "identity_sha256": identity_sha256, + "identity": identity, + "created_at_utc": _utc_now(), + "classification": "private-derived-lidar-diagnostic", + "ground_truth": False, + "artifacts": [ + _artifact("local-surface", arrays_path, "application/x-npz"), + _artifact("local-surface-report", report_path, "application/json"), + ], + } + _write_json(staging / K1_LOCAL_SURFACE_MANIFEST_NAME, manifest) + os.replace(staging, output) + except BaseException: + shutil.rmtree(staging, ignore_errors=True) + raise + validation = K1LocalSurfaceV1(output) + validation.close() + return output + + +def k1_local_surface_catalog_item(model: K1LocalSurfaceV1) -> dict[str, object]: + return { + "model_id": model.model_id, + "display_name": model.report["display_name"], + "session_id": model.report["session_id"], + "source_pack_id": model.report["source_pack_id"], + "status": model.report["status"], + "source": model.report["source"], + "surface_model": model.report["surface_model"], + "occupancy_policy": model.report["occupancy_policy"], + "metrics": model.report["metrics"], + "anchors": model.report["anchors"], + "decision": model.report["decision"], + "created_at_utc": model.manifest.get("created_at_utc"), + "ground_truth": False, + "authority": model.report["authority"], + } + + +def _empty_arrays( + frame_count: int, + point_count: int, +) -> dict[str, npt.NDArray[Any]]: + return { + "frame_valid": np.zeros(frame_count, dtype="?"), + "frame_failure_code": np.full( + frame_count, FRAME_SOURCE_UNAVAILABLE, dtype="u1" + ), + "plane_coefficients_map": np.zeros((frame_count, 4), dtype=" None: + if cloud.shape[0] == 0: + return + cells = np.floor(cloud[:, :2] / profile.cell_size_m).astype(np.int64) + order = np.lexsort((cells[:, 1], cells[:, 0])) + sorted_cells = cells[order] + sorted_z = cloud[order, 2] + changes = np.flatnonzero(np.any(np.diff(sorted_cells, axis=0) != 0, axis=1)) + 1 + starts = np.concatenate((np.asarray([0]), changes)) + ends = np.concatenate((changes, np.asarray([cloud.shape[0]]))) + for start, end in zip(starts, ends, strict=True): + key = (int(sorted_cells[start, 0]), int(sorted_cells[start, 1])) + z = float(np.percentile(sorted_z[start:end], profile.cell_lower_percentile)) + cache[key] = (z, session_seconds) + + +def _expire_cache( + cache: dict[tuple[int, int], tuple[float, float]], + session_seconds: float, + position: npt.NDArray[np.float64], + profile: K1LocalSurfaceProfile, +) -> None: + maximum_radius_sq = (profile.local_radius_m + profile.cell_size_m) ** 2 + expired = [ + key + for key, (_, observed_seconds) in cache.items() + if session_seconds - observed_seconds > profile.surface_ttl_s + or ( + (key[0] + 0.5) * profile.cell_size_m - float(position[0]) + ) + ** 2 + + ( + (key[1] + 0.5) * profile.cell_size_m - float(position[1]) + ) + ** 2 + > maximum_radius_sq + ] + for key in expired: + del cache[key] + + +def _local_cache_points( + cache: Mapping[tuple[int, int], tuple[float, float]], + position: npt.NDArray[np.float64], + profile: K1LocalSurfaceProfile, +) -> tuple[npt.NDArray[np.float64], npt.NDArray[np.float64]]: + values = [ + ( + (key[0] + 0.5) * profile.cell_size_m, + (key[1] + 0.5) * profile.cell_size_m, + z, + observed_seconds, + ) + for key, (z, observed_seconds) in sorted(cache.items()) + if ( + ((key[0] + 0.5) * profile.cell_size_m - float(position[0])) ** 2 + + ((key[1] + 0.5) * profile.cell_size_m - float(position[1])) ** 2 + <= profile.local_radius_m**2 + ) + ] + if not values: + return np.empty((0, 3), dtype=np.float64), np.empty(0, dtype=np.float64) + array = np.asarray(values, dtype=np.float64) + return array[:, :3], array[:, 3] + + +def _fit_surface( + cell_points: npt.NDArray[np.float64], + position: npt.NDArray[np.float64], + profile: K1LocalSurfaceProfile, +) -> tuple[ + npt.NDArray[np.float64], + npt.NDArray[np.bool_], + npt.NDArray[np.float64], +] | None: + centered_xy = cell_points[:, :2] - position[:2] + design = np.column_stack( + (centered_xy[:, 0], centered_xy[:, 1], np.ones(cell_points.shape[0])) + ) + cutoff = float(np.quantile(cell_points[:, 2], profile.initial_lower_fraction)) + inliers = cell_points[:, 2] <= cutoff + if int(np.count_nonzero(inliers)) < profile.minimum_surface_cells: + return None + coefficients = np.zeros(3, dtype=np.float64) + for _ in range(profile.robust_iterations): + try: + coefficients, _, rank, _ = np.linalg.lstsq( + design[inliers], cell_points[inliers, 2], rcond=None + ) + except np.linalg.LinAlgError: + return None + if rank < 3 or not np.isfinite(coefficients).all(): + return None + residuals = cell_points[:, 2] - design @ coefficients + center = float(np.median(residuals[inliers])) + mad = float(np.median(np.abs(residuals[inliers] - center))) + band = max( + profile.minimum_inlier_band_m, + profile.robust_mad_scale * 1.4826 * mad, + ) + updated = np.abs(residuals - center) <= band + if int(np.count_nonzero(updated)) < profile.minimum_surface_cells: + return None + if np.array_equal(updated, inliers): + break + inliers = updated + coefficients, _, rank, _ = np.linalg.lstsq( + design[inliers], cell_points[inliers, 2], rcond=None + ) + if rank < 3 or not np.isfinite(coefficients).all(): + return None + a, b, c = (float(value) for value in coefficients) + unnormalized = np.asarray( + [-a, -b, 1.0, a * float(position[0]) + b * float(position[1]) - c], + dtype=np.float64, + ) + norm = float(np.linalg.norm(unnormalized[:3])) + if norm <= 0 or not np.isfinite(norm): + return None + plane = unnormalized / norm + residuals = _height_above_plane(cell_points, plane) + center = float(np.median(residuals[inliers])) + mad = float(np.median(np.abs(residuals[inliers] - center))) + band = max( + profile.minimum_inlier_band_m, + profile.robust_mad_scale * 1.4826 * mad, + ) + inliers = np.abs(residuals - center) <= band + if int(np.count_nonzero(inliers)) < profile.minimum_surface_cells: + return None + return plane.astype(" npt.NDArray[np.float64]: + return points @ plane[:3] + float(plane[3]) + + +def _anchors( + source: E10LidarFieldSource, + valid: npt.NDArray[np.bool_], +) -> list[dict[str, object]]: + source_indices = source.arrays["source_frame_indices"] + available_valid = source.arrays["sample_available"] & valid + candidates = np.flatnonzero(available_valid) + if candidates.size == 0: + candidates = np.flatnonzero(source.arrays["sample_available"]) + anchors: list[dict[str, object]] = [] + for window in RAVNOVES00_CENTRAL_WINDOWS: + if candidates.size == 0: + frame_index = 0 + else: + frame_index = int( + candidates[ + np.argmin( + np.abs( + source_indices[candidates].astype(np.int64) + - window.preview_source_frame_index + ) + ) + ] + ) + anchors.append( + { + "key": window.key, + "label": window.label, + "frame_index": frame_index, + "source_frame_index": int(source_indices[frame_index]), + "session_seconds": float(source.arrays["session_seconds"][frame_index]), + "valid": bool(valid[frame_index]), + } + ) + return anchors + + +def _validate_source_binding( + model: K1LocalSurfaceV1, + source: E10LidarFieldSource, +) -> None: + if ( + model.identity.get("source_pack_id") != source.pack_id + or model.identity.get("source_pack_identity_sha256") + != source.manifest.get("identity_sha256") + or model.identity.get("source_artifact_sha256") + != source.manifest.get("artifact", {}).get("sha256") + or model.identity.get("frame_count") != source.frame_count + or model.identity.get("point_count") != source.point_count + ): + raise LidarGroundError("K1 local-surface source binding is invalid") + + +def _valid_distribution( + values: npt.NDArray[np.float64], + mask: npt.NDArray[np.bool_], +) -> dict[str, float | int | None]: + selected = values[mask] + if selected.size == 0: + return { + "sample_count": 0, + "minimum": None, + "mean": None, + "p50": None, + "p95": None, + "maximum": None, + } + if not np.isfinite(selected).all(): + raise LidarGroundError("K1 local-surface distribution is invalid") + return { + "sample_count": int(selected.shape[0]), + "minimum": float(np.min(selected)), + "mean": float(np.mean(selected)), + "p50": float(np.percentile(selected, 50)), + "p95": float(np.percentile(selected, 95)), + "maximum": float(np.max(selected)), + } + + +def _logical_sha256(arrays: Mapping[str, npt.NDArray[Any]]) -> str: + digest = hashlib.sha256() + for name in sorted(arrays): + array = np.ascontiguousarray(arrays[name]) + digest.update(name.encode()) + digest.update(array.dtype.str.encode()) + digest.update(_canonical_json(list(array.shape))) + digest.update(memoryview(array).cast("B")) + return digest.hexdigest() + + +def _validate_artifacts(root: Path, value: object) -> dict[str, Path]: + artifacts = _list(value, "K1 local-surface artifacts") + resolved: dict[str, Path] = {} + for value in artifacts: + item = _object(value, "K1 local-surface artifact") + role = item.get("role") + relative = item.get("path") + if ( + not isinstance(role, str) + or role in resolved + or not isinstance(relative, str) + or Path(relative).name != relative + or not isinstance(item.get("byte_length"), int) + or isinstance(item.get("byte_length"), bool) + or item["byte_length"] < 1 + or _SHA256.fullmatch(str(item.get("sha256"))) is None + ): + raise LidarGroundError("K1 local-surface artifact descriptor is invalid") + path = root / relative + if ( + path.is_symlink() + or not path.is_file() + or path.stat().st_size != item["byte_length"] + or _sha256(path) != item["sha256"] + ): + raise LidarGroundError("K1 local-surface artifact is invalid") + resolved[role] = path + if "local-surface" not in resolved or "local-surface-report" not in resolved: + raise LidarGroundError("K1 local-surface artifacts are incomplete") + return resolved + + +def _artifact(role: str, path: Path, media_type: str) -> dict[str, object]: + return { + "role": role, + "path": path.name, + "media_type": media_type, + "byte_length": path.stat().st_size, + "sha256": _sha256(path), + } + + +def _canonical_json(value: object) -> bytes: + return json.dumps( + value, + ensure_ascii=False, + sort_keys=True, + separators=(",", ":"), + allow_nan=False, + ).encode() + + +def _read_json(path: Path) -> dict[str, Any]: + if path.is_symlink() or not path.is_file(): + raise LidarGroundError("K1 local-surface JSON is missing") + try: + value = json.loads(path.read_text(encoding="utf-8")) + except (OSError, UnicodeDecodeError, json.JSONDecodeError) as exc: + raise LidarGroundError("K1 local-surface JSON is invalid") from exc + return _object(value, "K1 local-surface JSON") + + +def _write_json(path: Path, value: object) -> None: + path.write_bytes(_canonical_json(value) + b"\n") + + +def _object(value: object, label: str) -> dict[str, Any]: + if not isinstance(value, dict): + raise LidarGroundError(f"{label} is invalid") + return value + + +def _list(value: object, label: str) -> list[Any]: + if not isinstance(value, list): + raise LidarGroundError(f"{label} is invalid") + return value + + +def _nonnegative_int(value: object, label: str) -> int: + if not isinstance(value, int) or isinstance(value, bool) or value < 0: + raise LidarGroundError(f"{label} is invalid") + return value + + +def _sha256(path: Path) -> str: + digest = hashlib.sha256() + with path.open("rb") as stream: + while chunk := stream.read(1024 * 1024): + digest.update(chunk) + return digest.hexdigest() + + +def _utc_now() -> str: + return datetime.now(UTC).isoformat() diff --git a/src/k1link/web/app.py b/src/k1link/web/app.py index 03ff75b..653a80f 100644 --- a/src/k1link/web/app.py +++ b/src/k1link/web/app.py @@ -420,6 +420,27 @@ app.include_router( / "lidar-ground-v1" / "benchmarks" ), + field_review_root_provider=lambda: ( + REPOSITORY_ROOT + / ".runtime" + / "compute-experiments" + / "lidar-field-review-v1" + / "reviews" + ), + local_surface_root_provider=lambda: ( + REPOSITORY_ROOT + / ".runtime" + / "compute-experiments" + / "k1-local-surface-v1" + / "models" + ), + e10_source_root_provider=lambda: ( + REPOSITORY_ROOT + / ".runtime" + / "compute-experiments" + / "e10" + / "lidar-packs" + ), dataset_admission_provider=lambda: ( REPOSITORY_ROOT / ".runtime" / "dataset-gateway" / "admission.json" ), diff --git a/src/k1link/web/lidar_api.py b/src/k1link/web/lidar_api.py index e276392..0812bc5 100644 --- a/src/k1link/web/lidar_api.py +++ b/src/k1link/web/lidar_api.py @@ -9,11 +9,14 @@ from typing import Annotated, Any, Final from fastapi import APIRouter, HTTPException, Query, Response from k1link.compute import ( + E10LidarFieldSource, + K1LocalSurfaceV1, LidarFieldReviewV1, LidarGroundBenchmarkV1, LidarGroundError, LidarReplayError, LidarReplayPackV2, + k1_local_surface_catalog_item, lidar_field_review_catalog_item, lidar_ground_benchmark_catalog_item, lidar_ground_frame_detail, @@ -34,9 +37,12 @@ from k1link.datasets import ( LIDAR_CATALOG_SCHEMA: Final = "missioncore.lidar-replay-pack-catalog/v1" LIDAR_GROUND_CATALOG_SCHEMA: Final = "missioncore.lidar-ground-benchmark-catalog/v1" LIDAR_FIELD_REVIEW_CATALOG_SCHEMA: Final = "missioncore.lidar-field-review-catalog/v1" +K1_LOCAL_SURFACE_CATALOG_SCHEMA: Final = "missioncore.k1-local-surface-catalog/v1" _PACK_ID = re.compile(r"^lidar-replay-pack-[a-f0-9]{64}$") _BENCHMARK_ID = re.compile(r"^ground-benchmark-[a-f0-9]{64}$") _FIELD_REVIEW_ID = re.compile(r"^lidar-field-review-[a-f0-9]{64}$") +_LOCAL_SURFACE_ID = re.compile(r"^k1-local-surface-[a-f0-9]{64}$") +_E10_PACK_ID = re.compile(r"^e10-lidar-pack-[a-f0-9]{64}$") RootProvider = Callable[[], Path | None] DatasetArtifactProvider = Callable[[], Path | None] @@ -56,11 +62,23 @@ def configured_lidar_field_review_root() -> Path | None: return Path(value).expanduser().absolute() if value else None +def configured_k1_local_surface_root() -> Path | None: + value = os.environ.get("MISSIONCORE_K1_LOCAL_SURFACE_ROOT", "").strip() + return Path(value).expanduser().absolute() if value else None + + +def configured_e10_lidar_source_root() -> Path | None: + value = os.environ.get("MISSIONCORE_E10_LIDAR_SOURCE_ROOT", "").strip() + return Path(value).expanduser().absolute() if value else None + + def build_lidar_router( *, root_provider: RootProvider = configured_lidar_replay_root, ground_root_provider: RootProvider = configured_lidar_ground_root, field_review_root_provider: RootProvider = configured_lidar_field_review_root, + local_surface_root_provider: RootProvider = configured_k1_local_surface_root, + e10_source_root_provider: RootProvider = configured_e10_lidar_source_root, dataset_admission_provider: DatasetArtifactProvider = configured_dataset_admission_manifest, dataset_preview_provider: DatasetArtifactProvider = configured_dataset_preview, dataset_rellis_preview_provider: DatasetArtifactProvider = lambda: None, @@ -483,4 +501,111 @@ def build_lidar_router( headers={"Cache-Control": "private, max-age=31536000, immutable"}, ) + @router.get("/local-surfaces") + def list_k1_local_surfaces( + limit: int = Query(default=10, ge=1, le=50), + ) -> dict[str, Any]: + root = local_surface_root_provider() + if root is None or not root.is_dir(): + return { + "schema_version": K1_LOCAL_SURFACE_CATALOG_SCHEMA, + "configured": root is not None, + "items": [], + "valid_total": 0, + "invalid_total": 0, + "access": "read-only", + } + items: list[dict[str, object]] = [] + invalid_total = 0 + candidates = sorted( + ( + candidate + for candidate in root.iterdir() + if candidate.is_dir() + and _LOCAL_SURFACE_ID.fullmatch(candidate.name) is not None + ), + key=lambda candidate: candidate.stat().st_mtime_ns, + reverse=True, + ) + for candidate in candidates: + try: + model = K1LocalSurfaceV1(candidate) + try: + items.append(k1_local_surface_catalog_item(model)) + finally: + model.close() + except (LidarGroundError, OSError): + invalid_total += 1 + return { + "schema_version": K1_LOCAL_SURFACE_CATALOG_SCHEMA, + "configured": True, + "items": items[:limit], + "valid_total": len(items), + "invalid_total": invalid_total, + "access": "read-only", + } + + @router.get("/local-surfaces/{model_id}/frames/{frame_index}") + def get_k1_local_surface_frame( + model_id: str, + frame_index: int, + ) -> dict[str, object]: + if _LOCAL_SURFACE_ID.fullmatch(model_id) is None or frame_index < 0: + raise HTTPException( + status_code=404, + detail="K1 local-surface frame не найден", + ) + model_root = local_surface_root_provider() + source_root = e10_source_root_provider() + if model_root is None or not model_root.is_dir(): + raise HTTPException( + status_code=503, + detail="K1 local-surface storage не настроен", + ) + if source_root is None or not source_root.is_dir(): + raise HTTPException( + status_code=503, + detail="E10 LiDAR source storage не настроен", + ) + model_path = model_root / model_id + if not model_path.is_dir(): + raise HTTPException( + status_code=404, + detail="K1 local-surface model не найден", + ) + try: + model = K1LocalSurfaceV1(model_path) + try: + source_pack_id = model.identity.get("source_pack_id") + if ( + not isinstance(source_pack_id, str) + or _E10_PACK_ID.fullmatch(source_pack_id) is None + ): + raise LidarGroundError("K1 local-surface source id is invalid") + source_path = source_root / source_pack_id + if not source_path.is_dir(): + raise HTTPException( + status_code=404, + detail="Связанный E10 LiDAR source не найден", + ) + source = E10LidarFieldSource(source_path) + try: + return model.frame_detail(source, frame_index) + finally: + source.close() + finally: + model.close() + except IndexError as exc: + raise HTTPException( + status_code=404, + detail="K1 local-surface frame не найден", + ) from exc + except HTTPException: + raise + except (LidarGroundError, OSError) as exc: + raise HTTPException( + status_code=409, + detail="K1 local-surface evidence не прошло проверку целостности", + ) from exc + return router diff --git a/tests/test_lidar_local_surface.py b/tests/test_lidar_local_surface.py new file mode 100644 index 0000000..76dd55d --- /dev/null +++ b/tests/test_lidar_local_surface.py @@ -0,0 +1,195 @@ +from __future__ import annotations + +import hashlib +import json +from pathlib import Path + +import numpy as np +from fastapi import APIRouter +from fastapi.routing import APIRoute + +from k1link.compute import ( + E10_LIDAR_PACK_SCHEMA, + E10LidarFieldSource, + K1LocalSurfaceProfile, + K1LocalSurfaceV1, + build_k1_local_surface, +) +from k1link.web.lidar_api import build_lidar_router + + +def _canonical_json(value: object) -> bytes: + return json.dumps( + value, + ensure_ascii=False, + sort_keys=True, + separators=(",", ":"), + allow_nan=False, + ).encode() + + +def _sha256(path: Path) -> str: + return hashlib.sha256(path.read_bytes()).hexdigest() + + +def _source_pack(root: Path) -> Path: + frame_count = 8 + available = np.asarray([True, True, True, True, True, True, True, False]) + poses: list[list[float]] = [] + clouds: list[np.ndarray] = [] + offsets = [0] + for frame_index in range(frame_count): + pose_x = frame_index * 0.35 + pose_y = 0.1 * np.sin(frame_index) + ground_z = 0.04 * pose_x - 0.015 * pose_y + poses.append([pose_x, pose_y, ground_z + 1.42]) + if not available[frame_index]: + offsets.append(offsets[-1]) + continue + axis = np.linspace(-3.5, 3.5, 12) + xx, yy = np.meshgrid(axis + pose_x, axis + pose_y) + zz = 0.04 * xx - 0.015 * yy + 0.008 * np.sin(xx * 2 + frame_index) + ground = np.column_stack((xx.ravel(), yy.ravel(), zz.ravel())) + obstacle_xy = ground[::13, :2] + obstacle_z = ( + 0.04 * obstacle_xy[:, 0] - 0.015 * obstacle_xy[:, 1] + 0.75 + ) + obstacle = np.column_stack((obstacle_xy, obstacle_z)) + cloud = np.concatenate((ground, obstacle)).astype(" object: + for route in router.routes: + if ( + isinstance(route, APIRoute) + and route.path == path + and route.methods is not None + and "GET" in route.methods + ): + return route.endpoint + raise AssertionError(f"GET {path} route is missing") + + +def test_k1_local_surface_is_dynamic_source_bound_and_read_only( + tmp_path: Path, +) -> None: + source_path = _source_pack(tmp_path / "source") + source_artifact = source_path / "lidar-pack.npz" + source_sha256 = _sha256(source_artifact) + profile = K1LocalSurfaceProfile( + profile_id="synthetic-dynamic-local-surface/v1", + local_radius_m=5.0, + cell_size_m=0.5, + surface_ttl_s=0.5, + minimum_surface_cells=12, + ) + source = E10LidarFieldSource(source_path) + try: + output = build_k1_local_surface( + source, + tmp_path / "models", + profile=profile, + ) + duplicate = build_k1_local_surface( + source, + tmp_path / "models", + profile=profile, + ) + finally: + source.close() + assert duplicate == output + assert _sha256(source_artifact) == source_sha256 + + model = K1LocalSurfaceV1(output) + source = E10LidarFieldSource(source_path) + try: + detail = model.frame_detail(source, 2) + assert model.report["source"]["passive_processing_only"] is True + assert model.report["source"]["firmware_or_device_commands_used"] is False + assert model.report["surface_model"]["hardcoded_height_m"] is None + assert model.report["occupancy_policy"]["absence_of_points_means_free"] is False + assert model.report["metrics"]["frames"]["valid"] >= 5 + assert detail["valid"] is True + assert 1.2 < detail["surface"]["sensor_height_m"] < 1.6 + assert detail["counts"]["surface"] > 50 + assert detail["counts"]["occupied"] > 0 + assert detail["authority"]["commands_enabled"] is False + assert "1.27" not in repr({"identity": model.identity, "report": model.report}) + assert str(tmp_path) not in repr(detail) + finally: + source.close() + model.close() + + router = build_lidar_router( + root_provider=lambda: None, + ground_root_provider=lambda: None, + field_review_root_provider=lambda: None, + local_surface_root_provider=lambda: output.parent, + e10_source_root_provider=lambda: source_path.parent, + ) + catalog_route = _endpoint(router, "/api/v1/lidar/local-surfaces") + frame_route = _endpoint( + router, + "/api/v1/lidar/local-surfaces/{model_id}/frames/{frame_index}", + ) + catalog = catalog_route(limit=10) # type: ignore[operator] + frame = frame_route(model_id=output.name, frame_index=2) # type: ignore[operator] + assert catalog["valid_total"] == 1 + assert catalog["items"][0]["status"] == "diagnostic-only" + assert frame["model_id"] == output.name + assert frame["access"] == "read-only"