refactor(lab): canonicalize recorded spatial replay
This commit is contained in:
DCCONSTRUCTIONS
@@ -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),
|
||||
};
|
||||
}
|
||||
@@ -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<string, unknown> {
|
||||
if (!value || typeof value !== "object" || Array.isArray(value)) {
|
||||
throw new CanonicalRecordedLabSpatialContractError(`${label}: ожидался объект.`);
|
||||
}
|
||||
return value as Record<string, unknown>;
|
||||
}
|
||||
|
||||
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<CanonicalRecordedLabSpatialFrame> {
|
||||
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,
|
||||
};
|
||||
}
|
||||
@@ -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<CanonicalRecordedLabSpatialFrame> {
|
||||
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,
|
||||
{
|
||||
|
||||
Reference in New Issue
Block a user