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

feat(lab): add bounded local SLAM surface

Browse Source
This commit is contained in:
DCCONSTRUCTIONS
2026-08-06 09:41:25 +03:00
parent b8fb4ebcba
commit b7a51e26e6
10 changed files with 369 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/control-station/src/core/laboratory/m4LocalSurface.ts
+131
View File
@@ -0,0 +1,131 @@
import type {
M4Matrix3,
M4Point3,
M4ThreatTimelineFrame,
} from "./m4ReplayThreat";
export interface M4LocalSurfaceProfile {
windowSeconds: number;
voxelSizeM: number;
radiusM: number;
pointLimit: number;
}
export interface M4LocalSurface {
pointsBodyXyzM: readonly M4Point3[];
sourceFrameCount: number;
sourcePointCount: number;
voxelCount: number;
}
function bodyPointToMap(
point: M4Point3,
origin: M4Point3,
basis: M4Matrix3,
): M4Point3 {
return [
origin[0] + point[0] * basis[0][0] + point[1] * basis[0][1] + point[2] * basis[0][2],
origin[1] + point[0] * basis[1][0] + point[1] * basis[1][1] + point[2] * basis[1][2],
origin[2] + point[0] * basis[2][0] + point[1] * basis[2][1] + point[2] * basis[2][2],
];
}
function mapPointToBody(
point: M4Point3,
origin: M4Point3,
basis: M4Matrix3,
): M4Point3 {
const delta: M4Point3 = [
point[0] - origin[0],
point[1] - origin[1],
point[2] - origin[2],
];
return [
delta[0] * basis[0][0] + delta[1] * basis[1][0] + delta[2] * basis[2][0],
delta[0] * basis[0][1] + delta[1] * basis[1][1] + delta[2] * basis[2][1],
delta[0] * basis[0][2] + delta[1] * basis[1][2] + delta[2] * basis[2][2],
];
}
function emptySurface(): M4LocalSurface {
return {
pointsBodyXyzM: [],
sourceFrameCount: 0,
sourcePointCount: 0,
voxelCount: 0,
};
}
export function buildM4LocalSurface(
availableFrames: readonly M4ThreatTimelineFrame[],
activeFrame: M4ThreatTimelineFrame | null,
profile: M4LocalSurfaceProfile,
): M4LocalSurface {
const activeBody = activeFrame?.bodyFrame;
if (
!activeFrame
|| !activeBody
|| profile.windowSeconds <= 0
|| profile.voxelSizeM <= 0
|| profile.radiusM <= 0
|| profile.pointLimit < 1
) {
return emptySurface();
}
const startTimeNs = activeFrame.sourceTimeNs - profile.windowSeconds * 1_000_000_000;
const frames = availableFrames
.filter((frame) => (
frame.bodyFrame
&& frame.sourceTimeNs >= startTimeNs
&& frame.sourceTimeNs <= activeFrame.sourceTimeNs
))
.sort((left, right) => left.sequence - right.sequence);
if (!frames.length) return emptySurface();
const radiusSquared = profile.radiusM * profile.radiusM;
const voxels = new Map<string, M4Point3>();
let sourcePointCount = 0;
for (const frame of frames) {
const sourceBody = frame.bodyFrame;
if (!sourceBody) continue;
sourcePointCount += frame.pointCloudBodyXyzM.length;
for (const sourcePoint of frame.pointCloudBodyXyzM) {
const mapPoint = bodyPointToMap(
sourcePoint,
sourceBody.originMapXyzM,
sourceBody.basisMapFromBody,
);
const activePoint = mapPointToBody(
mapPoint,
activeBody.originMapXyzM,
activeBody.basisMapFromBody,
);
if (
activePoint[0] * activePoint[0]
+ activePoint[1] * activePoint[1]
+ activePoint[2] * activePoint[2]
> radiusSquared
) {
continue;
}
const key = [
Math.floor(mapPoint[0] / profile.voxelSizeM),
Math.floor(mapPoint[1] / profile.voxelSizeM),
Math.floor(mapPoint[2] / profile.voxelSizeM),
].join(":");
if (!voxels.has(key)) voxels.set(key, activePoint);
}
}
const retained = [...voxels.values()];
const stride = Math.max(1, Math.ceil(retained.length / profile.pointLimit));
return {
pointsBodyXyzM: retained
.filter((_, index) => index % stride === 0)
.slice(0, profile.pointLimit),
sourceFrameCount: frames.length,
sourcePointCount,
voxelCount: voxels.size,
};
}
apps/control-station/src/core/laboratory/m4ReplayThreat.ts
+63 -1
View File
@@ -1,6 +1,7 @@
export type M4ThreatDecision = "threat" | "not-threat" | "unknown";
export type M4ThreatMotion = "moving" | "stationary" | "unknown";
export type M4Point3 = readonly [number, number, number];
export type M4Matrix3 = readonly [M4Point3, M4Point3, M4Point3];
export interface M4ThreatReplayResult {
resultId: string;
@@ -132,6 +133,10 @@ export interface M4ThreatTimelineFrame {
sessionSeconds: number;
sourceAvailable: boolean;
spatialAvailable: boolean;
bodyFrame: {
originMapXyzM: M4Point3;
basisMapFromBody: M4Matrix3;
} | null;
pointCloudBodyXyzM: readonly M4Point3[];
pointCloudSourceCount: number;
pointCloudSampleCount: number;
@@ -159,6 +164,14 @@ export interface M4ThreatTimeline {
maximumSourcePointsPerFrame: number;
pointDelivery: "exact-current-increment";
sourceRepresentationId: "registered-map-increment-v1";
localSurfaceVisualization: {
derivation: "bounded-registered-increment-accumulation";
windowSeconds: number;
voxelSizeM: number;
radiusM: number;
pointLimit: number;
authority: "visual-derived";
};
occupiedVoxelSizeM: number;
rig: M4ThreatVisualFrame["rig"];
corridor: M4ThreatVisualFrame["corridor"];
@@ -219,6 +232,10 @@ const vector = (value: unknown, size: number, label: string): number[] => {
if (parsed.length !== size) throw new M4ThreatContractError(`${label}: неверная размерность.`);
return parsed;
};
const point3 = (value: unknown, label: string): M4Point3 => {
const parsed = vector(value, 3, label);
return [parsed[0]!, parsed[1]!, parsed[2]!];
};
const decision = (value: unknown, label: string): M4ThreatDecision => {
if (value !== "threat" && value !== "not-threat" && value !== "unknown") {
throw new M4ThreatContractError(`${label}: неизвестное решение.`);
@@ -539,6 +556,10 @@ export async function fetchM4ThreatTimeline(
}
const rig = object(payload.rig, "M4.6 timeline rig");
const corridor = object(payload.corridor, "M4.6 timeline corridor");
const localSurface = object(
payload.local_surface_visualization,
"M4.6 local surface profile",
);
return {
resultId: result,
recordedSourceSessionId: "20260720T065719Z_viewer_live",
@@ -565,6 +586,22 @@ export async function fetchM4ThreatTimeline(
"M4.6 point delivery",
),
sourceRepresentationId: "registered-map-increment-v1",
localSurfaceVisualization: {
derivation: exact(
localSurface.derivation,
"bounded-registered-increment-accumulation",
"M4.6 local surface derivation",
),
windowSeconds: number(localSurface.window_seconds, "M4.6 local surface window"),
voxelSizeM: number(localSurface.voxel_size_m, "M4.6 local surface voxel"),
radiusM: number(localSurface.radius_m, "M4.6 local surface radius"),
pointLimit: integer(localSurface.point_limit, "M4.6 local surface point limit"),
authority: exact(
localSurface.authority,
"visual-derived",
"M4.6 local surface authority",
),
},
occupiedVoxelSizeM: number(
corridor.occupied_voxel_size_m,
"M4.6 occupied voxel size",
@@ -647,6 +684,22 @@ function parseTimelineFrame(
throw new M4ThreatContractError("M4.6 timeline frame order: нарушен контракт.");
}
const counts = object(item.decision_counts, "M4.6 timeline decisions");
const spatialAvailable = typeof item.spatial_available === "boolean"
&& item.spatial_available;
const bodyFrame = item.body_frame === null
? null
: object(item.body_frame, "M4.6 timeline body frame");
if (spatialAvailable !== (bodyFrame !== null)) {
throw new M4ThreatContractError("M4.6 timeline body frame: нарушена доступность.");
}
const basis = bodyFrame === null
? null
: array(bodyFrame.basis_map_from_body, "M4.6 timeline body basis").map(
(row) => point3(row, "M4.6 timeline body basis row"),
);
if (basis !== null && basis.length !== 3) {
throw new M4ThreatContractError("M4.6 timeline body basis: нарушен размер.");
}
const cameraUrl = text(item.camera_url, "M4.6 timeline camera URL");
if (!cameraUrl.includes(`/results/${result}/timeline/frames/${sequence}/camera`)) {
throw new M4ThreatContractError("M4.6 timeline camera URL: нарушена идентичность.");
@@ -657,7 +710,16 @@ function parseTimelineFrame(
sourceTimeNs: integer(item.source_time_ns, "M4.6 timeline source time"),
sessionSeconds: number(item.session_seconds, "M4.6 timeline time"),
sourceAvailable: typeof item.source_available === "boolean" && item.source_available,
spatialAvailable: typeof item.spatial_available === "boolean" && item.spatial_available,
spatialAvailable,
bodyFrame: bodyFrame === null || basis === null
? null
: {
originMapXyzM: point3(
bodyFrame.origin_map_xyz_m,
"M4.6 timeline body origin",
),
basisMapFromBody: [basis[0]!, basis[1]!, basis[2]!],
},
pointCloudBodyXyzM: array(item.point_cloud_body_xyz_m, "M4.6 timeline points").map(
(point) => vector(point, 3, "M4.6 timeline point") as [number, number, number],
),