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/components/laboratory/LaboratoryMetricEvidenceScene.tsx
+26
View File
@@ -104,6 +104,7 @@ export const LaboratoryMetricEvidenceScene = forwardRef<
LaboratoryMetricEvidenceSceneHandle,
{
pointCloudBodyXyzM: readonly LaboratoryMetricPoint3[];
localSurfaceBodyXyzM: readonly LaboratoryMetricPoint3[];
obstacles: readonly LaboratoryMetricObstacleVisual[];
rig: LaboratoryMetricRigVisual;
corridor: LaboratoryMetricCorridorVisual;
@@ -111,10 +112,12 @@ LaboratoryMetricEvidenceSceneHandle,
mode: LaboratoryMetricSceneMode;
label: string;
showCurrentIncrement: boolean;
showLocalSurface: boolean;
showRollingMap: boolean;
}
>(function LaboratoryMetricEvidenceScene({
pointCloudBodyXyzM,
localSurfaceBodyXyzM,
obstacles,
rig,
corridor,
@@ -122,6 +125,7 @@ LaboratoryMetricEvidenceSceneHandle,
mode,
label,
showCurrentIncrement,
showLocalSurface,
showRollingMap,
}, ref) {
const hostRef = useRef<HTMLDivElement | null>(null);
@@ -211,6 +215,25 @@ LaboratoryMetricEvidenceSceneHandle,
if (!host || !content) return;
clearGroup(content);
if (showLocalSurface) {
const localSurfaceGeometry = new THREE.BufferGeometry();
localSurfaceGeometry.setAttribute(
"position",
new THREE.BufferAttribute(positions(localSurfaceBodyXyzM), 3),
);
content.add(new THREE.Points(
localSurfaceGeometry,
new THREE.PointsMaterial({
color: tokenColor(host, "--nodedc-accent-rgb", [247, 248, 244]),
size: 1.3,
sizeAttenuation: false,
transparent: true,
opacity: 0.42,
depthWrite: false,
}),
));
}
if (showCurrentIncrement) {
const contextGeometry = new THREE.BufferGeometry();
contextGeometry.setAttribute(
@@ -296,8 +319,10 @@ LaboratoryMetricEvidenceSceneHandle,
}, [
obstacles,
occupiedVoxelSizeM,
localSurfaceBodyXyzM,
pointCloudBodyXyzM,
showCurrentIncrement,
showLocalSurface,
showRollingMap,
]);
@@ -396,6 +421,7 @@ LaboratoryMetricEvidenceSceneHandle,
<span data-decision="not-threat">Вне коридора</span>
<span data-decision="unknown">Неизвестно</span>
<span data-decision="context">Current increment</span>
<span data-decision="local-surface">Local SLAM surface</span>
<span data-decision="rolling">Rolling-map occupied</span>
</div>
</div>
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],
),
apps/control-station/src/styles/m4-replay-threat.css
+9
View File
@@ -66,6 +66,10 @@
margin-left: auto;
}
.m4-replay-threat-visual__layer-controls .nodedc-segmented__item {
padding-inline: 0.72rem;
}
.m4-replay-threat-evidence-viewer .laboratory-evidence-viewer__transport {
bottom: 0.3rem;
}
@@ -155,3 +159,8 @@
border: 1px solid rgb(var(--nodedc-accent-rgb));
background: transparent;
}
.laboratory-metric-evidence-scene__legend span[data-decision="local-surface"]::before {
background: rgb(var(--nodedc-accent-rgb));
opacity: 0.72;
}
apps/control-station/src/workspaces/laboratory/M4ReplayThreatVisual.tsx
+28 -4
View File
@@ -18,6 +18,7 @@ import type {
M4ThreatCameraProposal,
M4ThreatTimelineFrame,
} from "../../core/laboratory/m4ReplayThreat";
import { buildM4LocalSurface } from "../../core/laboratory/m4LocalSurface";
import { recordedObservationSources } from "../../core/observation/recordedObservationSources";
import { replayObservationSession } from "../../core/observation/sessionArchive";
import type { ObservationSourceDescriptor } from "../../core/runtime/contracts";
@@ -68,6 +69,7 @@ export function M4ReplayThreatVisual({ resultId }: { resultId: string }) {
const [mode, setMode] = useState<M4ThreatViewMode>("video");
const [spatialMode, setSpatialMode] = useState<LaboratoryMetricSceneMode>("3d");
const [showCurrentIncrement, setShowCurrentIncrement] = useState(true);
const [showLocalSurface, setShowLocalSurface] = useState(true);
const [showRollingMap, setShowRollingMap] = useState(true);
const [expanded, setExpanded] = useState(false);
const metricSceneRef = useRef<LaboratoryMetricEvidenceSceneHandle | null>(null);
@@ -161,6 +163,16 @@ export function M4ReplayThreatVisual({ resultId }: { resultId: string }) {
.map((item) => item.assessment.closestApproachM)
.filter((value): value is number => value !== null)
.sort((left, right) => left - right)[0] ?? null;
const localSurface = useMemo(() => buildM4LocalSurface(
timelineFrame.availableFrames,
frame,
metadata.timeline?.localSurfaceVisualization ?? {
windowSeconds: 2,
voxelSizeM: 0.1,
radiusM: 12,
pointLimit: 20_000,
},
), [frame, metadata.timeline, timelineFrame.availableFrames]);
const seek = (seconds: number) => playbackController.seek(seconds);
const handleModeChange = (next: M4ThreatViewMode) => {
@@ -206,7 +218,17 @@ export function M4ReplayThreatVisual({ resultId }: { resultId: string }) {
aria-pressed={showCurrentIncrement}
onClick={() => setShowCurrentIncrement((visible) => !visible)}
>
CURRENT INCREMENT
CURRENT
</button>
<button
type="button"
className="nodedc-segmented__item"
data-active={showLocalSurface ? "true" : undefined}
aria-pressed={showLocalSurface}
title="Bounded local SLAM surface · visual-derived"
onClick={() => setShowLocalSurface((visible) => !visible)}
>
LOCAL SLAM
</button>
<button
type="button"
@@ -215,7 +237,7 @@ export function M4ReplayThreatVisual({ resultId }: { resultId: string }) {
aria-pressed={showRollingMap}
onClick={() => setShowRollingMap((visible) => !visible)}
>
ROLLING MAP
ROLLING
</button>
</div>
) : null}
@@ -250,7 +272,7 @@ export function M4ReplayThreatVisual({ resultId }: { resultId: string }) {
</strong>
<small>
{frame.spatialAvailable
? `${frame.pointCloudSampleCount}/${frame.pointCloudSourceCount} exact lio_pcl increment`
? `${frame.pointCloudSampleCount}/${frame.pointCloudSourceCount} exact · ${localSurface.pointsBodyXyzM.length} local SLAM / ${localSurface.sourceFrameCount} frames`
: "body frame / current increment unavailable"}
</small>
</div>
@@ -328,13 +350,15 @@ export function M4ReplayThreatVisual({ resultId }: { resultId: string }) {
<LaboratoryMetricEvidenceScene
ref={metricSceneRef}
pointCloudBodyXyzM={frame.pointCloudBodyXyzM}
localSurfaceBodyXyzM={localSurface.pointsBodyXyzM}
obstacles={sceneObstacles}
rig={timeline.rig}
corridor={timeline.corridor}
occupiedVoxelSizeM={timeline.occupiedVoxelSizeM}
mode={spatialMode}
label="M4.6 recorded-realtime current increment and rolling occupancy"
label="M4.6 exact current increment, bounded local SLAM surface and rolling occupancy"
showCurrentIncrement={showCurrentIncrement}
showLocalSurface={showLocalSurface}
showRollingMap={showRollingMap}
/>
) : null}
apps/control-station/src/workspaces/laboratory/useM4ThreatTimeline.ts
+8
View File
@@ -146,10 +146,18 @@ export function useM4ThreatTimelineFrame({
(frame) => frame.sequence === activeSequence,
) ?? null;
}, [activeChunkStart, activeSequence, chunks]);
const availableFrames = useMemo(() => {
const unique = new Map<number, M4ThreatTimelineFrame>();
for (const chunk of chunks.values()) {
for (const frame of chunk.frames) unique.set(frame.sequence, frame);
}
return [...unique.values()].sort((left, right) => left.sequence - right.sequence);
}, [chunks]);
return {
activeSequence,
activeFrame,
availableFrames,
loading: error === null && Boolean(timeline) && !activeFrame,
error,
};