diff --git a/apps/control-station/src/components/laboratory/RecordedEvidenceVideoScene.tsx b/apps/control-station/src/components/laboratory/RecordedEvidenceVideoScene.tsx index 9df67df..d191815 100644 --- a/apps/control-station/src/components/laboratory/RecordedEvidenceVideoScene.tsx +++ b/apps/control-station/src/components/laboratory/RecordedEvidenceVideoScene.tsx @@ -1,8 +1,11 @@ +import { useEffect, useState } from "react"; + import { RecordedFmp4Player, type RecordedObservationPlayback, } from "../RecordedFmp4Player"; import type { ObservationSourceDescriptor } from "../../core/runtime/contracts"; +import type { RecordedCameraAdmissionState } from "../../core/observation/recordedSessionAdmission"; import { RecordedEvidenceBoxOverlay, type RecordedEvidenceBox, @@ -33,6 +36,7 @@ export function RecordedEvidenceVideoScene({ segmentCount, onPlaybackChange, onPlayingRejected, + onAdmissionChange, playbackAuthority = "media", playbackTransport = "segmented", }: { @@ -49,9 +53,22 @@ export function RecordedEvidenceVideoScene({ segmentCount?: number; onPlaybackChange?: (playback: RecordedObservationPlayback) => void; onPlayingRejected?: () => void; + onAdmissionChange?: (state: RecordedCameraAdmissionState) => void; playbackAuthority?: "media" | "host"; playbackTransport?: "segmented" | "epoch-stream"; }) { + const generation = source.delivery?.kind === "recorded-fmp4-manifest" + ? source.delivery.manifestGenerationSha256 + : "invalid"; + const [admissionPhase, setAdmissionPhase] = useState( + "loading", + ); + useEffect(() => setAdmissionPhase("loading"), [generation, source.id]); + const handleAdmissionChange = (next: RecordedCameraAdmissionState) => { + setAdmissionPhase(next.phase); + onAdmissionChange?.(next); + }; + const sourceReady = admissionPhase === "ready"; return (
- {semanticOverlay ? ( + {sourceReady && semanticOverlay ? ( ) : null} - {pointCloudOverlay ? ( + {sourceReady && pointCloudOverlay ? ( ) : null} - + {sourceReady ? ( + + ) : null}
); } diff --git a/apps/control-station/src/components/laboratory/useCanonicalRecordedLabSpatialFrame.ts b/apps/control-station/src/components/laboratory/useCanonicalRecordedLabSpatialFrame.ts new file mode 100644 index 0000000..e3ebb62 --- /dev/null +++ b/apps/control-station/src/components/laboratory/useCanonicalRecordedLabSpatialFrame.ts @@ -0,0 +1,116 @@ +import { useEffect, useRef, useState } from "react"; + +import { + fetchCanonicalRecordedLabSpatialFrame, + type CanonicalRecordedLabSpatialFrame, +} from "../../core/laboratory/canonicalRecordedLabSpatial"; + +const FRAME_CACHE_LIMIT = 12; + +/** + * Shared latest-request-wins scheduler for recorded LAB spatial evidence. + * + * A feature supplies only the sealed session identity and host-clock time. + * Cache ownership, identity fencing and stale-response suppression remain in + * the canonical instrument instead of being reimplemented per experiment. + */ +export function useCanonicalRecordedLabSpatialFrame({ + sessionId, + generationSha256, + targetTimeNs, +}: { + sessionId: string; + generationSha256: string | null; + targetTimeNs: number; +}) { + const [frame, setFrame] = useState(null); + const [error, setError] = useState(null); + const desiredRef = useRef(null); + const runningRef = useRef(false); + const mountedRef = useRef(true); + const identityRef = useRef(""); + const cacheRef = useRef(new Map()); + const pumpRef = useRef<() => void>(() => undefined); + const identity = `${sessionId}:${generationSha256 ?? "unavailable"}`; + identityRef.current = identity; + + pumpRef.current = () => { + if (runningRef.current || desiredRef.current === null || !generationSha256) return; + runningRef.current = true; + const requestIdentity = identity; + let settledTimeNs: number | null = null; + void (async () => { + while ( + mountedRef.current + && identityRef.current === requestIdentity + && desiredRef.current !== null + ) { + const requestedTimeNs = desiredRef.current; + const cached = cacheRef.current.get(requestedTimeNs); + try { + const next = cached ?? await fetchCanonicalRecordedLabSpatialFrame( + sessionId, + generationSha256, + requestedTimeNs, + ); + if (!mountedRef.current || identityRef.current !== requestIdentity) break; + if (!cached) { + cacheRef.current.set(requestedTimeNs, next); + while (cacheRef.current.size > FRAME_CACHE_LIMIT) { + const oldest = cacheRef.current.keys().next().value as number | undefined; + if (oldest === undefined) break; + cacheRef.current.delete(oldest); + } + } + setFrame(next); + setError(null); + } catch (caught: unknown) { + if (!mountedRef.current || identityRef.current !== requestIdentity) break; + setError(caught instanceof Error + ? caught.message + : "Spatial-слои записанной LAB недоступны."); + } + settledTimeNs = requestedTimeNs; + if (desiredRef.current === requestedTimeNs) break; + } + })().finally(() => { + runningRef.current = false; + if ( + mountedRef.current + && desiredRef.current !== null + && (identityRef.current !== requestIdentity || desiredRef.current !== settledTimeNs) + ) { + pumpRef.current(); + } + }); + }; + + useEffect(() => { + mountedRef.current = true; + return () => { + mountedRef.current = false; + desiredRef.current = null; + }; + }, []); + + useEffect(() => { + cacheRef.current.clear(); + desiredRef.current = null; + setFrame(null); + setError(null); + }, [identity]); + + useEffect(() => { + if (!generationSha256) return; + desiredRef.current = targetTimeNs; + const cached = cacheRef.current.get(targetTimeNs); + if (cached) { + setFrame(cached); + setError(null); + return; + } + pumpRef.current(); + }, [generationSha256, identity, targetTimeNs]); + + return { frame, error, loading: Boolean(generationSha256) && !frame && !error }; +} diff --git a/apps/control-station/src/core/laboratory/canonicalRecordedLab.ts b/apps/control-station/src/core/laboratory/canonicalRecordedLab.ts new file mode 100644 index 0000000..a028eab --- /dev/null +++ b/apps/control-station/src/core/laboratory/canonicalRecordedLab.ts @@ -0,0 +1,99 @@ +export const CANONICAL_RECORDED_LAB_TGS_HISTORY_SECONDS = 1; +export const CANONICAL_RECORDED_LAB_SPATIAL_PROFILE = "source-paced-ground-v2"; + +export interface CanonicalRecordedLabPackedCellEvidence { + centersBodyXyM: Float32Array; + zBoundsM: Float32Array; + stateCodes: Uint8Array; +} + +export interface CanonicalRecordedLabBodyGroundFrame { + originMapXyzM: readonly [number, number, number]; + sensorOriginMapXyzM: readonly [number, number, number]; + basisMapFromBody: readonly [ + readonly [number, number, number], + readonly [number, number, number], + readonly [number, number, number], + ]; +} + +export interface CanonicalRecordedLabTgsCostmap { + centersXyM: readonly (readonly [number, number])[]; + stateCodes: readonly number[]; + zBoundsM: readonly (readonly [number | null, number | null])[]; +} + +export function canonicalRecordedLabTgsIsCurrent( + currentTimeNs: number, + anchorTimeNs: number, + historySeconds = CANONICAL_RECORDED_LAB_TGS_HISTORY_SECONDS, +): boolean { + if ( + !Number.isSafeInteger(currentTimeNs) + || !Number.isSafeInteger(anchorTimeNs) + || !Number.isFinite(historySeconds) + || historySeconds <= 0 + ) return false; + const ageNs = currentTimeNs - anchorTimeNs; + return ageNs >= 0 && ageNs <= Math.round(historySeconds * 1_000_000_000); +} + +export function canonicalMapGravityLocalPointToBodyGround( + point: readonly [number, number, number], + anchor: CanonicalRecordedLabBodyGroundFrame, + current: CanonicalRecordedLabBodyGroundFrame, +): readonly [number, number, number] { + // TGS is translation-only map-gravity-local: its axes are map axes and its + // origin is the LiDAR at the source frame. It is not an anchor body frame. + const map: readonly [number, number, number] = [ + anchor.sensorOriginMapXyzM[0] + point[0], + anchor.sensorOriginMapXyzM[1] + point[1], + anchor.sensorOriginMapXyzM[2] + point[2], + ]; + const delta: readonly [number, number, number] = [ + map[0] - current.originMapXyzM[0], + map[1] - current.originMapXyzM[1], + map[2] - current.originMapXyzM[2], + ]; + return [ + current.basisMapFromBody[0][0] * delta[0] + + current.basisMapFromBody[1][0] * delta[1] + + current.basisMapFromBody[2][0] * delta[2], + current.basisMapFromBody[0][1] * delta[0] + + current.basisMapFromBody[1][1] * delta[1] + + current.basisMapFromBody[2][1] * delta[2], + current.basisMapFromBody[0][2] * delta[0] + + current.basisMapFromBody[1][2] * delta[1] + + current.basisMapFromBody[2][2] * delta[2], + ]; +} + +export function canonicalRecordedLabPackedTgsCells( + costmap: CanonicalRecordedLabTgsCostmap, + anchor: CanonicalRecordedLabBodyGroundFrame, + current: CanonicalRecordedLabBodyGroundFrame, +): CanonicalRecordedLabPackedCellEvidence { + if ( + costmap.centersXyM.length !== costmap.stateCodes.length + || costmap.centersXyM.length !== costmap.zBoundsM.length + ) throw new Error("Canonical recorded LAB TGS accounting changed"); + const centers: number[] = []; + const zBounds: number[] = []; + costmap.centersXyM.forEach(([x, y], index) => { + const bounds = costmap.zBoundsM[index] ?? [null, null]; + const center = canonicalMapGravityLocalPointToBodyGround([x, y, 0], anchor, current); + centers.push(center[0], center[1]); + if (bounds[0] === null || bounds[1] === null) { + zBounds.push(Number.NaN, Number.NaN); + return; + } + const bottom = canonicalMapGravityLocalPointToBodyGround([x, y, bounds[0]], anchor, current); + const top = canonicalMapGravityLocalPointToBodyGround([x, y, bounds[1]], anchor, current); + zBounds.push(Math.min(bottom[2], top[2]), Math.max(bottom[2], top[2])); + }); + return { + centersBodyXyM: Float32Array.from(centers), + zBoundsM: Float32Array.from(zBounds), + stateCodes: Uint8Array.from(costmap.stateCodes), + }; +} diff --git a/apps/control-station/src/core/laboratory/canonicalRecordedLabSpatial.ts b/apps/control-station/src/core/laboratory/canonicalRecordedLabSpatial.ts new file mode 100644 index 0000000..25d2e0a --- /dev/null +++ b/apps/control-station/src/core/laboratory/canonicalRecordedLabSpatial.ts @@ -0,0 +1,256 @@ +import type { LaboratoryFetch } from "./advancedResults"; +import { CANONICAL_RECORDED_LAB_SPATIAL_PROFILE } from "./canonicalRecordedLab"; + +const SAFE_SESSION_ID = /^[A-Za-z0-9][A-Za-z0-9._:-]{0,127}$/; +const SHA256 = /^[a-f0-9]{64}$/; + +export class CanonicalRecordedLabSpatialContractError extends Error {} + +export interface CanonicalRecordedLabSpatialFrame { + targetTimeNs: number; + sourceTimeNs: number; + poseTimeNs: number; + trajectoryTimeNs: number; + sourcePointCount: number; + coordinateFrame: "body-ground"; + sensorHeight: { + meters: number; + source: "initial-source-cloud-lower-quantile-median"; + sampleCount: number; + madM: number; + authority: "visual-derived"; + }; + spatialProfile: { + profileId: typeof CANONICAL_RECORDED_LAB_SPATIAL_PROFILE; + localSlamHistorySeconds: number; + localSlamRadiusM: number; + localSlamVoxelSizeM: number; + localSlamPointLimit: number; + }; + bodyFrame: { + originMapXyzM: readonly [number, number, number]; + sensorOriginMapXyzM: readonly [number, number, number]; + basisMapFromBody: readonly [ + readonly [number, number, number], + readonly [number, number, number], + readonly [number, number, number], + ]; + }; + sourcePointsBodyXyzM: readonly (readonly [number, number, number])[]; + localSlamSourceFrameCount: number; + localSlamSourcePointCount: number; + localSlamBodyXyzM: readonly (readonly [number, number, number])[]; +} + +function objectValue(value: unknown, label: string): Record { + if (!value || typeof value !== "object" || Array.isArray(value)) { + throw new CanonicalRecordedLabSpatialContractError(`${label}: ожидался объект.`); + } + return value as Record; +} + +function arrayValue(value: unknown, label: string): unknown[] { + if (!Array.isArray(value)) { + throw new CanonicalRecordedLabSpatialContractError(`${label}: ожидался массив.`); + } + return value; +} + +function exact(value: unknown, expected: unknown, label: string): void { + if (value !== expected) { + throw new CanonicalRecordedLabSpatialContractError(`${label}: контракт изменён.`); + } +} + +function numberValue(value: unknown, label: string): number { + if (typeof value !== "number" || !Number.isFinite(value)) { + throw new CanonicalRecordedLabSpatialContractError(`${label}: ожидалось число.`); + } + return value; +} + +function integerValue(value: unknown, label: string): number { + const parsed = numberValue(value, label); + if (!Number.isSafeInteger(parsed) || parsed < 0) { + throw new CanonicalRecordedLabSpatialContractError(`${label}: ожидалось целое значение.`); + } + return parsed; +} + +function pointList( + value: unknown, + label: string, +): readonly (readonly [number, number, number])[] { + return arrayValue(value, label).map((entry, index) => { + const point = arrayValue(entry, `${label}[${index}]`).map( + (channel, channelIndex) => numberValue(channel, `${label}[${index}][${channelIndex}]`), + ); + if (point.length !== 3) { + throw new CanonicalRecordedLabSpatialContractError(`${label}[${index}]: размер изменён.`); + } + return [point[0]!, point[1]!, point[2]!] as const; + }); +} + +export async function fetchCanonicalRecordedLabSpatialFrame( + sessionId: string, + generationSha256: string, + targetTimeNs: number, + { + fetcher = fetch, + signal, + }: { fetcher?: LaboratoryFetch; signal?: AbortSignal } = {}, +): Promise { + if ( + !SAFE_SESSION_ID.test(sessionId) + || !SHA256.test(generationSha256) + || !Number.isSafeInteger(targetTimeNs) + || targetTimeNs < 0 + ) { + throw new CanonicalRecordedLabSpatialContractError( + "Canonical LAB spatial identity недопустима.", + ); + } + const query = new URLSearchParams({ + generation: generationSha256, + time_ns: String(targetTimeNs), + profile: CANONICAL_RECORDED_LAB_SPATIAL_PROFILE, + }); + const response = await fetcher( + `/api/v1/observation-sessions/${encodeURIComponent(sessionId)}` + + `/canonical-lab/spatial-frame?${query.toString()}`, + { method: "GET", headers: { Accept: "application/json" }, signal }, + ); + if (!response.ok) { + throw new CanonicalRecordedLabSpatialContractError( + `Canonical LAB spatial frame недоступен: HTTP ${response.status}.`, + ); + } + const payload = objectValue(await response.json(), "canonical_lab.spatial_frame"); + exact( + payload.schema_version, + "missioncore.canonical-recorded-lab-spatial-frame/v2", + "canonical_lab.spatial_frame.schema_version", + ); + exact(payload.coordinate_frame, "body-ground", "canonical_lab.spatial_frame.coordinate_frame"); + exact(payload.target_time_ns, targetTimeNs, "canonical_lab.spatial_frame.target_time_ns"); + const sourcePoints = pointList( + payload.source_points_body_xyz_m, + "canonical_lab.spatial_frame.source_points", + ); + const localSlam = pointList( + payload.local_slam_body_xyz_m, + "canonical_lab.spatial_frame.local_slam", + ); + const sourcePointCount = integerValue( + payload.source_point_count, + "canonical_lab.spatial_frame.source_point_count", + ); + const localSlamPointCount = integerValue( + payload.local_slam_point_count, + "canonical_lab.spatial_frame.local_slam_point_count", + ); + if ( + sourcePointCount !== sourcePoints.length + || sourcePointCount > 100_000 + || localSlamPointCount !== localSlam.length + || localSlam.length > 27_000 + ) { + throw new CanonicalRecordedLabSpatialContractError( + "Canonical LAB spatial accounting изменён.", + ); + } + const bodyFrame = objectValue(payload.body_frame, "canonical_lab.spatial_frame.body_frame"); + const origin = pointList( + [bodyFrame.origin_map_xyz_m], + "canonical_lab.spatial_frame.body_frame.origin", + )[0]!; + const sensorOrigin = pointList( + [bodyFrame.sensor_origin_map_xyz_m], + "canonical_lab.spatial_frame.body_frame.sensor_origin", + )[0]!; + const basisRows = pointList( + bodyFrame.basis_map_from_body, + "canonical_lab.spatial_frame.body_frame.basis", + ); + if (basisRows.length !== 3) { + throw new CanonicalRecordedLabSpatialContractError( + "Canonical LAB spatial basis изменён.", + ); + } + const sensorHeight = objectValue(payload.sensor_height, "canonical_lab.spatial_frame.sensor_height"); + exact( + sensorHeight.source, + "initial-source-cloud-lower-quantile-median", + "canonical_lab.spatial_frame.sensor_height.source", + ); + exact( + sensorHeight.authority, + "visual-derived", + "canonical_lab.spatial_frame.sensor_height.authority", + ); + const spatialProfile = objectValue( + payload.spatial_profile, + "canonical_lab.spatial_frame.spatial_profile", + ); + exact( + spatialProfile.profile_id, + CANONICAL_RECORDED_LAB_SPATIAL_PROFILE, + "canonical_lab.spatial_frame.spatial_profile.profile_id", + ); + return { + targetTimeNs, + sourceTimeNs: integerValue(payload.source_time_ns, "canonical_lab.spatial_frame.source_time_ns"), + poseTimeNs: integerValue(payload.pose_time_ns, "canonical_lab.spatial_frame.pose_time_ns"), + trajectoryTimeNs: integerValue( + payload.trajectory_time_ns, + "canonical_lab.spatial_frame.trajectory_time_ns", + ), + sourcePointCount, + coordinateFrame: "body-ground", + sensorHeight: { + meters: numberValue(sensorHeight.meters, "canonical_lab.spatial_frame.sensor_height.meters"), + source: "initial-source-cloud-lower-quantile-median", + sampleCount: integerValue( + sensorHeight.sample_count, + "canonical_lab.spatial_frame.sensor_height.sample_count", + ), + madM: numberValue(sensorHeight.mad_m, "canonical_lab.spatial_frame.sensor_height.mad_m"), + authority: "visual-derived", + }, + spatialProfile: { + profileId: CANONICAL_RECORDED_LAB_SPATIAL_PROFILE, + localSlamHistorySeconds: numberValue( + spatialProfile.local_slam_history_seconds, + "canonical_lab.spatial_frame.spatial_profile.history", + ), + localSlamRadiusM: numberValue( + spatialProfile.local_slam_radius_m, + "canonical_lab.spatial_frame.spatial_profile.radius", + ), + localSlamVoxelSizeM: numberValue( + spatialProfile.local_slam_voxel_size_m, + "canonical_lab.spatial_frame.spatial_profile.voxel", + ), + localSlamPointLimit: integerValue( + spatialProfile.local_slam_point_limit, + "canonical_lab.spatial_frame.spatial_profile.limit", + ), + }, + bodyFrame: { + originMapXyzM: origin, + sensorOriginMapXyzM: sensorOrigin, + basisMapFromBody: [basisRows[0]!, basisRows[1]!, basisRows[2]!], + }, + sourcePointsBodyXyzM: sourcePoints, + localSlamSourceFrameCount: integerValue( + payload.local_slam_source_frame_count, + "canonical_lab.spatial_frame.local_slam_source_frames", + ), + localSlamSourcePointCount: integerValue( + payload.local_slam_source_point_count, + "canonical_lab.spatial_frame.local_slam_source_points", + ), + localSlamBodyXyzM: localSlam, + }; +} diff --git a/apps/control-station/src/core/laboratory/vegetationShadow.ts b/apps/control-station/src/core/laboratory/vegetationShadow.ts index 8b35f6d..9ed26b8 100644 --- a/apps/control-station/src/core/laboratory/vegetationShadow.ts +++ b/apps/control-station/src/core/laboratory/vegetationShadow.ts @@ -118,24 +118,6 @@ export interface VegetationRouteTgsAnchor { }; } -export interface CanonicalRecordedLabSpatialFrame { - targetTimeNs: number; - sourceTimeNs: number; - poseTimeNs: number; - trajectoryTimeNs: number; - sourcePointCount: number; - bodyFrame: { - originMapXyzM: readonly [number, number, number]; - basisMapFromBody: readonly [ - readonly [number, number, number], - readonly [number, number, number], - readonly [number, number, number], - ]; - }; - sourcePointsBodyXyzM: readonly (readonly [number, number, number])[]; - localSlamBodyXyzM: readonly (readonly [number, number, number])[]; -} - export interface VegetationFullRouteLayer { name: string; resultId: string; @@ -1044,100 +1026,6 @@ export async function fetchVegetationRouteTgsAnchor( }; } -export async function fetchCanonicalRecordedLabSpatialFrame( - sessionId: string, - generationSha256: string, - targetTimeNs: number, - { - fetcher = fetch, - signal, - }: { fetcher?: LaboratoryFetch; signal?: AbortSignal } = {}, -): Promise { - if ( - !/^[A-Za-z0-9][A-Za-z0-9._:-]{0,127}$/.test(sessionId) - || !SHA256.test(generationSha256) - || !Number.isSafeInteger(targetTimeNs) - || targetTimeNs < 0 - ) { - throw new VegetationShadowContractError("Canonical LAB spatial identity недопустима."); - } - const query = new URLSearchParams({ - generation: generationSha256, - time_ns: String(targetTimeNs), - }); - const response = await fetcher( - `/api/v1/observation-sessions/${encodeURIComponent(sessionId)}` - + `/canonical-lab/spatial-frame?${query.toString()}`, - { method: "GET", headers: { Accept: "application/json" }, signal }, - ); - if (!response.ok) { - throw new VegetationShadowContractError( - `Canonical LAB spatial frame недоступен: HTTP ${response.status}.`, - ); - } - const payload = objectValue(await response.json(), "canonical_lab.spatial_frame"); - exact( - payload.schema_version, - "missioncore.canonical-recorded-lab-spatial-frame/v1", - "canonical_lab.spatial_frame.schema_version", - ); - exact(payload.target_time_ns, targetTimeNs, "canonical_lab.spatial_frame.target_time_ns"); - const pointList = (value: unknown, label: string) => arrayValue(value, label).map( - (entry, index) => { - const point = arrayValue(entry, `${label}[${index}]`).map( - (channel, channelIndex) => numberValue(channel, `${label}[${index}][${channelIndex}]`), - ); - if (point.length !== 3) { - throw new VegetationShadowContractError(`${label}[${index}]: размер изменён.`); - } - return [point[0]!, point[1]!, point[2]!] as const; - }, - ); - const sourcePoints = pointList( - payload.source_points_body_xyz_m, - "canonical_lab.spatial_frame.source_points", - ); - const localSlam = pointList( - payload.local_slam_body_xyz_m, - "canonical_lab.spatial_frame.local_slam", - ); - const sourcePointCount = integerValue( - payload.source_point_count, - "canonical_lab.spatial_frame.source_point_count", - ); - if (sourcePointCount !== sourcePoints.length || sourcePointCount > 100_000 || localSlam.length > 10_000) { - throw new VegetationShadowContractError("Canonical LAB spatial accounting изменён."); - } - const bodyFrame = objectValue(payload.body_frame, "canonical_lab.spatial_frame.body_frame"); - const origin = pointList( - [bodyFrame.origin_map_xyz_m], - "canonical_lab.spatial_frame.body_frame.origin", - )[0]!; - const basisRows = pointList( - bodyFrame.basis_map_from_body, - "canonical_lab.spatial_frame.body_frame.basis", - ); - if (basisRows.length !== 3) { - throw new VegetationShadowContractError("Canonical LAB spatial basis изменён."); - } - return { - targetTimeNs, - sourceTimeNs: integerValue(payload.source_time_ns, "canonical_lab.spatial_frame.source_time_ns"), - poseTimeNs: integerValue(payload.pose_time_ns, "canonical_lab.spatial_frame.pose_time_ns"), - trajectoryTimeNs: integerValue( - payload.trajectory_time_ns, - "canonical_lab.spatial_frame.trajectory_time_ns", - ), - sourcePointCount, - bodyFrame: { - originMapXyzM: origin, - basisMapFromBody: [basisRows[0]!, basisRows[1]!, basisRows[2]!], - }, - sourcePointsBodyXyzM: sourcePoints, - localSlamBodyXyzM: localSlam, - }; -} - export async function fetchVegetationShadowResult( resultId: string, { diff --git a/apps/control-station/src/workspaces/laboratory/VegetationShadowResult.tsx b/apps/control-station/src/workspaces/laboratory/VegetationShadowResult.tsx index 4b37f56..fb7454b 100644 --- a/apps/control-station/src/workspaces/laboratory/VegetationShadowResult.tsx +++ b/apps/control-station/src/workspaces/laboratory/VegetationShadowResult.tsx @@ -23,6 +23,7 @@ import { type LaboratoryMetricPackedCellEvidence, } from "../../components/laboratory/LaboratoryMetricEvidenceScene"; import { RecordedEvidenceVideoScene } from "../../components/laboratory/RecordedEvidenceVideoScene"; +import { useCanonicalRecordedLabSpatialFrame } from "../../components/laboratory/useCanonicalRecordedLabSpatialFrame"; import { useRecordedEvidencePlayback } from "../../components/laboratory/useRecordedEvidencePlayback"; import { LaboratoryEvidence, @@ -35,12 +36,14 @@ import { type RecordedEvidenceSemanticPaletteEntry, } from "../../components/laboratory/RecordedEvidenceSemanticMaskOverlay"; import { - fetchCanonicalRecordedLabSpatialFrame, + canonicalRecordedLabPackedTgsCells, + canonicalRecordedLabTgsIsCurrent, +} from "../../core/laboratory/canonicalRecordedLab"; +import { fetchVegetationShadowResult, fetchVegetationRouteTgsAnchor, vegetationFullRouteMaskUrl, vegetationVideoMaskUrl, - type CanonicalRecordedLabSpatialFrame, type VegetationFullRouteLayer, type VegetationFullRouteReview, type VegetationMixedRouteCase, @@ -105,9 +108,7 @@ function causalTgsCase( && (!latest || candidate.sourceSequence > latest.sourceSequence) ? candidate : latest - ), null) ?? cases.reduce((first, candidate) => ( - candidate.sourceSequence < first.sourceSequence ? candidate : first - )); + ), null); } function nearestFullRouteFrameIndex( @@ -130,92 +131,6 @@ function nearestFullRouteFrameIndex( : low; } -function useCanonicalRavSpatialFrame( - review: VegetationFullRouteReview, - replayLaunch: ObservationSessionReplayLaunch | null, - targetTimeNs: number, -) { - const [frame, setFrame] = useState(null); - const [error, setError] = useState(null); - const desiredRef = useRef(null); - const runningRef = useRef(false); - const mountedRef = useRef(true); - const cacheRef = useRef(new Map()); - const pumpRef = useRef<() => void>(() => undefined); - - pumpRef.current = () => { - if (runningRef.current || desiredRef.current === null || !replayLaunch) return; - runningRef.current = true; - let settledTimeNs: number | null = null; - void (async () => { - while (mountedRef.current && desiredRef.current !== null) { - const requestedTimeNs = desiredRef.current; - const cached = cacheRef.current.get(requestedTimeNs); - try { - const next = cached ?? await fetchCanonicalRecordedLabSpatialFrame( - review.sessionId, - replayLaunch.sha256, - requestedTimeNs, - ); - if (!cached) { - cacheRef.current.set(requestedTimeNs, next); - while (cacheRef.current.size > 12) { - const oldest = cacheRef.current.keys().next().value as number | undefined; - if (oldest === undefined) break; - cacheRef.current.delete(oldest); - } - } - if (!mountedRef.current) break; - setFrame(next); - setError(null); - } catch (caught: unknown) { - if (!mountedRef.current) break; - setError(caught instanceof Error ? caught.message : "Spatial-слои RAV004 недоступны."); - } - settledTimeNs = requestedTimeNs; - if (desiredRef.current === requestedTimeNs) break; - } - })().finally(() => { - runningRef.current = false; - if ( - mountedRef.current - && desiredRef.current !== null - && desiredRef.current !== settledTimeNs - ) { - pumpRef.current(); - } - }); - }; - - useEffect(() => { - mountedRef.current = true; - return () => { - mountedRef.current = false; - desiredRef.current = null; - }; - }, []); - - useEffect(() => { - cacheRef.current.clear(); - setFrame(null); - setError(null); - }, [replayLaunch?.sha256, review.sessionId]); - - useEffect(() => { - if (!replayLaunch) return; - desiredRef.current = targetTimeNs; - const cached = cacheRef.current.get(targetTimeNs); - if (cached) { - setFrame(cached); - setError(null); - return; - } - pumpRef.current(); - }, [replayLaunch, targetTimeNs]); - - return { frame, error, loading: Boolean(replayLaunch) && !frame && !error }; -} - function FullRouteReviewEvidence({ resultId, review, @@ -266,7 +181,11 @@ function FullRouteReviewEvidence({ const spatialRequestTimeNs = review.frameSourceTimesNs[spatialRequestIndex] ?? review.frameSourceTimesNs[sequenceIndex] ?? Math.round(playbackController.playback.currentSeconds * 1_000_000_000); - const spatialEvidence = useCanonicalRavSpatialFrame(review, replayLaunch, spatialRequestTimeNs); + const spatialEvidence = useCanonicalRecordedLabSpatialFrame({ + sessionId: review.sessionId, + generationSha256: replayLaunch?.sha256 ?? null, + targetTimeNs: spatialRequestTimeNs, + }); const layer = review[semanticLayer]; const semantic = useMemo(() => semanticPresentation(layer), [layer]); const prefetchSrcs = useMemo(() => showCameraSemantic @@ -343,11 +262,17 @@ function FullRouteReviewEvidence({ ? review.frameSourceTimesNs[selectedTgsCase.sourceSequence - 1] ?? Math.round(selectedTgsCase.sessionSeconds * 1_000_000_000) : spatialRequestTimeNs; - const tgsReferenceEvidence = useCanonicalRavSpatialFrame( - review, - replayLaunch, - selectedTgsTimeNs, + const currentFrameTimeNs = review.frameSourceTimesNs[sequenceIndex] + ?? Math.round(playbackController.playback.currentSeconds * 1_000_000_000); + const tgsWithinEvidenceWindow = Boolean( + selectedTgsCase + && canonicalRecordedLabTgsIsCurrent(currentFrameTimeNs, selectedTgsTimeNs), ); + const tgsReferenceEvidence = useCanonicalRecordedLabSpatialFrame({ + sessionId: review.sessionId, + generationSha256: replayLaunch?.sha256 ?? null, + targetTimeNs: selectedTgsTimeNs, + }); useEffect(() => { if (!showTgs || !selectedTgsCase) { @@ -376,53 +301,24 @@ function FullRouteReviewEvidence({ }, [review.linkedRouteReviewResultId, selectedTgsCase?.sourceSequence, showTgs]); const packedTgsCells = useMemo(() => { - if (!tgsAnchor) return undefined; + if (!tgsAnchor || !tgsWithinEvidenceWindow) return undefined; const currentBody = spatialEvidence.frame?.bodyFrame; const anchorBody = tgsReferenceEvidence.frame?.bodyFrame; - const transformPoint = (point: readonly [number, number, number]) => { - if (!currentBody || !anchorBody) return point; - const map = [0, 1, 2].map((row) => ( - anchorBody.originMapXyzM[row]! - + anchorBody.basisMapFromBody[row]!.reduce( - (sum, coefficient, column) => sum + coefficient * point[column]!, - 0, - ) - )); - const delta = map.map((value, index) => value - currentBody.originMapXyzM[index]!); - return [0, 1, 2].map((column) => ( - currentBody.basisMapFromBody.reduce( - (sum, row, rowIndex) => sum + row[column]! * delta[rowIndex]!, - 0, - ) - )) as [number, number, number]; - }; - const centers: number[] = []; - const zBounds: number[] = []; - tgsAnchor.costmap.centersXyM.forEach(([x, y], index) => { - const bounds = tgsAnchor.costmap.zBoundsM[index] ?? [null, null]; - const center = transformPoint([x, y, 0]); - centers.push(center[0], center[1]); - if (bounds[0] === null || bounds[1] === null) { - zBounds.push(Number.NaN, Number.NaN); - } else { - const bottom = transformPoint([x, y, bounds[0]]); - const top = transformPoint([x, y, bounds[1]]); - zBounds.push(Math.min(bottom[2], top[2]), Math.max(bottom[2], top[2])); - } - }); - return { - centersBodyXyM: Float32Array.from(centers), - zBoundsM: Float32Array.from(zBounds), - stateCodes: Uint8Array.from(tgsAnchor.costmap.stateCodes), - }; - }, [spatialEvidence.frame?.bodyFrame, tgsAnchor, tgsReferenceEvidence.frame?.bodyFrame]); + if (!currentBody || !anchorBody) return undefined; + return canonicalRecordedLabPackedTgsCells(tgsAnchor.costmap, anchorBody, currentBody); + }, [ + spatialEvidence.frame?.bodyFrame, + tgsAnchor, + tgsReferenceEvidence.frame?.bodyFrame, + tgsWithinEvidenceWindow, + ]); const semanticOverlay = showCameraSemantic ? { src: vegetationFullRouteMaskUrl(resultId, semanticLayer, sequenceIndex), prefetchSrcs, classes: semantic.classes, palette: semantic.palette, - opacity: 0.76, + opacity: 0.46, ariaLabel: `${layer.name} semantic prediction frame ${sequence}`, } : undefined; @@ -464,7 +360,11 @@ function FullRouteReviewEvidence({ ? spatialEvidence.frame.localSlamBodyXyzM : []} obstacles={[]} - rig={{ lengthM: 1, widthM: 0.8, nominalSensorHeightM: 0.4 }} + rig={{ + lengthM: 1, + widthM: 0.8, + nominalSensorHeightM: spatialEvidence.frame.sensorHeight.meters, + }} corridor={{ forwardLengthM: 12, rearMarginM: 1, halfWidthM: 0.4 }} occupiedVoxelSizeM={tgsAnchor?.costmap.cellSizeM ?? 0.45} mode={spatialMode} @@ -487,7 +387,9 @@ function FullRouteReviewEvidence({
{tgsAnchorError ?? linkedReviewError ?? (tgsAnchorLoading ? `Открываем sealed TGS anchor ${selectedTgsCase.sourceSequence}; source/SLAM и общий clock продолжаются.` - : `TGS anchor ${selectedTgsCase.sourceSequence} из 10; source/SLAM и общий clock продолжаются.`)} + : tgsWithinEvidenceWindow + ? `TGS anchor ${selectedTgsCase.sourceSequence} из 10; source/SLAM и общий clock продолжаются.` + : `TGS anchor ${selectedTgsCase.sourceSequence} старше доказанного окна 1 с; слой скрыт, playback продолжается.`)}
) : null} @@ -597,12 +499,12 @@ function FullRouteReviewEvidence({
Spatial evidence - {showTgs && selectedTgsCase + {showTgs && selectedTgsCase && tgsWithinEvidenceWindow ? `TGS anchor ${selectedTgsCase.sourceSequence} · ${selectedTgsCase.tgs.occupiedCells} occupied` - : "source RRD · points + SLAM"} + : "source RRD · points + bounded Local SLAM"} {showTgs - ? "latest causal of 10 sealed anchors · continuous playback retained" - : "causal 1 s view · grayscale intensity · recorded source identity"} + ? "TGS visible only inside sealed 1 s evidence window · playback retained" + : "5 s bounded Local SLAM · ground-rebased recorded source"}
); @@ -673,7 +575,7 @@ function FullRouteReviewResult({ summary={( { + server = await createServer({ + appType: "custom", + logLevel: "silent", + server: { middlewareMode: true }, + }); + ({ + canonicalMapGravityLocalPointToBodyGround, + canonicalRecordedLabPackedTgsCells, + canonicalRecordedLabTgsIsCurrent, + } = await server.ssrLoadModule("/src/core/laboratory/canonicalRecordedLab.ts")); +}); + +after(async () => { + await server?.close(); +}); + +const identity = [ + [1, 0, 0], + [0, 1, 0], + [0, 0, 1], +]; + +test("canonical TGS validity never retains a sparse anchor beyond its sealed history", () => { + assert.equal(canonicalRecordedLabTgsIsCurrent(2_000_000_000, 1_000_000_000), true); + assert.equal(canonicalRecordedLabTgsIsCurrent(2_000_000_001, 1_000_000_000), false); + assert.equal(canonicalRecordedLabTgsIsCurrent(999_999_999, 1_000_000_000), false); +}); + +test("map-gravity-local TGS uses sensor translation and current ground body exactly once", () => { + const anchor = { + originMapXyzM: [10, 20, 0.68], + sensorOriginMapXyzM: [10, 20, 1], + basisMapFromBody: identity, + }; + const current = { + originMapXyzM: [8, 20, 0], + sensorOriginMapXyzM: [8, 20, 0.32], + basisMapFromBody: identity, + }; + assert.deepEqual( + canonicalMapGravityLocalPointToBodyGround([1, 2, -1], anchor, current), + [3, 2, 0], + ); + const packed = canonicalRecordedLabPackedTgsCells({ + centersXyM: [[1, 2]], + stateCodes: [2], + zBoundsM: [[-1, 0]], + }, anchor, current); + assert.deepEqual([...packed.centersBodyXyM], [3, 2]); + assert.deepEqual([...packed.zBoundsM], [0, 1]); + assert.deepEqual([...packed.stateCodes], [2]); +}); + +test("recorded LAB spatial loading is shared, profile-bound and experiment-neutral", async () => { + const [contract, scheduler, vegetation] = await Promise.all([ + readFile(new URL("../src/core/laboratory/canonicalRecordedLabSpatial.ts", import.meta.url), "utf8"), + readFile(new URL("../src/components/laboratory/useCanonicalRecordedLabSpatialFrame.ts", import.meta.url), "utf8"), + readFile(new URL("../src/core/laboratory/vegetationShadow.ts", import.meta.url), "utf8"), + ]); + assert.match(contract, /CANONICAL_RECORDED_LAB_SPATIAL_PROFILE/); + assert.match(contract, /profile: CANONICAL_RECORDED_LAB_SPATIAL_PROFILE/); + assert.match(scheduler, /Shared latest-request-wins scheduler/); + assert.match(scheduler, /identityRef\.current !== requestIdentity/); + assert.doesNotMatch(scheduler, /RAVNOVES|vegetation|DDRNet/); + assert.doesNotMatch(vegetation, /fetchCanonicalRecordedLabSpatialFrame|CanonicalRecordedLabSpatialFrame/); +}); diff --git a/apps/control-station/test/vegetationShadow.test.mjs b/apps/control-station/test/vegetationShadow.test.mjs index ccde03a..e15bd2e 100644 --- a/apps/control-station/test/vegetationShadow.test.mjs +++ b/apps/control-station/test/vegetationShadow.test.mjs @@ -19,13 +19,15 @@ before(async () => { }); ({ fetchVegetationBenchmarkResult, - fetchCanonicalRecordedLabSpatialFrame, fetchVegetationShadowResult, fetchVegetationRouteTgsAnchor, vegetationFullRouteMaskUrl, } = await server.ssrLoadModule( "/src/core/laboratory/vegetationShadow.ts", )); + ({ fetchCanonicalRecordedLabSpatialFrame } = await server.ssrLoadModule( + "/src/core/laboratory/canonicalRecordedLabSpatial.ts", + )); }); after(async () => { @@ -405,16 +407,35 @@ test("canonical recorded LAB spatial frame keeps source, SLAM and body identity fetcher: async (url) => { requestedUrl = String(url); return new Response(JSON.stringify({ - schema_version: "missioncore.canonical-recorded-lab-spatial-frame/v1", + schema_version: "missioncore.canonical-recorded-lab-spatial-frame/v2", target_time_ns: 82_770_000_000, source_time_ns: 82_769_535_708, pose_time_ns: 82_769_535_708, trajectory_time_ns: 82_700_000_000, + coordinate_frame: "body-ground", + sensor_height: { + meters: 0.32, + source: "initial-source-cloud-lower-quantile-median", + sample_count: 20, + mad_m: 0.03, + authority: "visual-derived", + }, + spatial_profile: { + profile_id: "source-paced-ground-v2", + local_slam_history_seconds: 5, + local_slam_radius_m: 30, + local_slam_voxel_size_m: 0.12, + local_slam_point_limit: 27000, + }, source_point_count: 2, source_points_body_xyz_m: [[1, 2, 3], [4, 5, 6]], + local_slam_source_frame_count: 2, + local_slam_source_point_count: 4, + local_slam_point_count: 2, local_slam_body_xyz_m: [[0, 0, 0], [1, 0, 0]], body_frame: { origin_map_xyz_m: [33, 4, 1], + sensor_origin_map_xyz_m: [33, 4, 1.32], basis_map_from_body: [[1, 0, 0], [0, 1, 0], [0, 0, 1]], }, }), { status: 200, headers: { "Content-Type": "application/json" } }); @@ -422,10 +443,11 @@ test("canonical recorded LAB spatial frame keeps source, SLAM and body identity }); assert.equal( requestedUrl, - `/api/v1/observation-sessions/session-004/canonical-lab/spatial-frame?generation=${generation}&time_ns=82770000000`, + `/api/v1/observation-sessions/session-004/canonical-lab/spatial-frame?generation=${generation}&time_ns=82770000000&profile=source-paced-ground-v2`, ); assert.equal(frame.sourcePointCount, 2); assert.equal(frame.localSlamBodyXyzM.length, 2); + assert.equal(frame.sensorHeight.meters, 0.32); assert.deepEqual(frame.bodyFrame.originMapXyzM, [33, 4, 1]); }); @@ -461,11 +483,13 @@ test("vegetation realtime LAB and archival benchmark use separate admitted instr assert.match(resultSource, /RecordedEvidenceVideoScene/); assert.match(resultSource, /LaboratoryMetricEvidenceScene/); assert.doesNotMatch(resultSource, /RerunViewport/); - assert.match(resultSource, /fetchCanonicalRecordedLabSpatialFrame/); + assert.match(resultSource, /useCanonicalRecordedLabSpatialFrame/); + assert.doesNotMatch(resultSource, /cacheRef|pumpRef|desiredRef/); assert.match(resultSource, /useCanonicalRecordedLabReplayState/); assert.match(resultSource, /playbackTransport="epoch-stream"/); assert.match(resultSource, /causalTgsCase/); - assert.match(resultSource, /latest causal of 10 sealed anchors/); + assert.match(resultSource, /TGS visible only inside sealed 1 s evidence window/); + assert.match(resultSource, /canonicalRecordedLabPackedTgsCells/); assert.doesNotMatch(resultSource, /LaboratoryRecordedClipPlayer|M48EvidenceModeRail/); assert.doesNotMatch(resultSource, /assets\.tgs| Any | None: @@ -159,7 +175,17 @@ def _load_index_cached( ) if not points.times_ns or not poses.times_ns or not trajectories.times_ns: raise ValueError("Recorded LAB source has no canonical spatial layers") - return _CanonicalSpatialIndex(points=points, poses=poses, trajectories=trajectories) + sensor_height_m, sensor_height_sample_count, sensor_height_mad_m = ( + _estimate_sensor_height(points, poses) + ) + return _CanonicalSpatialIndex( + points=points, + poses=poses, + trajectories=trajectories, + sensor_height_m=sensor_height_m, + sensor_height_sample_count=sensor_height_sample_count, + sensor_height_mad_m=sensor_height_mad_m, + ) def _load_index( @@ -207,12 +233,108 @@ def _map_points_to_body( return body.astype(np.float32) +def _estimate_sensor_height(points: _TimedPoints, poses: _TimedPoses) -> tuple[float, int, float]: + """Estimate one session mount height from the initial qualified cloud. + + The K1 recording has no explicit physical mount-height entity. The initial + stationary minute is therefore the only admissible automatic calibration + source. A low near-field quantile is measured per source increment and the + session median rejects vegetation/ravine outliers. The result stays + diagnostic and is never promoted to navigation authority by this adapter. + """ + + first_time_ns = points.times_ns[0] + calibration_end_ns = first_time_ns + round(_HEIGHT_CALIBRATION_SECONDS * 1_000_000_000) + candidates = [ + index + for index, timestamp in enumerate(points.times_ns) + if timestamp <= calibration_end_ns + ][:_HEIGHT_CALIBRATION_MAX_FRAMES] + estimates: list[float] = [] + for point_index in candidates: + pose_index = _latest_index(poses.times_ns, points.times_ns[point_index]) + body = _map_points_to_body( + points.values[point_index], + poses.translations[pose_index], + poses.quaternions_xyzw[pose_index], + ) + radius = np.linalg.norm(body[:, :2], axis=1) + eligible = body[ + (radius >= _HEIGHT_NEAR_MIN_RADIUS_M) + & (radius <= _HEIGHT_NEAR_MAX_RADIUS_M) + & (body[:, 2] >= -2.0) + & (body[:, 2] <= 0.5) + ] + if eligible.shape[0] < 100: + continue + estimate = -float(np.quantile(eligible[:, 2], _HEIGHT_LOWER_QUANTILE)) + if 0.08 <= estimate <= 2.5: + estimates.append(estimate) + if len(estimates) < 8: + raise ValueError("Recorded LAB sensor height cannot be estimated from source cloud") + values = np.asarray(estimates, dtype=np.float64) + height = float(np.median(values)) + mad = float(np.median(np.abs(values - height))) + return height, len(estimates), mad + + +def _ground_origin_map( + sensor_origin_map: np.ndarray, + basis_map_from_body: np.ndarray, + sensor_height_m: float, +) -> np.ndarray: + return sensor_origin_map - basis_map_from_body[:, 2] * sensor_height_m + + +def _map_points_to_ground_body( + points_map: np.ndarray, + ground_origin_map: np.ndarray, + basis_map_from_body: np.ndarray, +) -> np.ndarray: + body = (points_map.astype(np.float64) - ground_origin_map) @ basis_map_from_body + return body.astype(np.float32) + + +def _bounded_local_slam( + points: _TimedPoints, + target_time_ns: int, + ground_origin_map: np.ndarray, + basis_map_from_body: np.ndarray, +) -> tuple[np.ndarray, int, int]: + start_ns = target_time_ns - round(_LOCAL_SLAM_HISTORY_SECONDS * 1_000_000_000) + first = bisect_right(points.times_ns, start_ns - 1) + last = bisect_right(points.times_ns, target_time_ns) + selected = points.values[first:last] + if not selected: + return np.empty((0, 3), dtype=np.float32), 0, 0 + source_count = sum(int(value.shape[0]) for value in selected) + local = _map_points_to_ground_body( + np.concatenate(selected, axis=0), + ground_origin_map, + basis_map_from_body, + ) + mask = ( + (np.linalg.norm(local[:, :2], axis=1) <= _LOCAL_SLAM_RADIUS_M) + & (np.abs(local[:, 2]) <= _LOCAL_SLAM_VERTICAL_LIMIT_M) + ) + local = local[mask] + if local.shape[0] == 0: + return local, len(selected), source_count + voxel = np.floor(local / _LOCAL_SLAM_VOXEL_SIZE_M).astype(np.int32) + _, retained = np.unique(voxel, axis=0, return_index=True) + local = local[np.sort(retained)] + if local.shape[0] > _LOCAL_SLAM_POINT_LIMIT: + stride = int(np.ceil(local.shape[0] / _LOCAL_SLAM_POINT_LIMIT)) + local = local[::stride][:_LOCAL_SLAM_POINT_LIMIT] + return np.ascontiguousarray(local, dtype=np.float32), len(selected), source_count + + def canonical_lab_spatial_frame( recording_path: Path, generation_sha256: str, target_time_ns: int, ) -> dict[str, object]: - """Return the latest sealed source cloud and SLAM route on one host time.""" + """Return the current source cloud and bounded Local SLAM on one host time.""" if target_time_ns < 0: raise ValueError("Recorded LAB target time is invalid") @@ -229,31 +351,52 @@ def canonical_lab_spatial_frame( translation = index.poses.translations[pose_index] quaternion = index.poses.quaternions_xyzw[pose_index] basis_map_from_body = _rotation_map_from_body(quaternion) - points_body = _map_points_to_body(index.points.values[point_index], translation, quaternion) - trajectory_body = _map_points_to_body( - index.trajectories.values[trajectory_index], + ground_origin = _ground_origin_map( translation, - quaternion, + basis_map_from_body, + index.sensor_height_m, ) - # The canonical local-SLAM layer is bounded around the vehicle. It must - # never turn into the full world-route "blob" seen in the raw Rerun view. - local_mask = ( - (np.abs(trajectory_body[:, 0]) <= 30.0) - & (np.abs(trajectory_body[:, 1]) <= 30.0) - & (np.abs(trajectory_body[:, 2]) <= 6.0) + points_body = _map_points_to_ground_body( + index.points.values[point_index], + ground_origin, + basis_map_from_body, + ) + local_slam, local_slam_source_frames, local_slam_source_points = _bounded_local_slam( + index.points, + index.points.times_ns[point_index], + ground_origin, + basis_map_from_body, ) - local_trajectory = trajectory_body[local_mask] return { - "schema_version": "missioncore.canonical-recorded-lab-spatial-frame/v1", + "schema_version": "missioncore.canonical-recorded-lab-spatial-frame/v2", "target_time_ns": target_time_ns, "source_time_ns": index.points.times_ns[point_index], "pose_time_ns": index.poses.times_ns[pose_index], "trajectory_time_ns": index.trajectories.times_ns[trajectory_index], + "coordinate_frame": "body-ground", + "sensor_height": { + "meters": index.sensor_height_m, + "source": "initial-source-cloud-lower-quantile-median", + "sample_count": index.sensor_height_sample_count, + "mad_m": index.sensor_height_mad_m, + "authority": "visual-derived", + }, + "spatial_profile": { + "profile_id": CANONICAL_LAB_SPATIAL_PROFILE, + "local_slam_history_seconds": _LOCAL_SLAM_HISTORY_SECONDS, + "local_slam_radius_m": _LOCAL_SLAM_RADIUS_M, + "local_slam_voxel_size_m": _LOCAL_SLAM_VOXEL_SIZE_M, + "local_slam_point_limit": _LOCAL_SLAM_POINT_LIMIT, + }, "body_frame": { - "origin_map_xyz_m": translation.tolist(), + "origin_map_xyz_m": ground_origin.tolist(), + "sensor_origin_map_xyz_m": translation.tolist(), "basis_map_from_body": basis_map_from_body.tolist(), }, "source_point_count": int(points_body.shape[0]), "source_points_body_xyz_m": points_body.tolist(), - "local_slam_body_xyz_m": local_trajectory.tolist(), + "local_slam_source_frame_count": local_slam_source_frames, + "local_slam_source_point_count": local_slam_source_points, + "local_slam_point_count": int(local_slam.shape[0]), + "local_slam_body_xyz_m": local_slam.tolist(), } diff --git a/src/k1link/web/session_api.py b/src/k1link/web/session_api.py index e6837da..e84f1e0 100644 --- a/src/k1link/web/session_api.py +++ b/src/k1link/web/session_api.py @@ -37,7 +37,10 @@ from k1link.sessions import ( SessionStore, validate_recorded_media_timeline, ) -from k1link.sessions.canonical_lab_spatial import canonical_lab_spatial_frame +from k1link.sessions.canonical_lab_spatial import ( + CANONICAL_LAB_SPATIAL_PROFILE, + canonical_lab_spatial_frame, +) from k1link.sessions.plugin_contract import RecordedPointColorRenderer from k1link.viewer.recorded import ( APPLICATION_ID as RECORDED_APPLICATION_ID, @@ -832,6 +835,7 @@ def build_session_router( session_id: str, generation: Annotated[str, Query(min_length=64, max_length=64)], time_ns: Annotated[int, Query(ge=0, le=MAX_SAFE_INTEGER)], + profile: Literal["source-paced-ground-v2"], ) -> JSONResponse: """Serve one body-frame sample for the canonical recorded-LAB clock. @@ -890,7 +894,10 @@ def build_session_router( payload, headers={ "Cache-Control": "private, max-age=31536000, immutable", - "ETag": f'"{generation}:{payload["source_time_ns"]}"', + "ETag": ( + f'"{generation}:{CANONICAL_LAB_SPATIAL_PROFILE}:' + f'{payload["source_time_ns"]}"' + ), "X-Content-Type-Options": "nosniff", }, ) diff --git a/tests/test_canonical_lab_spatial.py b/tests/test_canonical_lab_spatial.py new file mode 100644 index 0000000..5ea0375 --- /dev/null +++ b/tests/test_canonical_lab_spatial.py @@ -0,0 +1,71 @@ +from __future__ import annotations + +import numpy as np +import pytest + +from k1link.sessions.canonical_lab_spatial import ( + _TimedPoints, + _TimedPoses, + _bounded_local_slam, + _estimate_sensor_height, + _ground_origin_map, +) + + +def _calibration_cloud(height_m: float, seed: int) -> np.ndarray: + rng = np.random.default_rng(seed) + xy = rng.uniform(-5.5, 5.5, size=(500, 2)).astype(np.float32) + radius = np.linalg.norm(xy, axis=1) + xy = xy[(radius >= 1.0) & (radius <= 5.5)][:360] + ground = np.column_stack(( + xy, + rng.normal(-height_m, 0.006, size=xy.shape[0]), + )).astype(np.float32) + vegetation = np.column_stack(( + rng.uniform(-5, 5, size=(300, 2)), + rng.uniform(0.0, 1.2, size=300), + )).astype(np.float32) + return np.concatenate((ground, vegetation), axis=0) + + +def test_session_sensor_height_is_derived_from_initial_source_cloud() -> None: + times = tuple(index * 500_000_000 for index in range(12)) + points = _TimedPoints( + times_ns=times, + values=tuple(_calibration_cloud(0.32, index) for index in range(12)), + ) + poses = _TimedPoses( + times_ns=times, + translations=tuple(np.zeros(3) for _ in times), + quaternions_xyzw=tuple(np.asarray([0.0, 0.0, 0.0, 1.0]) for _ in times), + ) + + height, sample_count, mad = _estimate_sensor_height(points, poses) + + assert height == pytest.approx(0.32, abs=0.02) + assert sample_count == 12 + assert mad < 0.02 + + +def test_local_slam_accumulates_source_increments_in_ground_body_frame() -> None: + points = _TimedPoints( + times_ns=(0, 1_000_000_000, 2_000_000_000), + values=( + np.asarray([[1.0, 0.0, -0.32]], dtype=np.float32), + np.asarray([[2.0, 0.0, -0.32]], dtype=np.float32), + np.asarray([[3.0, 0.0, -0.32]], dtype=np.float32), + ), + ) + basis = np.eye(3) + ground_origin = _ground_origin_map(np.asarray([0.0, 0.0, 0.0]), basis, 0.32) + + local, frame_count, source_count = _bounded_local_slam( + points, + 2_000_000_000, + ground_origin, + basis, + ) + + assert frame_count == 3 + assert source_count == 3 + assert local[:, 2].tolist() == pytest.approx([0.0, 0.0, 0.0], abs=1e-6) diff --git a/tests/test_session_api.py b/tests/test_session_api.py index 740dc42..a4300a1 100644 --- a/tests/test_session_api.py +++ b/tests/test_session_api.py @@ -489,17 +489,36 @@ def test_canonical_lab_spatial_frame_uses_ready_immutable_recording( assert resolved is not None and resolved.recording is not None generation = hashlib.sha256(payload).hexdigest() expected = { - "schema_version": "missioncore.canonical-recorded-lab-spatial-frame/v1", + "schema_version": "missioncore.canonical-recorded-lab-spatial-frame/v2", "target_time_ns": 500_000_000, "source_time_ns": 499_000_000, "pose_time_ns": 499_000_000, "trajectory_time_ns": 490_000_000, + "coordinate_frame": "body-ground", + "sensor_height": { + "meters": 0.32, + "source": "initial-source-cloud-lower-quantile-median", + "sample_count": 20, + "mad_m": 0.03, + "authority": "visual-derived", + }, + "spatial_profile": { + "profile_id": "source-paced-ground-v2", + "local_slam_history_seconds": 5.0, + "local_slam_radius_m": 30.0, + "local_slam_voxel_size_m": 0.12, + "local_slam_point_limit": 27000, + }, "body_frame": { "origin_map_xyz_m": [0.0, 0.0, 0.0], + "sensor_origin_map_xyz_m": [0.0, 0.0, 0.32], "basis_map_from_body": [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]], }, "source_point_count": 1, "source_points_body_xyz_m": [[1.0, 2.0, 3.0]], + "local_slam_source_frame_count": 1, + "local_slam_source_point_count": 1, + "local_slam_point_count": 1, "local_slam_body_xyz_m": [[0.0, 0.0, 0.0]], } @@ -525,9 +544,12 @@ def test_canonical_lab_spatial_frame_uses_ready_immutable_recording( session_id=session.name, generation=generation, time_ns=500_000_000, + profile="source-paced-ground-v2", )) assert json.loads(response.body) == expected - assert response.headers["etag"] == f'"{generation}:499000000"' + assert response.headers["etag"] == ( + f'"{generation}:source-paced-ground-v2:499000000"' + ) assert response.headers["cache-control"].endswith("immutable") finally: manager.close()