feat: finalize corrected-route planning and Rerun recording review
This commit is contained in:
@@ -15,6 +15,7 @@ from k1link.device_plugins.xgrids_k1.archive import (
|
||||
from k1link.device_plugins.xgrids_k1.recorded_point_colors import (
|
||||
RecordedPointColorOverlayStore,
|
||||
)
|
||||
from k1link.device_plugins.xgrids_k1.recorded_point_display import render_point_display
|
||||
from k1link.device_plugins.xgrids_k1.rrd_export import (
|
||||
RrdExportCancelled,
|
||||
RrdExportError,
|
||||
@@ -74,6 +75,7 @@ def build_xgrids_k1_observation(repository_root: Path, live_planning_source=None
|
||||
),
|
||||
recording_exporter=_export_recording,
|
||||
point_color_renderer=point_colors.render,
|
||||
point_display_renderer=render_point_display,
|
||||
overview_exporter=export_session_overview,
|
||||
planning_exporter=export_planning_source,
|
||||
submap_extractor=extract_submap,
|
||||
@@ -267,6 +269,8 @@ def _export_recording(
|
||||
cancel_event: threading.Event | None = None,
|
||||
activity_callback: object | None = None,
|
||||
) -> dict[str, object]:
|
||||
from k1link.reconstruction.recorded_geometry import RecordedMapGeometry
|
||||
|
||||
try:
|
||||
return dict(
|
||||
export_k1mqtt_to_rrd(
|
||||
@@ -280,9 +284,14 @@ def _export_recording(
|
||||
),
|
||||
cancel_event=cancel_event,
|
||||
activity_callback=activity_callback if callable(activity_callback) else None,
|
||||
map_geometry=RecordedMapGeometry.optional(artifacts),
|
||||
)
|
||||
)
|
||||
except RrdExportCancelled as exc:
|
||||
raise PluginRecordingExportCancelled("K1 recording export was cancelled") from exc
|
||||
except RrdExportError as exc:
|
||||
raise PluginRecordingExportError("K1 recording export failed") from exc
|
||||
|
||||
|
||||
# Explicit exporter capability: other plugins must not silently ignore a pinned map.
|
||||
_export_recording.supports_map_versions = True
|
||||
|
||||
@@ -0,0 +1,47 @@
|
||||
"""Read display rows from the pinned, already verified operator RRD.
|
||||
|
||||
The canonical exporter bakes intensity/Turbo colors into this derived file.
|
||||
Reading Arrow arrays avoids normalizing the complete raw transport again, and
|
||||
retains the exact corrected geometry and colors already used by the viewer.
|
||||
"""
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
import pyarrow as pa
|
||||
import rerun_bindings as bindings
|
||||
|
||||
|
||||
def prepared_point_rows(path: Path):
|
||||
reader = bindings.RrdReaderInternal(str(path))
|
||||
stores = [entry for entry in reader.store_entries() if entry.kind == "recording"]
|
||||
if len(stores) != 1:
|
||||
raise ValueError("display source must contain exactly one recording")
|
||||
count = 0
|
||||
for chunk in reader.stream(stores[0]):
|
||||
if str(chunk.entity_path) != "/world/points":
|
||||
continue
|
||||
batch = chunk.to_record_batch()
|
||||
names = batch.column_names
|
||||
if "Points3D:positions" not in names:
|
||||
continue
|
||||
if not {"session_time", "message_sequence", "Points3D:colors"}.issubset(names):
|
||||
raise ValueError("prepared points are missing temporal/color ownership")
|
||||
times = batch.column(names.index("session_time")).cast(pa.int64()).to_numpy()
|
||||
sequences = batch.column(names.index("message_sequence")).to_numpy()
|
||||
positions = batch.column(names.index("Points3D:positions"))
|
||||
colors = batch.column(names.index("Points3D:colors"))
|
||||
for row, time_ns in enumerate(times):
|
||||
if not positions[row].is_valid:
|
||||
continue
|
||||
xyz = positions[row].values.flatten().to_numpy().reshape(-1, 3)
|
||||
if not colors[row].is_valid:
|
||||
raise ValueError("prepared point colors are unavailable")
|
||||
rgba = colors[row].values.to_numpy()
|
||||
if len(rgba) == 1:
|
||||
rgba = np.repeat(rgba, len(xyz))
|
||||
if len(rgba) != len(xyz):
|
||||
raise ValueError("prepared color count does not match positions")
|
||||
count += 1
|
||||
yield int(sequences[row]), int(time_ns), xyz, rgba
|
||||
if not count:
|
||||
raise ValueError("prepared recording has no point frames")
|
||||
@@ -6,6 +6,7 @@ import hashlib
|
||||
import json
|
||||
import os
|
||||
import threading
|
||||
from collections.abc import Iterator
|
||||
from collections import OrderedDict
|
||||
from contextlib import suppress
|
||||
from dataclasses import dataclass
|
||||
@@ -121,6 +122,16 @@ class RecordedPointColorOverlayStore:
|
||||
capture_clock,
|
||||
capture_clock_origin,
|
||||
)
|
||||
geometry = None
|
||||
if command.map_version is not None:
|
||||
from k1link.reconstruction.recorded_geometry import RecordedMapGeometry
|
||||
|
||||
paths = {
|
||||
"map-version-" + name: command.map_version.directory / name
|
||||
for name in ("manifest.json", "points.f32", "trajectory.npz")
|
||||
}
|
||||
geometry = RecordedMapGeometry(paths)
|
||||
source_identity += (geometry.generation, *_source_identity(*paths.values()))
|
||||
settings = RerunSceneSettings(
|
||||
color_mode=color_mode,
|
||||
palette=palette,
|
||||
@@ -129,11 +140,7 @@ class RecordedPointColorOverlayStore:
|
||||
settings_key = (
|
||||
color_mode,
|
||||
palette,
|
||||
(
|
||||
custom_color.casefold()
|
||||
if palette == "custom" or color_mode == "class"
|
||||
else "-"
|
||||
),
|
||||
(custom_color.casefold() if palette == "custom" or color_mode == "class" else "-"),
|
||||
)
|
||||
payload_key = (*source_identity, application_id, recording_id, *settings_key)
|
||||
|
||||
@@ -156,6 +163,7 @@ class RecordedPointColorOverlayStore:
|
||||
metadata,
|
||||
capture_clock,
|
||||
capture_clock_origin,
|
||||
geometry,
|
||||
)
|
||||
payload = _render_color_overlay(
|
||||
index.frames,
|
||||
@@ -173,6 +181,7 @@ class RecordedPointColorOverlayStore:
|
||||
metadata: Path | None,
|
||||
capture_clock: Path | None,
|
||||
capture_clock_origin: Path | None,
|
||||
geometry=None,
|
||||
) -> _PointColorIndex:
|
||||
with self._lock:
|
||||
cached = self._indexes.get(source_identity)
|
||||
@@ -187,6 +196,7 @@ class RecordedPointColorOverlayStore:
|
||||
capture_clock,
|
||||
capture_clock_origin,
|
||||
source_identity=source_identity,
|
||||
geometry=geometry,
|
||||
)
|
||||
if index.byte_length > self._index_cache_bytes:
|
||||
return index
|
||||
@@ -222,7 +232,17 @@ def _build_index(
|
||||
capture_clock_origin: Path | None,
|
||||
*,
|
||||
source_identity: tuple[object, ...],
|
||||
geometry=None,
|
||||
) -> _PointColorIndex:
|
||||
frames = tuple(_iter_point_frames(source, metadata, capture_clock, capture_clock_origin,
|
||||
geometry=geometry))
|
||||
return _PointColorIndex(source_identity, frames, sum(frame.byte_length for frame in frames))
|
||||
|
||||
|
||||
def _iter_point_frames(
|
||||
source: Path, metadata: Path, capture_clock: Path | None,
|
||||
capture_clock_origin: Path | None, *, geometry=None,
|
||||
) -> Iterator[_PointColorFrame]:
|
||||
try:
|
||||
envelope = None if capture_clock is None else read_capture_clock_envelope(capture_clock)
|
||||
origin = (
|
||||
@@ -232,9 +252,13 @@ def _build_index(
|
||||
)
|
||||
except CaptureFormatError as exc:
|
||||
raise RecordedPointColorError("native point-color clock is invalid") from exc
|
||||
if envelope is not None and origin is not None and (
|
||||
envelope.started_at_epoch_ns != origin.started_at_epoch_ns
|
||||
or envelope.started_monotonic_ns != origin.started_monotonic_ns
|
||||
if (
|
||||
envelope is not None
|
||||
and origin is not None
|
||||
and (
|
||||
envelope.started_at_epoch_ns != origin.started_at_epoch_ns
|
||||
or envelope.started_monotonic_ns != origin.started_monotonic_ns
|
||||
)
|
||||
):
|
||||
raise RecordedPointColorError("native point-color clocks do not match")
|
||||
session_origin_ns = (
|
||||
@@ -245,11 +269,11 @@ def _build_index(
|
||||
else None
|
||||
)
|
||||
|
||||
frames: list[_PointColorFrame] = []
|
||||
total_bytes = 0
|
||||
frame_count = 0
|
||||
point_frame_number = 0
|
||||
previous_sequence = 0
|
||||
previous_monotonic_ns: int | None = None
|
||||
first_raw_receipt_s = None
|
||||
try:
|
||||
source_size = source.stat().st_size
|
||||
with source.open("rb") as raw_stream, metadata.open("r", encoding="utf-8") as index_stream:
|
||||
@@ -274,6 +298,8 @@ def _build_index(
|
||||
raise RecordedPointColorError("native point-color timeline decreases")
|
||||
previous_sequence = sequence
|
||||
previous_monotonic_ns = monotonic_ns
|
||||
if first_raw_receipt_s is None:
|
||||
first_raw_receipt_s = monotonic_ns / 1e9
|
||||
if session_origin_ns is None:
|
||||
session_origin_ns = monotonic_ns
|
||||
topic = record.get("topic")
|
||||
@@ -333,6 +359,12 @@ def _build_index(
|
||||
if decoded.colors_rgb is None
|
||||
else np.frombuffer(decoded.colors_rgb, dtype=np.uint8).reshape((-1, 3))
|
||||
)
|
||||
if geometry is not None:
|
||||
positions = geometry.points(
|
||||
point_frame_number - 1,
|
||||
monotonic_ns / 1e9 - first_raw_receipt_s,
|
||||
decoded.point_count,
|
||||
)
|
||||
positions, intensities, rgb = _recorded_view_points(
|
||||
positions,
|
||||
intensities,
|
||||
@@ -346,17 +378,12 @@ def _build_index(
|
||||
intensities=intensities.copy(),
|
||||
rgb=None if rgb is None else rgb.copy(),
|
||||
)
|
||||
frames.append(frame)
|
||||
total_bytes += frame.byte_length
|
||||
frame_count += 1
|
||||
yield frame
|
||||
except (OSError, json.JSONDecodeError, UnicodeError) as exc:
|
||||
raise RecordedPointColorError("native point-color index could not be read") from exc
|
||||
if session_origin_ns is None or not frames:
|
||||
if session_origin_ns is None or not frame_count:
|
||||
raise RecordedPointColorError("native point-color index contains no point frames")
|
||||
return _PointColorIndex(
|
||||
source_identity=source_identity,
|
||||
frames=tuple(frames),
|
||||
byte_length=total_bytes,
|
||||
)
|
||||
|
||||
|
||||
def _render_color_overlay(
|
||||
|
||||
@@ -0,0 +1,119 @@
|
||||
"""Streaming, display-only point thinning. Never edits raw or mapping evidence."""
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
import logging
|
||||
import re
|
||||
import struct
|
||||
import threading
|
||||
import time
|
||||
from pathlib import Path
|
||||
from collections.abc import Iterator
|
||||
from contextlib import suppress
|
||||
|
||||
import numpy as np
|
||||
import rerun as rr
|
||||
|
||||
from k1link.sessions import ReplayCommand
|
||||
from k1link.viewer.rerun_bridge import RerunSceneSettings, _point_colors
|
||||
from .recorded_point_colors import _artifact_path, _iter_point_frames
|
||||
from .rrd_export import APPLICATION_ID, SESSION_TIMELINE
|
||||
|
||||
DISPLAY_POINTS_PATH = "/world/display_points"
|
||||
_render_slot = threading.BoundedSemaphore(1)
|
||||
_logger = logging.getLogger(__name__)
|
||||
|
||||
|
||||
def retained_indices(count: int, decimation: float, sequence: int) -> np.ndarray:
|
||||
"""Exact per-frame count; stable ranked samples, not a LiDAR-ring stride."""
|
||||
if not math.isfinite(decimation) or not 0 <= decimation <= 100:
|
||||
raise ValueError("invalid point decimation")
|
||||
keep = int(math.floor(count * (100 - decimation) / 100 + 0.5))
|
||||
if keep == count:
|
||||
return np.arange(count)
|
||||
if keep == 0:
|
||||
return np.empty(0, dtype=np.int64)
|
||||
# A bijective integer mix gives a reproducible order and nested samples.
|
||||
keys = np.arange(count, dtype=np.uint32) ^ np.uint32(sequence & 0xFFFFFFFF)
|
||||
keys ^= keys >> 16
|
||||
keys *= np.uint32(0x7FEB352D)
|
||||
keys ^= keys >> 15
|
||||
keys *= np.uint32(0x846CA68B)
|
||||
keys ^= keys >> 16
|
||||
return np.sort(np.argpartition(keys, keep - 1)[:keep])
|
||||
|
||||
|
||||
def render_point_display(command: ReplayCommand, *, application_id: str, recording_id: str,
|
||||
color_mode: str, palette: str, custom_color: str,
|
||||
point_decimation_percent: float, display_bank: str,
|
||||
prepared_recording_path: Path | None = None) -> Iterator[bytes]:
|
||||
if application_id != APPLICATION_ID or not 0 < point_decimation_percent < 100 or not re.fullmatch(r"[a-f0-9]{32}", display_bank):
|
||||
raise ValueError("invalid point display request")
|
||||
settings = RerunSceneSettings(color_mode=color_mode, palette=palette, custom_color=custom_color)
|
||||
frames = _display_rows(command, settings, prepared_recording_path)
|
||||
if not _render_slot.acquire(timeout=30):
|
||||
raise RuntimeError("point display preparation is busy")
|
||||
recording = None
|
||||
started = time.monotonic()
|
||||
input_points = output_points = frame_count = 0
|
||||
try:
|
||||
yield b'NPD1'
|
||||
batch_points = 0
|
||||
for index, (sequence, time_ns, positions, colors) in enumerate(frames):
|
||||
if recording is None:
|
||||
recording = rr.RecordingStream(application_id, recording_id=recording_id, send_properties=False)
|
||||
stream = rr.binary_stream(recording)
|
||||
selected = retained_indices(len(positions), point_decimation_percent, sequence)
|
||||
input_points += len(positions)
|
||||
output_points += len(selected)
|
||||
frame_count += 1
|
||||
recording.set_time(SESSION_TIMELINE, duration=np.timedelta64(time_ns, "ns"))
|
||||
recording.log(f"{DISPLAY_POINTS_PATH}/{display_bank}", rr.Points3D(positions[selected], colors=colors[selected]))
|
||||
batch_points += len(selected)
|
||||
if index % 64 == 63 or batch_points >= 131072:
|
||||
payload = stream.read(flush=True, flush_timeout_sec=30.0)
|
||||
recording.disconnect()
|
||||
recording = None
|
||||
batch_points = 0
|
||||
yield struct.pack('<I', len(payload)) + payload
|
||||
if recording is not None:
|
||||
payload = stream.read(flush=True, flush_timeout_sec=30.0)
|
||||
recording.disconnect()
|
||||
recording = None
|
||||
yield struct.pack('<I', len(payload)) + payload
|
||||
_logger.info("Point display ready: bank=%s frames=%d input_points=%d output_points=%d decimation=%s elapsed_s=%.3f",
|
||||
display_bank, frame_count, input_points, output_points,
|
||||
point_decimation_percent, time.monotonic() - started)
|
||||
yield struct.pack('<I', 0)
|
||||
finally:
|
||||
frames.close()
|
||||
with suppress(Exception):
|
||||
if recording is not None:
|
||||
recording.disconnect()
|
||||
_render_slot.release()
|
||||
|
||||
|
||||
def _display_rows(command, settings, prepared_recording_path):
|
||||
if prepared_recording_path is not None and settings.color_mode == "intensity" and settings.palette == "turbo":
|
||||
from .prepared_point_display import prepared_point_rows
|
||||
yield from prepared_point_rows(prepared_recording_path)
|
||||
return
|
||||
metadata = _artifact_path(command, "raw-transport-index")
|
||||
if metadata is None:
|
||||
raise ValueError("point index is unavailable")
|
||||
geometry = None
|
||||
if command.map_version is not None:
|
||||
from k1link.reconstruction.recorded_geometry import RecordedMapGeometry
|
||||
geometry = RecordedMapGeometry({"map-version-" + name: command.map_version.directory / name
|
||||
for name in ("manifest.json", "points.f32", "trajectory.npz")})
|
||||
frames = _iter_point_frames(command.primary_artifact.path, metadata,
|
||||
_artifact_path(command, "raw-transport-clock"),
|
||||
_artifact_path(command, "raw-transport-clock-origin"), geometry=geometry)
|
||||
try:
|
||||
for frame in frames:
|
||||
# Compute the same palette range on the full frame, then select the
|
||||
# matching rows; thinning must not shift colors or corrected geometry.
|
||||
colors = _point_colors(frame.positions, frame.intensities, frame.rgb, settings)
|
||||
yield frame.sequence, frame.session_time_ns, frame.positions, colors
|
||||
finally:
|
||||
frames.close()
|
||||
@@ -6,7 +6,7 @@ import os
|
||||
import threading
|
||||
from collections.abc import Callable
|
||||
from contextlib import suppress
|
||||
from dataclasses import dataclass
|
||||
from dataclasses import dataclass, replace
|
||||
from pathlib import Path
|
||||
from typing import Any, Literal, TypedDict
|
||||
from uuid import UUID, uuid4
|
||||
@@ -52,9 +52,6 @@ APPLICATION_ID = "nodedc_mission_core_recorded"
|
||||
SESSION_TIMELINE = "session_time"
|
||||
CAPTURE_TIMELINE = "capture_time"
|
||||
JS_MAX_SAFE_INTEGER = (1 << 53) - 1
|
||||
RECORDED_VIEW_POINT_DECIMATION_THRESHOLD = 100_000
|
||||
RECORDED_VIEW_POINT_STRIDE = 4
|
||||
RECORDED_VIEW_POINT_FRAME_STRIDE = 5
|
||||
RECORDED_RRD_IDENTITY_VERSION = "missioncore.recorded-rrd/v1"
|
||||
|
||||
# Rerun keys viewer state by these IDs. Reusing them for every settings-only
|
||||
@@ -195,16 +192,17 @@ def export_k1mqtt_to_rrd(
|
||||
capture_clock_origin_path: Path | None = None,
|
||||
cancel_event: threading.Event | None = None,
|
||||
activity_callback: Callable[[], None] | None = None,
|
||||
map_geometry=None,
|
||||
) -> RrdExportSummary:
|
||||
"""Project a bounded-rate view of K1 data into one operator RRD.
|
||||
"""Project every captured K1 cloud frame into one operator RRD.
|
||||
|
||||
The raw capture remains the source of record. The derived RRD uses a
|
||||
recording-local duration timeline whose zero is the durable capture-clock
|
||||
origin for v2 recordings (or the first raw message for legacy captures).
|
||||
It never traverses the bounded live-preview queue. Point-cloud frames and
|
||||
very dense point batches are deterministically sampled for interactive
|
||||
rendering while counters, poses, capture boundaries and the native capture
|
||||
remain complete. AI jobs always read the complete native capture.
|
||||
It never traverses the bounded live-preview queue or samples away points
|
||||
and frames. Counters, poses and capture boundaries remain complete, and
|
||||
an admitted corrected map supplies its exact per-frame positions. AI jobs
|
||||
still read the complete native capture, independently of this projection.
|
||||
|
||||
The destination is replaced only after the temporary RRD has been closed,
|
||||
flushed and fsynced. Any decode, timing, sink or rename failure therefore
|
||||
@@ -236,6 +234,8 @@ def export_k1mqtt_to_rrd(
|
||||
capture_clock,
|
||||
capture_clock_origin,
|
||||
)
|
||||
if map_geometry is not None:
|
||||
recording_id = map_geometry.recording_id(recording_id)
|
||||
temporary = destination.with_name(f".{destination.name}.{uuid4()}.tmp")
|
||||
settings = RerunSceneSettings()
|
||||
blueprint = _recorded_blueprint(settings)
|
||||
@@ -264,6 +264,7 @@ def export_k1mqtt_to_rrd(
|
||||
previous_monotonic_ns: int | None = None
|
||||
last_source_time_ns: int | None = None
|
||||
last_source_capture_ns: int | None = None
|
||||
first_raw_receipt_s: float | None = None
|
||||
|
||||
try:
|
||||
recording = rr.RecordingStream(APPLICATION_ID, recording_id=recording_id)
|
||||
@@ -297,6 +298,9 @@ def export_k1mqtt_to_rrd(
|
||||
f"native capture message {message.sequence} is outside its clock envelope"
|
||||
)
|
||||
session_time_ns = monotonic_ns - session_origin_ns
|
||||
if first_raw_receipt_s is None:
|
||||
first_raw_receipt_s = monotonic_ns / 1e9
|
||||
map_time_s = monotonic_ns / 1e9 - first_raw_receipt_s
|
||||
if session_time_ns > JS_MAX_SAFE_INTEGER:
|
||||
raise RrdExportError(
|
||||
"session duration exceeds the exact JavaScript nanosecond range"
|
||||
@@ -345,9 +349,19 @@ def export_k1mqtt_to_rrd(
|
||||
if isinstance(decoded, DecodedPointCloudView):
|
||||
counters.point_frames += 1
|
||||
counters.points += decoded.point_count
|
||||
positions = (
|
||||
None
|
||||
if map_geometry is None
|
||||
else map_geometry.points(
|
||||
counters.point_frames - 1, map_time_s, decoded.point_count
|
||||
)
|
||||
)
|
||||
if _should_publish_recorded_point_frame(counters.point_frames):
|
||||
_log_points(recording, decoded, settings)
|
||||
_log_points(recording, decoded, settings, positions=positions)
|
||||
elif isinstance(decoded, DecodedPoseView):
|
||||
if map_geometry is not None:
|
||||
xyz, quaternion = map_geometry.pose(counters.pose_frames, map_time_s)
|
||||
decoded = replace(decoded, position_xyz=xyz, orientation_xyzw=quaternion)
|
||||
position = (
|
||||
float(decoded.position_xyz[0]),
|
||||
float(decoded.position_xyz[1]),
|
||||
@@ -359,6 +373,8 @@ def export_k1mqtt_to_rrd(
|
||||
else:
|
||||
counters.ignored_messages += 1
|
||||
|
||||
if map_geometry is not None:
|
||||
map_geometry.complete(counters.point_frames, counters.pose_frames)
|
||||
if session_origin_ns is None:
|
||||
raise RrdExportError("native capture contains no messages")
|
||||
if counters.decoded_messages == 0:
|
||||
@@ -471,12 +487,8 @@ def _stable_recording_id(
|
||||
str(capture_clock.started_monotonic_ns) if capture_clock is not None else "-",
|
||||
str(capture_clock.completed_at_epoch_ns) if capture_clock is not None else "-",
|
||||
str(capture_clock.completed_monotonic_ns) if capture_clock is not None else "-",
|
||||
str(capture_clock_origin.started_at_epoch_ns)
|
||||
if capture_clock_origin is not None
|
||||
else "-",
|
||||
str(capture_clock_origin.started_monotonic_ns)
|
||||
if capture_clock_origin is not None
|
||||
else "-",
|
||||
str(capture_clock_origin.started_at_epoch_ns) if capture_clock_origin is not None else "-",
|
||||
str(capture_clock_origin.started_monotonic_ns) if capture_clock_origin is not None else "-",
|
||||
):
|
||||
identity.update(value.encode("ascii"))
|
||||
identity.update(b"\0")
|
||||
@@ -814,8 +826,11 @@ def _log_points(
|
||||
recording: rr.RecordingStream,
|
||||
frame: DecodedPointCloudView,
|
||||
settings: RerunSceneSettings,
|
||||
*,
|
||||
positions: np.ndarray | None = None,
|
||||
) -> None:
|
||||
positions = np.asarray(frame.positions_xyz, dtype=np.float32).reshape((-1, 3))
|
||||
if positions is None:
|
||||
positions = np.asarray(frame.positions_xyz, dtype=np.float32).reshape((-1, 3))
|
||||
if frame.intensities is None:
|
||||
intensities = np.full(frame.point_count, 255, dtype=np.uint8)
|
||||
else:
|
||||
@@ -841,28 +856,18 @@ def _recorded_view_points(
|
||||
intensities: np.ndarray,
|
||||
rgb: np.ndarray | None,
|
||||
) -> tuple[np.ndarray, np.ndarray, np.ndarray | None]:
|
||||
# The native K1 capture remains the complete source of record and all AI
|
||||
# jobs read that source directly. Rerun is the interactive operator
|
||||
# projection: bound the temporal frame rate, but preserve complete normal
|
||||
# K1 scans. The AI composition intentionally uses one latest point frame
|
||||
# so dynamic cuboids do not stack; thinning a normal ~2.4k-point scan here
|
||||
# made that view visibly bald. Keep spatial decimation only as an emergency
|
||||
# guard for unusually large (>100k point) frames from future hardware.
|
||||
if len(positions) <= RECORDED_VIEW_POINT_DECIMATION_THRESHOLD:
|
||||
return positions, intensities, rgb
|
||||
return (
|
||||
positions[::RECORDED_VIEW_POINT_STRIDE],
|
||||
intensities[::RECORDED_VIEW_POINT_STRIDE],
|
||||
None if rgb is None else rgb[::RECORDED_VIEW_POINT_STRIDE],
|
||||
)
|
||||
# An archive is a faithful projection, not a lossy preview. Keep attributes
|
||||
# aligned and use the visible time window to control displayed history.
|
||||
# Resource admission must fail explicitly rather than silently drop points.
|
||||
return positions, intensities, rgb
|
||||
|
||||
|
||||
def _should_publish_recorded_point_frame(frame_number: int) -> bool:
|
||||
"""Keep the first point frame and then a stable 2 Hz operator cadence."""
|
||||
"""Preserve every captured point frame, including its original timestamp."""
|
||||
|
||||
if frame_number < 1:
|
||||
raise ValueError("point frame number must be positive")
|
||||
return frame_number == 1 or (frame_number - 1) % RECORDED_VIEW_POINT_FRAME_STRIDE == 0
|
||||
return True
|
||||
|
||||
|
||||
def _log_pose(
|
||||
|
||||
@@ -0,0 +1,25 @@
|
||||
"""Project recorded acquisition ownership without moving or changing captures."""
|
||||
|
||||
import json
|
||||
from uuid import UUID
|
||||
|
||||
|
||||
def reconcile_planning_captures(root, record_capture):
|
||||
"""Backfill only explicit live-run bindings, including failed/deleted studies.
|
||||
|
||||
Recorded comparisons do not change the origin of an existing independent
|
||||
survey. Project deletion is a presentation tombstone; its binding survives.
|
||||
"""
|
||||
for path in root.glob("*/report.json"):
|
||||
doc = json.loads(path.read_text())
|
||||
if doc.get("schema_version") != "missioncore.planning-live-test/v1":
|
||||
continue
|
||||
if doc.get("profile") != "planning" or not doc.get("query_session_id"):
|
||||
continue
|
||||
run_id = str(UUID(doc["id"]))
|
||||
if path.parent.name != run_id:
|
||||
raise ValueError("Planning capture report identity mismatch")
|
||||
session_id = doc["query_session_id"]
|
||||
if session_id in {doc["draft"]["zone"]["session_id"], doc.get("baseline_session_id")}:
|
||||
raise ValueError("Planning capture cannot own its reference or baseline")
|
||||
record_capture(session_id, run_id)
|
||||
@@ -0,0 +1,51 @@
|
||||
"""Resolve new references by session default, existing studies by pinned version."""
|
||||
|
||||
from .versioned_sources import VersionedPlanningSources
|
||||
|
||||
|
||||
class DefaultPlanningSources:
|
||||
def __init__(self, original, versions):
|
||||
self.original, self.versions = original, versions
|
||||
self.store, self.root = original.store, original.root
|
||||
|
||||
def get(self, session_id):
|
||||
version = self.versions.selected(session_id)
|
||||
return (
|
||||
self.original.get(session_id)
|
||||
if version is None
|
||||
else self.bound(session_id, version.generation)
|
||||
)
|
||||
|
||||
def _reader(self, session_id, generation):
|
||||
version = self.versions.version(session_id, generation)
|
||||
if version is None:
|
||||
return self.original
|
||||
return VersionedPlanningSources(self.original, version, self.root / "map-snapshots")
|
||||
|
||||
def bound(self, session_id, generation):
|
||||
doc = self._reader(session_id, generation).bound(session_id, generation)
|
||||
# The catalog identity/name describes the physical session, not its revision.
|
||||
return {**doc, "label": self.original.get(session_id)["label"]}
|
||||
|
||||
def verify(self, session_id, generation):
|
||||
return self._reader(session_id, generation).verify(session_id, generation)
|
||||
|
||||
def prepared_submaps(self, session_id, generation, **options):
|
||||
return self._reader(session_id, generation).prepared_submaps(
|
||||
session_id, generation, **options
|
||||
)
|
||||
|
||||
def submap(self, session_id, generation, start, end, **options):
|
||||
return self._reader(session_id, generation).submap(
|
||||
session_id, generation, start, end, **options
|
||||
)
|
||||
|
||||
def reference_map(self, session_id, generation, start, end, **options):
|
||||
return self._reader(session_id, generation).reference_map(
|
||||
session_id, generation, start, end, **options
|
||||
)
|
||||
|
||||
def scene_reference_map(self, session_id, generation, start, end, **options):
|
||||
return self._reader(session_id, generation).scene_reference_map(
|
||||
session_id, generation, start, end, **options
|
||||
)
|
||||
@@ -53,7 +53,13 @@ class MissionDrafts:
|
||||
|
||||
def save(self, request):
|
||||
source = self.sources.bound(request.session_id, request.generation)
|
||||
route = route_from_source(source, request.start_index, request.end_index, request.direction)
|
||||
if getattr(request, 'whole_recording', False):
|
||||
start, end = 0, len(source['poses']) - 1
|
||||
else:
|
||||
start, end = request.start_index, request.end_index
|
||||
if start is None or end is None:
|
||||
raise ValueError('Выберите полную запись эталона.')
|
||||
route = route_from_source(source, start, end, request.direction)
|
||||
id = str(request.id or uuid4())
|
||||
body = {'schema_version': 'missioncore.mission-draft/v1', 'name': request.name.strip(),
|
||||
'vehicle_id': None, 'status': 'draft', 'zone': {key: source[key] for key in
|
||||
|
||||
@@ -195,6 +195,7 @@ def acquire_entry(
|
||||
fitter=None,
|
||||
clock=time.monotonic,
|
||||
policy=ENTRY_POLICY,
|
||||
progress=None,
|
||||
):
|
||||
reference, query, initial = cloud(reference), cloud(query), rigid(initial)
|
||||
started = clock()
|
||||
@@ -207,7 +208,9 @@ def acquire_entry(
|
||||
|
||||
attempts = []
|
||||
for seed in entry_seeds(initial, query_entry, reference_forward, policy=policy):
|
||||
if clock() - started >= policy["deadline_s"]:
|
||||
if progress is not None:
|
||||
progress(dict(stage="dense-start", completed_fits=len(attempts)))
|
||||
if policy["deadline_s"] is not None and clock() - started >= policy["deadline_s"]:
|
||||
break
|
||||
matrix = seed.pop("matrix")
|
||||
result = fitter(reference, query, matrix)
|
||||
@@ -217,7 +220,7 @@ def acquire_entry(
|
||||
attempts,
|
||||
initial,
|
||||
query_entry,
|
||||
complete=elapsed <= policy["deadline_s"],
|
||||
complete=policy["deadline_s"] is None or elapsed <= policy["deadline_s"],
|
||||
policy=policy,
|
||||
)
|
||||
result["initialization"]["elapsed_s"] = elapsed
|
||||
|
||||
@@ -1,9 +1,9 @@
|
||||
"""One admission and termination policy for a selected live route."""
|
||||
"""Reference admission is independent from the lifetime of a live pass."""
|
||||
|
||||
import math
|
||||
|
||||
LIVE_ROUTE_POLICY = dict(
|
||||
version="selected-live-route/v1",
|
||||
version="selected-live-route/v2",
|
||||
minimum_m=3.0,
|
||||
maximum_m=None,
|
||||
maximum_seconds=None,
|
||||
@@ -16,6 +16,8 @@ def live_route_limits(length_m):
|
||||
raise ValueError("Для привязки выберите участок длиной не менее 3 м.")
|
||||
return dict(
|
||||
route_policy=LIVE_ROUTE_POLICY.copy(),
|
||||
maximum_distance_m=length,
|
||||
# Reference length describes map coverage, never a travel budget.
|
||||
# Keep null in the wire contract (and do not rewrite historical runs).
|
||||
maximum_distance_m=None,
|
||||
maximum_seconds=None,
|
||||
)
|
||||
|
||||
@@ -37,10 +37,15 @@ logger = logging.getLogger(__name__)
|
||||
|
||||
|
||||
class PlanningLiveTests:
|
||||
def __init__(self, drafts, sources, compute_lock):
|
||||
def __init__(self, drafts, sources, compute_lock, *, capture_recorder=None):
|
||||
self.drafts, self.sources, self.compute_lock = drafts, sources, compute_lock
|
||||
self.root = drafts.database.parent / "live-tests"
|
||||
self.root.mkdir(exist_ok=True)
|
||||
self.capture_recorder = capture_recorder
|
||||
if capture_recorder is not None:
|
||||
from .capture_catalog import reconcile_planning_captures
|
||||
|
||||
reconcile_planning_captures(self.root, capture_recorder)
|
||||
self.lock = threading.RLock()
|
||||
self.run = None
|
||||
self.sample = None
|
||||
@@ -177,6 +182,9 @@ class PlanningLiveTests:
|
||||
|
||||
def update(self, **values):
|
||||
with self.lock:
|
||||
capture = values.get("query_session_id")
|
||||
if capture and self.capture_recorder is not None:
|
||||
self.capture_recorder(capture, self.run["id"])
|
||||
self.run.update(values)
|
||||
self.revision += 1
|
||||
self.persist()
|
||||
|
||||
@@ -1,9 +1,8 @@
|
||||
"""Staged stationary localisation before the normal fresh-data tracking gate.
|
||||
|
||||
A known start is the reliable laboratory path, so it first receives a dense
|
||||
multi-start fit. Only its honest rejection permits retrieval over the entire
|
||||
selected route. That preserves a repeatable start while retaining an auditable
|
||||
recovery path for a restarted rover that must look for *where it is*.
|
||||
A known start first receives a dense multi-start fit, but it cannot shortcut
|
||||
comparison with the entire selected route. A finite queue, not elapsed wall
|
||||
time, defines completeness. The process owner handles cancellation and stalls.
|
||||
|
||||
Neither stage grants tracking or vehicle authority: both only produce a
|
||||
provisional hypothesis for the separate, disjoint fresh-data gate.
|
||||
@@ -13,7 +12,6 @@ from __future__ import annotations
|
||||
|
||||
import math
|
||||
import time
|
||||
from copy import deepcopy
|
||||
from dataclasses import dataclass
|
||||
from itertools import product
|
||||
|
||||
@@ -21,15 +19,17 @@ import numpy as np
|
||||
|
||||
from .entry_acquisition import acquire_entry
|
||||
from .observation_profiles import TRACKING_INPUT
|
||||
from .reference_window import reference_window
|
||||
from .reference_window import ReferenceCoverageError, reference_window
|
||||
from .registration import POLICY as TRACKING_POLICY
|
||||
from .registration import PreparedReference, angle_deg, cloud, rigid, transform
|
||||
from .stationary_entry import STATIONARY_POLICY
|
||||
|
||||
ROUTE_RELOCALIZATION_POLICY = dict(
|
||||
version="route-relocalization/v6",
|
||||
version="route-relocalization/v7",
|
||||
scope="selected-route",
|
||||
strategy="dense-start-first-then-route-recovery/v1",
|
||||
strategy="dense-start-and-complete-route-comparison/v2",
|
||||
hypothesis_freshness="stationary-receipts-and-disjoint-confirmation/v1",
|
||||
seed_modes=["pose-anchor", "cloud-median"],
|
||||
# Local geometry is independent of the 80-m presentation envelope.
|
||||
query_radius_m=TRACKING_INPUT["radius_m"],
|
||||
anchor_spacing_m=5.0,
|
||||
@@ -54,13 +54,9 @@ ROUTE_RELOCALIZATION_POLICY = dict(
|
||||
cluster_rotation_deg=8.0,
|
||||
ambiguity_overlap_margin=0.05,
|
||||
ambiguity_rmse_margin_m=0.03,
|
||||
# Keep the stationary prefix younger than the bootstrap's 40-s source-age
|
||||
# fence. A late exhaustive calculation is an explicit incomplete search,
|
||||
# never a stale provisional position.
|
||||
deadline_s=30.0,
|
||||
maximum_search_wall_s=35.0,
|
||||
# This is a numerical convergence envelope, not an operator start-radius
|
||||
# admission rule. Reaching its wall deadline is reported as incomplete.
|
||||
# No overall search timer: every admitted place must be compared. A child
|
||||
# that makes NO progress is separately stopped, never called a map mismatch.
|
||||
worker_stall_s=60.0,
|
||||
registration_policy={
|
||||
**TRACKING_POLICY,
|
||||
"version": "route-relocalization-gicp/v1",
|
||||
@@ -257,7 +253,8 @@ class RouteCandidate:
|
||||
|
||||
|
||||
def rank_route_candidates(
|
||||
reference, reference_path, query, *, policy=ROUTE_RELOCALIZATION_POLICY, grid=None
|
||||
reference, reference_path, query, *, policy=ROUTE_RELOCALIZATION_POLICY, grid=None,
|
||||
on_progress=None,
|
||||
):
|
||||
"""Rank every resampled route position against the stationary query cloud."""
|
||||
reference, query = route_reference_cloud(reference), cloud(query)
|
||||
@@ -267,6 +264,9 @@ def rank_route_candidates(
|
||||
query_descriptor = radial_height_descriptor(query, query_center, policy=policy)
|
||||
ranked = []
|
||||
for index, (position, distance) in enumerate(zip(anchors, progress, strict=True)):
|
||||
if on_progress is not None:
|
||||
on_progress(dict(stage="route-index", completed_anchors=index,
|
||||
total_anchors=len(anchors)))
|
||||
target = local_submap(
|
||||
grid,
|
||||
position,
|
||||
@@ -454,6 +454,8 @@ def relocalize_route(
|
||||
*,
|
||||
clock=time.monotonic,
|
||||
policy=ROUTE_RELOCALIZATION_POLICY,
|
||||
on_progress=None,
|
||||
additional_hypotheses=(),
|
||||
):
|
||||
"""Run complete candidate retrieval and qualification against a selected route."""
|
||||
started = clock()
|
||||
@@ -461,7 +463,7 @@ def relocalize_route(
|
||||
query_entry = np.asarray(query_entry, dtype=float).reshape(3)
|
||||
grid = ReferenceGrid(reference, cell_m=policy["spatial_cell_m"])
|
||||
ranked, coverage = rank_route_candidates(
|
||||
reference, reference_path, query, policy=policy, grid=grid
|
||||
reference, reference_path, query, policy=policy, grid=grid, on_progress=on_progress
|
||||
)
|
||||
attempts, evaluated, batches = [], [], []
|
||||
query_center = np.median(query, axis=0)
|
||||
@@ -470,11 +472,10 @@ def relocalize_route(
|
||||
float(np.linalg.norm(query - query_center, axis=1).max())
|
||||
+ policy["target_context_margin_m"],
|
||||
)
|
||||
expected = len(ranked) * policy["yaw_candidates_per_place"]
|
||||
fits_per_place = policy["yaw_candidates_per_place"] * len(policy["seed_modes"])
|
||||
expected = len(ranked) * fits_per_place
|
||||
batch_size = policy["candidate_batch_size"]
|
||||
for candidate in ranked:
|
||||
if clock() - started > policy["deadline_s"]:
|
||||
break
|
||||
if len(evaluated) % batch_size == 0:
|
||||
batches.append([])
|
||||
target = local_submap(
|
||||
@@ -487,12 +488,18 @@ def relocalize_route(
|
||||
target_center = np.median(target, axis=0)
|
||||
count_before = len(attempts)
|
||||
prepared = None
|
||||
for yaw_deg in _yaw_candidates(
|
||||
query, target, query_center, target_center, policy=policy
|
||||
for yaw_deg, seed_mode in product(
|
||||
_yaw_candidates(query, target, query_center, target_center, policy=policy),
|
||||
policy["seed_modes"],
|
||||
):
|
||||
if clock() - started > policy["deadline_s"]:
|
||||
break
|
||||
initial = _seed(query_center, target_center, yaw_deg)
|
||||
if on_progress is not None:
|
||||
on_progress(dict(stage="route-search", completed_fits=len(attempts),
|
||||
total_fits=expected, candidate_index=candidate.index))
|
||||
# The sensor pose is the spatial origin of this hypothesis. Cloud
|
||||
# medians shift with occlusion/vegetation and are not scanner poses.
|
||||
initial = (_seed(query_entry, candidate.position, yaw_deg)
|
||||
if seed_mode == "pose-anchor"
|
||||
else _seed(query_center, target_center, yaw_deg))
|
||||
try:
|
||||
# Target preprocessing is independent of yaw. Keep one tree
|
||||
# per place; all seeds and all eligibility checks stay intact.
|
||||
@@ -512,15 +519,21 @@ def relocalize_route(
|
||||
descriptor_distance=candidate.descriptor_distance,
|
||||
),
|
||||
yaw_deg=yaw_deg,
|
||||
seed_mode=seed_mode,
|
||||
result=result,
|
||||
)
|
||||
)
|
||||
if len(attempts) - count_before != policy["yaw_candidates_per_place"]:
|
||||
if len(attempts) - count_before != fits_per_place:
|
||||
break
|
||||
evaluated.append(candidate.index)
|
||||
batches[-1].append(candidate.index)
|
||||
complete = len(evaluated) == len(ranked) and clock() - started <= policy["deadline_s"]
|
||||
result = choose_route_location(attempts, query_entry, complete=complete, policy=policy)
|
||||
complete = len(evaluated) == len(ranked)
|
||||
result = choose_route_location(
|
||||
[*attempts, *additional_hypotheses], query_entry, complete=complete, policy=policy
|
||||
)
|
||||
# Dense-start evidence is accounted for by the caller, not counted twice as
|
||||
# one extra route seed. It nevertheless participates in spatial ambiguity.
|
||||
result["initialization"]["attempts"] = result["initialization"]["attempts"][:len(attempts)]
|
||||
result["initialization"].update(
|
||||
coverage,
|
||||
elapsed_s=clock() - started,
|
||||
@@ -644,23 +657,26 @@ def relocalize_start_then_route(
|
||||
policy=ROUTE_RELOCALIZATION_POLICY,
|
||||
reference_position=None,
|
||||
route_only=False,
|
||||
on_progress=None,
|
||||
):
|
||||
"""Use the proven start-area fit first, then a bounded route fallback.
|
||||
|
||||
This is deliberately not a looser acceptance rule. The dense start fit
|
||||
runs every stationary multi-start seed against its high-resolution local
|
||||
target. Only an honest rejection enters whole-route retrieval, whose
|
||||
result remains provisional until the existing fresh-data gate confirms it.
|
||||
"""
|
||||
"""Compare the proven dense start with every route place before deciding."""
|
||||
started = clock()
|
||||
if route_only:
|
||||
# A dense-start prior failed fresh confirmation. Recollect first, then
|
||||
# search the route without repeatedly retrying that unconfirmed start.
|
||||
return relocalize_route(reference, reference_path, query, query_entry,
|
||||
clock=clock, policy=policy)
|
||||
target, query, initial, entry, forward, window = _route_start_context(
|
||||
reference, reference_path, query, query_entry, reference_position
|
||||
)
|
||||
clock=clock, policy=policy, on_progress=on_progress)
|
||||
try:
|
||||
target, query, initial, entry, forward, window = _route_start_context(
|
||||
reference, reference_path, query, query_entry, reference_position
|
||||
)
|
||||
except ReferenceCoverageError as exc:
|
||||
# A sparse start patch is not proof that the whole known route is
|
||||
# unusable. Keep the ordinary global proof and fresh confirmation.
|
||||
result = relocalize_route(reference, reference_path, query, query_entry,
|
||||
clock=clock, policy=policy, on_progress=on_progress)
|
||||
result["initialization"]["dense_start_unavailable"] = str(exc)
|
||||
return result
|
||||
start_result = acquire_entry(
|
||||
target,
|
||||
query,
|
||||
@@ -668,7 +684,8 @@ def relocalize_start_then_route(
|
||||
entry,
|
||||
forward,
|
||||
clock=clock,
|
||||
policy=STATIONARY_POLICY,
|
||||
policy={**STATIONARY_POLICY, "deadline_s": None},
|
||||
progress=on_progress,
|
||||
)
|
||||
start_result["initialization"].update(
|
||||
scope=policy["scope"],
|
||||
@@ -686,35 +703,26 @@ def relocalize_start_then_route(
|
||||
]
|
||||
),
|
||||
)
|
||||
if start_result["status"] == "candidate":
|
||||
start_result["initialization"] = _hybrid_initialization(policy, start_result)
|
||||
return start_result
|
||||
if not start_result["initialization"].get("complete"):
|
||||
# Compute exhaustion is not evidence that this place did not match.
|
||||
start_result["initialization"] = _hybrid_initialization(policy, start_result)
|
||||
return start_result
|
||||
|
||||
# A failed standard start may still be a valid mid-route or recovery
|
||||
# position. Give retrieval only the fresh-prefix time remaining: it must
|
||||
# never turn a late calculation into an apparently usable prior.
|
||||
remaining = policy["maximum_search_wall_s"] - (clock() - started)
|
||||
if remaining <= 0:
|
||||
route_result = choose_route_location([], entry, complete=False, policy=policy)
|
||||
route_result["initialization"].update(
|
||||
elapsed_s=0.0, worker_timeout_reason="start-stage-timeout"
|
||||
)
|
||||
else:
|
||||
recovery_policy = deepcopy(policy)
|
||||
recovery_policy["deadline_s"] = min(policy["deadline_s"], remaining)
|
||||
route_result = relocalize_route(
|
||||
reference,
|
||||
reference_path,
|
||||
query,
|
||||
entry,
|
||||
clock=clock,
|
||||
policy=recovery_policy,
|
||||
)
|
||||
additional = []
|
||||
if start_result["status"] == "candidate":
|
||||
additional.append(dict(
|
||||
candidate=dict(index=-1, position=start_result["initialization"]["reference_position"],
|
||||
progress_m=start_result["initialization"]["route_progress_m"],
|
||||
descriptor_distance=0.0),
|
||||
yaw_deg=0.0,
|
||||
result={key: value for key, value in start_result.items() if key != "initialization"},
|
||||
))
|
||||
route_result = relocalize_route(
|
||||
reference, reference_path, query, entry, clock=clock, policy=policy,
|
||||
on_progress=on_progress, additional_hypotheses=additional,
|
||||
)
|
||||
route_result["initialization"] = _hybrid_initialization(policy, start_result, route_result)
|
||||
route_result["initialization"]["elapsed_s"] = clock() - started
|
||||
route_result["registration_seconds"] = start_result.get(
|
||||
"registration_seconds", 0.0
|
||||
) + route_result.get("registration_seconds", 0.0)
|
||||
|
||||
@@ -4,6 +4,8 @@ import json
|
||||
import os
|
||||
import subprocess
|
||||
import sys
|
||||
import time
|
||||
from contextlib import suppress
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
@@ -38,9 +40,13 @@ def incomplete_result(reason):
|
||||
def run_route_relocalization(
|
||||
directory, reference, reference_path, query, query_entry, *, reference_position=None,
|
||||
route_only=False,
|
||||
cancel_event=None,
|
||||
):
|
||||
source = directory / "route-relocalization-input.npz"
|
||||
destination = directory / "route-relocalization-result.json"
|
||||
progress_path = directory / "route-relocalization-progress.json"
|
||||
if cancel_event is not None and cancel_event.is_set():
|
||||
return incomplete_result("worker-cancelled")
|
||||
np.savez_compressed(
|
||||
source,
|
||||
reference=reference,
|
||||
@@ -57,8 +63,7 @@ def run_route_relocalization(
|
||||
"VECLIB_MAXIMUM_THREADS": "1",
|
||||
}
|
||||
with (directory / "calculation.log").open("wb") as log:
|
||||
try:
|
||||
subprocess.run(
|
||||
with subprocess.Popen(
|
||||
[
|
||||
sys.executable,
|
||||
"-m",
|
||||
@@ -69,20 +74,70 @@ def run_route_relocalization(
|
||||
env=environment,
|
||||
stdout=log,
|
||||
stderr=log,
|
||||
timeout=ROUTE_RELOCALIZATION_POLICY["maximum_search_wall_s"] + 5,
|
||||
check=True,
|
||||
)
|
||||
except subprocess.TimeoutExpired:
|
||||
# A process timeout says nothing about whether the scanner is at a
|
||||
# known place. Return a normal, persisted incomplete-search result
|
||||
# so the UI can distinguish it from a geometric rejection.
|
||||
destination.write_text(json.dumps(incomplete_result("worker-timeout"), allow_nan=False))
|
||||
) as process:
|
||||
reason = supervise_search(process, progress_path, cancel_event=cancel_event)
|
||||
if reason is not None:
|
||||
destination.write_text(json.dumps(incomplete_result(reason), allow_nan=False))
|
||||
return json.loads(destination.read_text())
|
||||
|
||||
|
||||
def supervise_search(process, progress_path, *, cancel_event=None, clock=time.monotonic,
|
||||
stall_s=ROUTE_RELOCALIZATION_POLICY["worker_stall_s"]):
|
||||
"""Only inactivity is timed. Advancing a finite queue may take any duration.
|
||||
|
||||
Cancellation owns this exact child, never other workers or the scanner.
|
||||
Reap it before returning so a retry cannot overlap the previous search.
|
||||
"""
|
||||
try:
|
||||
return _supervise_search(process, progress_path, cancel_event=cancel_event,
|
||||
clock=clock, stall_s=stall_s)
|
||||
finally:
|
||||
# An unexpected supervisor/file error must not leave a finite but long
|
||||
# numerical job running outside the planner's lifetime either.
|
||||
if process.poll() is None:
|
||||
process.terminate()
|
||||
try:
|
||||
process.wait(timeout=2)
|
||||
except subprocess.TimeoutExpired:
|
||||
process.kill()
|
||||
process.wait()
|
||||
|
||||
|
||||
def _supervise_search(process, progress_path, *, cancel_event, clock, stall_s):
|
||||
last_progress = clock()
|
||||
signature = None
|
||||
while process.poll() is None:
|
||||
try:
|
||||
current = progress_path.stat().st_mtime_ns
|
||||
except FileNotFoundError:
|
||||
current = None
|
||||
if current != signature:
|
||||
last_progress, signature = clock(), current
|
||||
reason = (
|
||||
"worker-cancelled" if cancel_event is not None and cancel_event.is_set()
|
||||
else "worker-stalled" if clock() - last_progress > stall_s else None
|
||||
)
|
||||
if reason:
|
||||
return reason
|
||||
with suppress(subprocess.TimeoutExpired):
|
||||
process.wait(timeout=0.1)
|
||||
if process.returncode:
|
||||
raise subprocess.CalledProcessError(process.returncode, process.args)
|
||||
return None
|
||||
|
||||
|
||||
def main():
|
||||
from .route_relocalization import relocalize_start_then_route
|
||||
|
||||
destination = Path(sys.argv[2])
|
||||
progress_path = destination.with_name("route-relocalization-progress.json")
|
||||
|
||||
def progress(value):
|
||||
temporary = progress_path.with_suffix(".tmp")
|
||||
temporary.write_text(json.dumps({**value, "monotonic_ns": time.monotonic_ns()}))
|
||||
temporary.replace(progress_path)
|
||||
|
||||
progress(dict(stage="loading"))
|
||||
with np.load(Path(sys.argv[1]), allow_pickle=False) as data:
|
||||
result = relocalize_start_then_route(
|
||||
data["reference"],
|
||||
@@ -91,8 +146,10 @@ def main():
|
||||
data["query_entry"],
|
||||
reference_position=data.get("reference_position"),
|
||||
route_only=bool(data.get("route_only", False)),
|
||||
on_progress=progress,
|
||||
)
|
||||
Path(sys.argv[2]).write_text(json.dumps(result, allow_nan=False))
|
||||
progress(dict(stage="complete"))
|
||||
destination.write_text(json.dumps(result, allow_nan=False))
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
|
||||
@@ -6,13 +6,13 @@ not prove SLAM frame continuity; this remains a laboratory-only protocol.
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .causal_tracking import CausalTracking
|
||||
from .causal_tracking import TRACKING_POLICY, CausalTracking
|
||||
from .live_buffer import LiveCloudBuffer
|
||||
from .registration import rigid
|
||||
from .stationary_entry import STATIONARY_POLICY, StationaryPrefix
|
||||
|
||||
BOOTSTRAP_POLICY = dict(
|
||||
version="stationary-fresh-bootstrap/v3",
|
||||
version="stationary-fresh-bootstrap/v4",
|
||||
prefix_seconds=10.0,
|
||||
maximum_motion_m=0.10,
|
||||
maximum_search_wall_s=30.0,
|
||||
@@ -58,6 +58,15 @@ class StationaryBootstrap:
|
||||
self.candidate_index = None
|
||||
self.dense_start_prior = False
|
||||
self.retry_route_search = False
|
||||
self.last_pose_ns = None
|
||||
self.last_cloud_ns = None
|
||||
self.search_continuity_proven = False
|
||||
|
||||
@property
|
||||
def continuous_search(self):
|
||||
return self.initialization_policy.get("hypothesis_freshness") == (
|
||||
"stationary-receipts-and-disjoint-confirmation/v1"
|
||||
)
|
||||
|
||||
def stop(self, reason):
|
||||
self.prior = None
|
||||
@@ -66,6 +75,7 @@ class StationaryBootstrap:
|
||||
self.candidate_queue = []
|
||||
self.retry_route_search = False
|
||||
self.gate.clear(reason)
|
||||
self.search_continuity_proven = False
|
||||
self.phase = "lost"
|
||||
self.reason = reason
|
||||
|
||||
@@ -82,6 +92,16 @@ class StationaryBootstrap:
|
||||
self.stop("source-order-changed")
|
||||
raise ValueError("Source sequence or receipt clock regressed.")
|
||||
self.last_event_ns, self.last_sequence = event.monotonic_ns, event.sequence
|
||||
if event.kind == "pose":
|
||||
self.last_pose_ns = event.monotonic_ns
|
||||
if self.phase == "searching" and self.continuous_search:
|
||||
motion = float(np.linalg.norm(
|
||||
np.asarray(event.position) - self.prefix.first_position
|
||||
))
|
||||
if not np.isfinite(motion) or motion > BOOTSTRAP_POLICY["maximum_motion_m"]:
|
||||
self.stop("search-motion")
|
||||
elif event.kind == "points":
|
||||
self.last_cloud_ns = event.monotonic_ns
|
||||
if segment != self.segment:
|
||||
# Worker completion is not a data receipt. A gap straddling that
|
||||
# instant may finish before the first fresh cloud. No validation has
|
||||
@@ -99,7 +119,9 @@ class StationaryBootstrap:
|
||||
if awaiting_first_cloud:
|
||||
self.segment = segment
|
||||
self._reset_fresh(self.floor_ns)
|
||||
elif self.phase in {"refreshing", "validating", "tracking"}:
|
||||
elif self.phase in {"refreshing", "validating", "tracking"} or (
|
||||
self.phase == "searching" and self.continuous_search
|
||||
):
|
||||
self.stop("receipt-gap")
|
||||
self.segment = segment
|
||||
if self.phase == "collecting":
|
||||
@@ -109,6 +131,16 @@ class StationaryBootstrap:
|
||||
|
||||
def tick(self, now_ns, segment):
|
||||
self.gate.tick(now_ns, segment)
|
||||
if self.phase == "searching" and self.continuous_search:
|
||||
# Old geometry may propose a place only while the live scanner is
|
||||
# still stationary in the same receipt segment. Neither a cloud-only
|
||||
# nor a pose-only stream can keep a long search alive.
|
||||
if segment != self.initialization_sample["segment"]:
|
||||
self.stop("receipt-gap")
|
||||
elif any(stamp is None or not 0 <= (now_ns - stamp) / 1e9
|
||||
<= TRACKING_POLICY["maximum_age_s"]
|
||||
for stamp in (self.last_pose_ns, self.last_cloud_ns)):
|
||||
self.stop("search-source-stale")
|
||||
if self.phase in {"validating", "tracking"} and self.gate.reason == "stale":
|
||||
self.stop("stale")
|
||||
if self.phase == "refreshing" and (
|
||||
@@ -129,8 +161,10 @@ class StationaryBootstrap:
|
||||
return sample, initial, forward, meta
|
||||
|
||||
def offer_prior(self, result, now_ns, segment):
|
||||
self.tick(now_ns, segment)
|
||||
if self.phase != "searching":
|
||||
return dict(accepted=False, reason="inactive-initialization", provisional=False)
|
||||
return dict(accepted=False, reason=self.reason if self.phase == "lost"
|
||||
else "inactive-initialization", provisional=False)
|
||||
sample = self.initialization_sample
|
||||
age = (now_ns - sample["monotonic_ns"]) / 1e9
|
||||
initialization = result.get("initialization", {})
|
||||
@@ -158,11 +192,14 @@ class StationaryBootstrap:
|
||||
or initialization.get("expected_attempts") != len(initialization.get("attempts", []))
|
||||
):
|
||||
reason = "initialization-incomplete"
|
||||
elif not 0 <= (now_ns - self.search_started_ns) / 1e9 <= self.initialization_policy.get(
|
||||
"maximum_search_wall_s", BOOTSTRAP_POLICY["maximum_search_wall_s"]
|
||||
elif not self.continuous_search and not (
|
||||
0 <= (now_ns - self.search_started_ns) / 1e9 <= self.initialization_policy.get(
|
||||
"maximum_search_wall_s", BOOTSTRAP_POLICY["maximum_search_wall_s"]
|
||||
)
|
||||
):
|
||||
reason = "initialization-expired"
|
||||
elif not 0 <= age <= BOOTSTRAP_POLICY["maximum_prior_source_age_s"]:
|
||||
elif age < 0 or (not self.continuous_search
|
||||
and age > BOOTSTRAP_POLICY["maximum_prior_source_age_s"]):
|
||||
reason = "prior-source-expired"
|
||||
elif not 0 <= segment - sample["segment"] <= BOOTSTRAP_POLICY["maximum_pre_ready_gaps"]:
|
||||
reason = "too-many-receipt-gaps"
|
||||
@@ -178,6 +215,7 @@ class StationaryBootstrap:
|
||||
self.stop("initialization-incomplete")
|
||||
return dict(accepted=False, reason=self.reason, provisional=False, age_s=age)
|
||||
self.candidate_queue = [dict(item) for item in queue[1:]]
|
||||
self.search_continuity_proven = self.continuous_search
|
||||
self.candidate_trial = 1
|
||||
self.candidate_index = initialization.get("selected_candidate_index")
|
||||
stages = initialization.get("stages", [])
|
||||
@@ -196,6 +234,7 @@ class StationaryBootstrap:
|
||||
provisional=True,
|
||||
source_segment=sample["segment"],
|
||||
validation_segment=segment,
|
||||
stationary_search_continuity=self.search_continuity_proven,
|
||||
)
|
||||
|
||||
def _reset_fresh(self, floor_ns):
|
||||
@@ -208,10 +247,13 @@ class StationaryBootstrap:
|
||||
|
||||
This is available only before tracking. Once tracking is established,
|
||||
loss must use the ordinary last-confirmed-place recovery instead.
|
||||
The original prefix's source-age fence is never extended by retries.
|
||||
The legacy local protocol retains its source-age fence. Whole-route
|
||||
hypotheses have proved stationary receipt continuity and still need a
|
||||
new, disjoint current-data trial for EACH candidate.
|
||||
"""
|
||||
age = (now_ns - self.initialization_sample["monotonic_ns"]) / 1e9
|
||||
if not 0 <= age <= BOOTSTRAP_POLICY["maximum_prior_source_age_s"]:
|
||||
if age < 0 or (not self.search_continuity_proven
|
||||
and age > BOOTSTRAP_POLICY["maximum_prior_source_age_s"]):
|
||||
self.stop("prior-source-expired")
|
||||
return
|
||||
candidate = self.candidate_queue.pop(0)
|
||||
|
||||
@@ -2,6 +2,7 @@
|
||||
|
||||
import json
|
||||
import subprocess
|
||||
import threading
|
||||
|
||||
import numpy as np
|
||||
|
||||
@@ -17,8 +18,7 @@ PHASE_MESSAGE = {
|
||||
"waiting-cloud": "Ожидание облака точек после подготовки сканера.",
|
||||
"collecting": "Накопление данных. Сканер должен оставаться неподвижным.",
|
||||
"searching": (
|
||||
"Точная привязка у стартовой зоны; при честном отказе — поиск по выбранному "
|
||||
"маршруту. Ожидание на месте."
|
||||
"Поиск по всему выбранному маршруту. Оставайтесь на месте до подтверждения привязки."
|
||||
),
|
||||
"refreshing": "Подтверждение привязки по новым кадрам. Ожидание на месте.",
|
||||
"validating": "Подтверждение привязки по новым кадрам. Ожидание на месте.",
|
||||
@@ -42,9 +42,15 @@ def phase_message(boot):
|
||||
)
|
||||
if boot.reason in {"initialization-incomplete", "initialization-expired"}:
|
||||
return (
|
||||
"Синхронизация маршрута не завершилась. Остановите устройство и запись, "
|
||||
"затем начните новое исследование и дождитесь неподвижной калибровки."
|
||||
"Поиск не завершён. Оставьте сканер неподвижно и нажмите "
|
||||
"«Переинициализировать». Запись продолжается."
|
||||
)
|
||||
if boot.reason == "search-motion":
|
||||
return ("Сканер перемещён во время поиска. "
|
||||
"Остановитесь и нажмите «Переинициализировать».")
|
||||
if boot.reason == "search-source-stale":
|
||||
return ("Данные сканера перестали поступать. "
|
||||
"Проверьте поток и нажмите «Переинициализировать».")
|
||||
return (
|
||||
"Синхронизация маршрута не выполнена. Убедитесь, что сканер находится "
|
||||
"у исследованного участка; можно выбрать другую различимую точку, остановиться "
|
||||
@@ -74,6 +80,8 @@ def run_stationary_live(service, source, run_id, executor, clock, initialize, ca
|
||||
recovery_attempt = 0
|
||||
recovery_position = None
|
||||
route_search_only = False
|
||||
search_cancel = threading.Event()
|
||||
requested_retry = None
|
||||
|
||||
def begin_recovery(reason):
|
||||
# Retain only the last confirmed place as a SEARCH HINT. Neither the
|
||||
@@ -360,18 +368,20 @@ def run_stationary_live(service, source, run_id, executor, clock, initialize, ca
|
||||
else "input-ended"
|
||||
)
|
||||
break
|
||||
retry_attempt = service.consume_reinitialization(run_id)
|
||||
if retry_attempt is not None:
|
||||
requested_retry = service.consume_reinitialization(run_id) or requested_retry
|
||||
if requested_retry is not None:
|
||||
if future is not None:
|
||||
# The public operation admits only a terminal initial
|
||||
# failure. Keep this fence in case a caller races an
|
||||
# internal state update.
|
||||
raise RuntimeError("Нельзя переинициализировать во время расчёта привязки.")
|
||||
reset_initialization(retry_attempt)
|
||||
continue
|
||||
search_cancel.set()
|
||||
finish(active=False)
|
||||
if future is None:
|
||||
reset_initialization(requested_retry)
|
||||
requested_retry = None
|
||||
continue
|
||||
now = clock.monotonic()
|
||||
if boot is not None:
|
||||
boot.tick(clock.monotonic_ns(), buffer.segment)
|
||||
if boot.phase == "lost":
|
||||
search_cancel.set()
|
||||
finish()
|
||||
current = source.snapshot()
|
||||
if not current["active"] or current.get("spatial_stop_requested", False):
|
||||
@@ -510,12 +520,9 @@ def run_stationary_live(service, source, run_id, executor, clock, initialize, ca
|
||||
receipt_gaps=sample["gaps"],
|
||||
)
|
||||
last_snapshot = now
|
||||
if buffer.distance >= service.run["maximum_distance_m"]:
|
||||
# A pose can reach the limit before the next cloud snapshot.
|
||||
service.update_sample(buffer.snapshot(), clock.monotonic_ns())
|
||||
service.update(distance_m=buffer.distance)
|
||||
end_reason = "distance-limit"
|
||||
break
|
||||
# Travel is telemetry, not completion. Detours and additional
|
||||
# laps must keep consuming the same capture and confirming the
|
||||
# same reference, even if an older run carried a distance cap.
|
||||
if boot is None or future is not None:
|
||||
continue
|
||||
# Do not freeze ahead of an already queued prefix receipt.
|
||||
@@ -548,6 +555,7 @@ def run_stationary_live(service, source, run_id, executor, clock, initialize, ca
|
||||
"initialization_policy": ROUTE_RELOCALIZATION_POLICY,
|
||||
},
|
||||
)
|
||||
search_cancel = threading.Event()
|
||||
future = executor.submit(
|
||||
initialize,
|
||||
target,
|
||||
@@ -557,6 +565,7 @@ def run_stationary_live(service, source, run_id, executor, clock, initialize, ca
|
||||
sample["path"][0],
|
||||
**({"reference_position": recovery_position} if recovering else {}),
|
||||
**({"route_only": True} if route_search_only else {}),
|
||||
cancel_event=search_cancel,
|
||||
)
|
||||
publish_phase()
|
||||
continue
|
||||
@@ -588,6 +597,7 @@ def run_stationary_live(service, source, run_id, executor, clock, initialize, ca
|
||||
sample, "fresh-validation", {"reference_window": window}
|
||||
)
|
||||
future = executor.submit(calculate, target, reference, sample["points"], hint)
|
||||
search_cancel.set()
|
||||
if boot is not None:
|
||||
boot.stop(end_reason)
|
||||
with service.lock:
|
||||
@@ -600,12 +610,7 @@ def run_stationary_live(service, source, run_id, executor, clock, initialize, ca
|
||||
tracking_state="lost",
|
||||
tracking_reason=end_reason,
|
||||
termination_reason=end_reason,
|
||||
message=(
|
||||
"Достигнут предел проверочного прохода. "
|
||||
"Запись управляется штатными кнопками сканера."
|
||||
if end_reason == "distance-limit"
|
||||
else "Исследование завершено. Запись управляется штатными кнопками сканера."
|
||||
),
|
||||
message="Исследование завершено. Запись управляется штатными кнопками сканера.",
|
||||
finished_at_utc=utc_now_iso(),
|
||||
)
|
||||
# Publish ended before waiting for an already-running bounded fit.
|
||||
@@ -627,3 +632,5 @@ def run_stationary_live(service, source, run_id, executor, clock, initialize, ca
|
||||
"Завершите проход и начните повторную проверку."
|
||||
) from exc
|
||||
raise
|
||||
finally:
|
||||
search_cancel.set()
|
||||
|
||||
@@ -0,0 +1,163 @@
|
||||
"""Explicit offline bridge from a pinned map candidate to existing planning APIs.
|
||||
|
||||
Not installed in the web composition. The original source remains the default;
|
||||
only an explicitly pinned derivative generation selects corrected geometry.
|
||||
"""
|
||||
|
||||
import shutil
|
||||
from contextlib import contextmanager
|
||||
from pathlib import Path
|
||||
from tempfile import TemporaryDirectory
|
||||
|
||||
import numpy as np
|
||||
|
||||
from k1link.reconstruction.map_version import POINTS, MapVersion
|
||||
|
||||
EXTRACTION = dict(
|
||||
version="map-version-submap/v1",
|
||||
max_frames=120,
|
||||
voxel_m=0.25,
|
||||
radius_m=20.0,
|
||||
height_relative_m=[-3.0, 6.0],
|
||||
)
|
||||
|
||||
|
||||
class VersionedPlanningSources:
|
||||
def __init__(self, original, version, scratch):
|
||||
self.original, self.version = original, version
|
||||
self.scratch = Path(scratch)
|
||||
self.scratch.mkdir(parents=True, exist_ok=True)
|
||||
|
||||
def get(self, session_id):
|
||||
# No implicit promotion and no changes to existing consumers.
|
||||
return self.original.get(session_id)
|
||||
|
||||
def bound(self, session_id, generation):
|
||||
if generation != self.version.generation:
|
||||
return self.original.bound(session_id, generation)
|
||||
parent = self.version.document["source"]
|
||||
if session_id != parent["session_id"]:
|
||||
raise ValueError("Map candidate belongs to another session.")
|
||||
source = self.original.verify(session_id, parent["generation"])
|
||||
return self.version.planning_source(source)
|
||||
|
||||
def verify(self, session_id, generation):
|
||||
if generation != self.version.generation:
|
||||
return self.original.verify(session_id, generation)
|
||||
return self.bound(session_id, generation)
|
||||
|
||||
@contextmanager
|
||||
def prepared_submaps(self, session_id, generation, *, presentation=False, cancel_event=None):
|
||||
if generation != self.version.generation:
|
||||
with self.original.prepared_submaps(
|
||||
session_id, generation, presentation=presentation, cancel_event=cancel_event
|
||||
) as extract:
|
||||
yield extract
|
||||
return
|
||||
|
||||
def cancelled():
|
||||
if cancel_event is not None and cancel_event.is_set():
|
||||
raise InterruptedError("Map version preparation cancelled.")
|
||||
|
||||
cancelled()
|
||||
doc = self.verify(session_id, generation)
|
||||
with TemporaryDirectory(prefix=".map-snapshot-", dir=self.scratch) as temporary:
|
||||
stage = Path(temporary)
|
||||
for path in self.version.directory.iterdir():
|
||||
if path.name == "manifest.json" or path.name in self.version.document["artifacts"]:
|
||||
cancelled()
|
||||
shutil.copyfile(path, stage / path.name)
|
||||
snapshot = MapVersion(stage, generation)
|
||||
snapshot.verify()
|
||||
arrays = snapshot.arrays()
|
||||
|
||||
def extract(start, end):
|
||||
cancelled()
|
||||
return _extract(snapshot, arrays, doc, start, end, presentation=presentation)
|
||||
|
||||
yield extract
|
||||
cancelled()
|
||||
# A changed parent or derivative invalidates the complete preparation.
|
||||
self.verify(session_id, generation)
|
||||
|
||||
def submap(self, session_id, generation, start, end, *, presentation=False):
|
||||
with self.prepared_submaps(session_id, generation, presentation=presentation) as extract:
|
||||
return extract(start, end)
|
||||
|
||||
def reference_map(self, session_id, generation, start, end, **options):
|
||||
from .reference_map import build_reference_map
|
||||
|
||||
return build_reference_map(self, session_id, generation, start, end, **options)
|
||||
|
||||
def scene_reference_map(self, session_id, generation, start, end, **options):
|
||||
from .reference_map import build_reference_map
|
||||
|
||||
return build_reference_map(
|
||||
self, session_id, generation, start, end, presentation=True, **options
|
||||
)
|
||||
|
||||
|
||||
def _extract(version, arrays, doc, start, end, *, presentation):
|
||||
poses, frames = arrays["positions"], arrays["frames"]
|
||||
t = arrays["receipt_time_s"]
|
||||
if not 0 <= start < end < len(poses):
|
||||
raise ValueError("Invalid map interval.")
|
||||
eligible = np.flatnonzero((frames[:, 0] >= t[start]) & (frames[:, 0] <= t[end]))
|
||||
if not len(eligible):
|
||||
raise ValueError("No cloud frames in the selected map interval.")
|
||||
selected = eligible[
|
||||
np.linspace(0, len(eligible) - 1, min(len(eligible), EXTRACTION["max_frames"]), dtype=int)
|
||||
]
|
||||
profile = {
|
||||
**EXTRACTION,
|
||||
**(dict(radius_m=80.0, height_relative_m=None) if presentation else {}),
|
||||
}
|
||||
# A frame uses the interpolated corrected sensor position only for cropping;
|
||||
# the stored cloud already is in map coordinates and is never transformed twice.
|
||||
centers = np.column_stack(
|
||||
[np.interp(frames[selected, 0], t, poses[:, axis]) for axis in range(3)]
|
||||
)
|
||||
chunks, provenance, raw_count, retained_count = [], [], 0, 0
|
||||
with (version.directory / POINTS).open("rb") as stream:
|
||||
for index, center in zip(selected, centers, strict=True):
|
||||
_, sequence, offset, count = frames[index]
|
||||
stream.seek(int(offset) * 12)
|
||||
chunk = np.frombuffer(stream.read(int(count) * 12), dtype="<f4").reshape(-1, 3)
|
||||
if len(chunk) != count or not np.isfinite(chunk).all():
|
||||
raise ValueError("Invalid map frame payload.")
|
||||
delta = chunk - center
|
||||
keep = np.linalg.norm(delta, axis=1) <= profile["radius_m"]
|
||||
if profile["height_relative_m"] is not None:
|
||||
low, high = profile["height_relative_m"]
|
||||
keep &= (delta[:, 2] >= low) & (delta[:, 2] <= high)
|
||||
raw_count += len(chunk)
|
||||
retained_count += int(keep.sum())
|
||||
chunks.append(chunk[keep])
|
||||
provenance.append(
|
||||
dict(
|
||||
frame_index=int(index),
|
||||
sequence=int(sequence),
|
||||
receipt_time_s=float(frames[index, 0]),
|
||||
source_distance_m=float(arrays["frame_source_distance_m"][index]),
|
||||
)
|
||||
)
|
||||
points = np.concatenate(chunks)
|
||||
_, indices = np.unique(
|
||||
np.floor(points / profile["voxel_m"]).astype(np.int64), axis=0, return_index=True
|
||||
)
|
||||
points = points[np.sort(indices)]
|
||||
return points, {
|
||||
**{
|
||||
key: doc[key]
|
||||
for key in ("session_id", "generation", "label", "frame_id", "units", "source_digests")
|
||||
},
|
||||
"reference_version": doc["reference_version"],
|
||||
"extraction": profile,
|
||||
"start_index": start,
|
||||
"end_index": end,
|
||||
"available_frames": len(eligible),
|
||||
"frames": provenance,
|
||||
"raw_points": raw_count,
|
||||
"retained_points": retained_count,
|
||||
"voxel_points": len(points),
|
||||
}
|
||||
@@ -0,0 +1 @@
|
||||
"""Offline, source-preserving map derivatives; no capture or vehicle authority."""
|
||||
@@ -0,0 +1,231 @@
|
||||
"""Training-only acquisition of a known start-area revisit, not live tracking.
|
||||
|
||||
The registrar is injected: this module has no device, planner, catalog or UI
|
||||
dependency. Every declared support window is evaluated before selection. No
|
||||
endpoint equality, session-name branch, or withheld-point selection is used.
|
||||
This is not arbitrary-loop discovery or a guarantee for unbounded SLAM drift.
|
||||
"""
|
||||
|
||||
from dataclasses import asdict, dataclass
|
||||
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation
|
||||
|
||||
from .smooth_correction import SurfaceLink
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ClosurePolicy:
|
||||
version: str = "known-revisit-acquisition/v2"
|
||||
reference_seconds: tuple = (10.0, 20.0, 40.0)
|
||||
query_seconds: tuple = (5.0, 10.0, 20.0, 30.0)
|
||||
primary_reference_s: float = 20.0
|
||||
primary_query_s: float = 5.0
|
||||
radius_m: float = 25.0
|
||||
cycle_m: float = 0.1
|
||||
cycle_deg: float = 0.2
|
||||
agreement_m: float = 0.5
|
||||
agreement_deg: float = 1.0
|
||||
translation_weight_m: float = 0.03
|
||||
rotation_weight_deg: float = 0.05
|
||||
|
||||
def __post_init__(self):
|
||||
values = [
|
||||
*self.reference_seconds,
|
||||
*self.query_seconds,
|
||||
self.radius_m,
|
||||
self.cycle_m,
|
||||
self.cycle_deg,
|
||||
self.agreement_m,
|
||||
self.agreement_deg,
|
||||
self.translation_weight_m,
|
||||
self.rotation_weight_deg,
|
||||
]
|
||||
if (
|
||||
not self.reference_seconds
|
||||
or not self.query_seconds
|
||||
or not np.isfinite(values).all()
|
||||
or min(values) <= 0
|
||||
or len(set(self.reference_seconds)) != len(self.reference_seconds)
|
||||
or len(set(self.query_seconds)) != len(self.query_seconds)
|
||||
or self.primary_reference_s not in self.reference_seconds
|
||||
or self.primary_query_s not in self.query_seconds
|
||||
):
|
||||
raise ValueError("Invalid closure acquisition policy.")
|
||||
|
||||
|
||||
class ClosureUnavailable(ValueError):
|
||||
def __init__(self, report):
|
||||
self.report = report
|
||||
super().__init__(report["reason"])
|
||||
|
||||
|
||||
def _apply(points, matrix):
|
||||
return points @ matrix[:3, :3].T + matrix[:3, 3]
|
||||
|
||||
|
||||
def _difference(a, b, center):
|
||||
return (
|
||||
float(np.linalg.norm(_apply(center, a) - _apply(center, b))),
|
||||
float(
|
||||
np.rad2deg(np.linalg.norm(Rotation.from_matrix(a[:3, :3].T @ b[:3, :3]).as_rotvec()))
|
||||
),
|
||||
)
|
||||
|
||||
|
||||
def acquire_closure(data, registrar, policy=None):
|
||||
"""Registrar(reference, query, seed, acquisition=bool) returns a gated fit.
|
||||
|
||||
Acquisition may allow a larger first correction. Reverse refinement MUST
|
||||
use the unchanged tracking-quality policy, starting from the inverse fit.
|
||||
Numerical exceptions are recorded as failures, never converted to identity.
|
||||
"""
|
||||
policy = policy or ClosurePolicy()
|
||||
f, p, s = data["frames"], data["poses"], data["frame_distance"]
|
||||
pts, ids, held = data["sample_points"], data["sample_frame"], data["heldout"]
|
||||
if (
|
||||
len(f) < 2
|
||||
or len(p) < 2
|
||||
or held.dtype != bool
|
||||
or held.shape != (len(f),)
|
||||
or s.shape != (len(f),)
|
||||
or ids.shape != (len(pts),)
|
||||
or ids.dtype.kind not in "iu"
|
||||
or (ids < 0).any()
|
||||
or (ids >= len(f)).any()
|
||||
or not all(np.isfinite(v).all() for v in (f, p, s, pts))
|
||||
or (np.diff(f[:, 0]) < 0).any()
|
||||
or (np.diff(s) < 0).any()
|
||||
):
|
||||
raise ValueError("Invalid closure frame ownership or chronology.")
|
||||
# One fixed spatial population in the original frame, independent of fits.
|
||||
training = ~held[ids] & (np.linalg.norm(pts - p[0, 1:4], axis=1) <= policy.radius_m)
|
||||
report = dict(
|
||||
schema_version="missioncore.closure-acquisition/v1",
|
||||
policy=asdict(policy),
|
||||
training_only=True,
|
||||
endpoint_constraint=False,
|
||||
attempts=[],
|
||||
status="rejected",
|
||||
expected_attempts=len(policy.reference_seconds) * len(policy.query_seconds),
|
||||
)
|
||||
links = {}
|
||||
for first in policy.reference_seconds:
|
||||
amask = (f[:, 0] <= f[0, 0] + first) & ~held
|
||||
a = pts[training & amask[ids]]
|
||||
for last in policy.query_seconds:
|
||||
bmask = (f[:, 0] >= f[-1, 0] - last) & ~held
|
||||
b = pts[training & bmask[ids]]
|
||||
row = dict(
|
||||
reference_s=first,
|
||||
query_s=last,
|
||||
qualified=False,
|
||||
reference_points=len(a),
|
||||
query_points=len(b),
|
||||
)
|
||||
report["attempts"].append(row)
|
||||
if np.any(amask & bmask):
|
||||
row["reason"] = "overlapping-source-windows"
|
||||
continue
|
||||
if min(len(a), len(b)) < 300:
|
||||
row["reason"] = "insufficient-training-geometry"
|
||||
continue
|
||||
try:
|
||||
fit = registrar(a, b, np.eye(4), acquisition=True)
|
||||
row["fit"] = fit
|
||||
if fit["status"] != "candidate":
|
||||
row["reason"] = "forward-quality"
|
||||
continue
|
||||
t = np.asarray(fit["T_reference_query"])
|
||||
reverse = registrar(b, a, np.linalg.inv(t), acquisition=False)
|
||||
row["reverse"] = reverse
|
||||
if reverse["status"] != "candidate":
|
||||
row["reason"] = "reverse-quality"
|
||||
continue
|
||||
center = np.median(b, axis=0)
|
||||
cycle = t @ np.asarray(reverse["T_reference_query"])
|
||||
cm, cr = _difference(cycle, np.eye(4), center)
|
||||
row.update(cycle_m=cm, cycle_deg=cr)
|
||||
if cm > policy.cycle_m or cr > policy.cycle_deg:
|
||||
row["reason"] = "bidirectional-inconsistency"
|
||||
continue
|
||||
link = SurfaceLink(
|
||||
float(np.mean(s[amask])),
|
||||
float(np.mean(s[bmask])),
|
||||
t,
|
||||
center,
|
||||
policy.translation_weight_m,
|
||||
policy.rotation_weight_deg,
|
||||
"start-area/revisit",
|
||||
)
|
||||
except (ValueError, np.linalg.LinAlgError) as exc:
|
||||
row["reason"] = "numerical-unavailable"
|
||||
row["detail"] = str(exc)
|
||||
continue
|
||||
row["qualified"] = True
|
||||
links[len(report["attempts"]) - 1] = link
|
||||
report["complete"] = len(report["attempts"]) == report["expected_attempts"]
|
||||
if not links:
|
||||
report["reason"] = "No training-only closure passed quality and reverse consistency."
|
||||
raise ClosureUnavailable(report)
|
||||
# Preserve the established short-window measurement when it qualifies.
|
||||
# Larger support is a fallback, not proof of a better measurement: mixing
|
||||
# more motion/vegetation can change an otherwise stable registration.
|
||||
# Still finish the full matrix and check competing fits before acceptance.
|
||||
selected = max(
|
||||
links,
|
||||
key=lambda i: (
|
||||
(
|
||||
report["attempts"][i]["reference_s"] == policy.primary_reference_s
|
||||
and report["attempts"][i]["query_s"] == policy.primary_query_s
|
||||
),
|
||||
report["attempts"][i]["reference_s"] * report["attempts"][i]["query_s"],
|
||||
min(report["attempts"][i]["reference_points"], report["attempts"][i]["query_points"]),
|
||||
-i,
|
||||
),
|
||||
)
|
||||
link = links[selected]
|
||||
report["selection_rule"] = "qualified-primary-else-largest-support; complete-consistency-check"
|
||||
agreement = []
|
||||
for i, other in links.items():
|
||||
# Check both patch centers; a rotation must not hide at one chosen pivot.
|
||||
distances = [
|
||||
_difference(link.T_reference_query, other.T_reference_query, c)
|
||||
for c in (link.query_center, other.query_center)
|
||||
]
|
||||
dm, deg = max(x[0] for x in distances), max(x[1] for x in distances)
|
||||
agreement.append(dict(attempt=i, distance_m=dm, angle_deg=deg))
|
||||
report.update(selected_attempt=selected, qualified_attempts=list(links), agreement=agreement)
|
||||
if any(
|
||||
r["distance_m"] > policy.agreement_m or r["angle_deg"] > policy.agreement_deg
|
||||
for r in agreement
|
||||
):
|
||||
report["reason"] = "Qualified support windows disagree; closure is ambiguous."
|
||||
raise ClosureUnavailable(report)
|
||||
report.update(status="candidate", reason=None)
|
||||
return link, report
|
||||
|
||||
|
||||
def review_acceptance(results, validation):
|
||||
"""Frozen same-source checks authorize packaging, never vehicle operation."""
|
||||
before, after = results
|
||||
seam = after["seam_holdout"]
|
||||
previous = before["seam_holdout"]
|
||||
checks = {
|
||||
"all_local_windows_qualified": validation[1]["total"] > 0
|
||||
and validation[1]["candidate_count"] == validation[1]["total"],
|
||||
"heldout_seam_present": seam["points"] >= 300 and seam["query_frames"] >= 2,
|
||||
"heldout_seam_quality": seam["overlap_05m"] >= 0.55
|
||||
and seam["inlier_rmse_m"] is not None
|
||||
and seam["inlier_rmse_m"] <= 0.25,
|
||||
"heldout_seam_not_degraded": seam["overlap_05m"] >= previous["overlap_05m"] - 0.02
|
||||
and seam["all_point_distances_m"]["median"]
|
||||
<= previous["all_point_distances_m"]["median"] + 0.02,
|
||||
}
|
||||
return dict(
|
||||
schema_version="missioncore.closure-review/v1",
|
||||
checks=checks,
|
||||
accepted=all(checks.values()),
|
||||
independent_accuracy=False,
|
||||
vehicle_control=False,
|
||||
)
|
||||
@@ -0,0 +1,271 @@
|
||||
"""Immutable, vendor-neutral map candidates. Publication is not promotion.
|
||||
|
||||
No session catalog writes or automatic latest-version selection. Readers pin a
|
||||
manifest digest, verify every artifact, and never reinterpret source traversal
|
||||
coordinates as corrected path length. No solver dependency is needed to read.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import hashlib
|
||||
import json
|
||||
import re
|
||||
import shutil
|
||||
from copy import deepcopy
|
||||
from pathlib import Path
|
||||
from tempfile import TemporaryDirectory
|
||||
|
||||
import numpy as np
|
||||
|
||||
SCHEMA = "missioncore.map-reference-version/v1"
|
||||
POINTS = "points.f32"
|
||||
TRAJECTORY = "trajectory.npz"
|
||||
AUTHORITY = dict(production_promotion=False, vehicle_control=False, independent_pass_verified=False)
|
||||
|
||||
|
||||
def sha256(path):
|
||||
with Path(path).open("rb") as stream:
|
||||
return hashlib.file_digest(stream, "sha256").hexdigest()
|
||||
|
||||
|
||||
def json_bytes(value):
|
||||
return json.dumps(value, sort_keys=True, separators=(",", ":"), allow_nan=False).encode()
|
||||
|
||||
|
||||
def _digest(value):
|
||||
if not isinstance(value, str) or not re.fullmatch(r"[a-f0-9]{64}", value):
|
||||
raise ValueError("Invalid map identity.")
|
||||
return value
|
||||
|
||||
|
||||
def _plain_file(path):
|
||||
if path.is_symlink() or not path.is_file():
|
||||
raise ValueError("Map artifact must be a regular file, not a link.")
|
||||
return path
|
||||
|
||||
|
||||
def _trajectory(path, point_count):
|
||||
with np.load(path, allow_pickle=False) as data:
|
||||
arrays = {
|
||||
key: np.array(data[key], dtype=float)
|
||||
for key in (
|
||||
"positions",
|
||||
"orientations_xyzw",
|
||||
"receipt_time_s",
|
||||
"source_distance_m",
|
||||
"frame_source_distance_m",
|
||||
"frames",
|
||||
)
|
||||
}
|
||||
p, q, t, s, fs, f = arrays.values()
|
||||
if (
|
||||
p.ndim != 2
|
||||
or p.shape[1:] != (3,)
|
||||
or len(p) < 2
|
||||
or q.shape != (len(p), 4)
|
||||
or t.shape != (len(p),)
|
||||
or s.shape != (len(p),)
|
||||
or f.ndim != 2
|
||||
or f.shape[1:] != (4,)
|
||||
or len(f) < 1
|
||||
or fs.shape != (len(f),)
|
||||
or not all(np.isfinite(a).all() for a in arrays.values())
|
||||
):
|
||||
raise ValueError("Invalid map trajectory dimensions or values.")
|
||||
if (
|
||||
(np.diff(t) <= 0).any()
|
||||
or s[0] != 0
|
||||
or (np.diff(s) < 0).any()
|
||||
or not np.allclose(np.linalg.norm(q, axis=1), 1, atol=1e-6, rtol=0)
|
||||
or (np.diff(f[:, 0]) < 0).any()
|
||||
or (np.diff(f[:, 1]) <= 0).any()
|
||||
or not np.equal(f[:, 1:], np.floor(f[:, 1:])).all()
|
||||
or (f[:, 1:] < 0).any()
|
||||
or f[0, 2] != 0
|
||||
or not np.array_equal(f[1:, 2], f[:-1, 2] + f[:-1, 3])
|
||||
or f[-1, 2] + f[-1, 3] != point_count
|
||||
or not np.allclose(fs, np.interp(f[:, 0], t, s), rtol=0, atol=1e-7)
|
||||
):
|
||||
raise ValueError("Invalid map frame ownership, clocks or source traversal binding.")
|
||||
arrays["distance_m"] = np.r_[0.0, np.cumsum(np.linalg.norm(np.diff(p, axis=0), axis=1))]
|
||||
return arrays
|
||||
|
||||
|
||||
def _source_identity(source):
|
||||
if (
|
||||
source.get("schema_version") != "missioncore.planning-source/v1"
|
||||
or source.get("reference_version")
|
||||
or source.get("units") != "m"
|
||||
or not re.fullmatch(r"[A-Za-z0-9][A-Za-z0-9._-]{0,127}", source["session_id"])
|
||||
or not source.get("source_digests")
|
||||
):
|
||||
raise ValueError("A map version requires an original, metre-based planning source.")
|
||||
return dict(
|
||||
session_id=source["session_id"],
|
||||
generation=_digest(source["generation"]),
|
||||
source_digests={k: _digest(v) for k, v in source["source_digests"].items()},
|
||||
)
|
||||
|
||||
|
||||
def publish_map_version(
|
||||
root,
|
||||
source,
|
||||
points,
|
||||
trajectory,
|
||||
*,
|
||||
expected_points_sha256,
|
||||
expected_trajectory_sha256,
|
||||
evidence,
|
||||
method,
|
||||
label,
|
||||
):
|
||||
"""Seal an already reviewed derivative in an exclusive content-addressed directory.
|
||||
|
||||
Evidence maps a simple filename to (source path, expected SHA-256). Caller
|
||||
owns scientific review and source verification; integrity is not accuracy.
|
||||
The trajectory input uses the explicit v1 keys checked by `_trajectory`.
|
||||
"""
|
||||
root = Path(root)
|
||||
root.mkdir(parents=True, exist_ok=True)
|
||||
source_id = _source_identity(source)
|
||||
if not label.strip() or not method or not evidence:
|
||||
raise ValueError("A labelled candidate requires method and review evidence.")
|
||||
inputs = {
|
||||
POINTS: (Path(points), expected_points_sha256),
|
||||
TRAJECTORY: (Path(trajectory), expected_trajectory_sha256),
|
||||
**evidence,
|
||||
}
|
||||
if len(inputs) != len(evidence) + 2:
|
||||
raise ValueError("Evidence may not replace map geometry.")
|
||||
if any(
|
||||
not re.fullmatch(r"[a-zA-Z0-9][a-zA-Z0-9._-]*", name) or name == "manifest.json"
|
||||
for name in inputs
|
||||
):
|
||||
raise ValueError("Invalid artifact name.")
|
||||
# Only this private staging directory is cleaned up on failure.
|
||||
with TemporaryDirectory(prefix=".map-version-", dir=root) as temporary:
|
||||
stage = Path(temporary)
|
||||
artifacts = {}
|
||||
for name, (path, expected) in inputs.items():
|
||||
path = _plain_file(Path(path))
|
||||
_digest(expected)
|
||||
shutil.copyfile(path, stage / name)
|
||||
if sha256(stage / name) != expected or sha256(path) != expected:
|
||||
raise ValueError("Map input changed or failed its expected digest: " + name)
|
||||
artifacts[name] = dict(sha256=expected, bytes=(stage / name).stat().st_size)
|
||||
size = artifacts[POINTS]["bytes"]
|
||||
if not size or size % 12:
|
||||
raise ValueError("Map points must be a nonempty little-endian Nx3 float32 array.")
|
||||
arrays = _trajectory(stage / TRAJECTORY, size // 12)
|
||||
if len(arrays["positions"]) != len(source["poses"]):
|
||||
raise ValueError("Map pose indices must preserve original source ownership.")
|
||||
if not np.allclose(
|
||||
arrays["source_distance_m"],
|
||||
[p["distance_m"] for p in source["poses"]],
|
||||
rtol=0,
|
||||
atol=1e-7,
|
||||
):
|
||||
raise ValueError("Map source traversal does not match the selected recording.")
|
||||
# Chunked finiteness check; never allocate the full cloud twice.
|
||||
with (stage / POINTS).open("rb") as stream:
|
||||
while block := stream.read(12 * 65536):
|
||||
if not np.isfinite(np.frombuffer(block, dtype="<f4")).all():
|
||||
raise ValueError("Map contains non-finite points.")
|
||||
doc = dict(
|
||||
schema_version=SCHEMA,
|
||||
source=source_id,
|
||||
label=label.strip(),
|
||||
units="m",
|
||||
kind="corrected-map-candidate",
|
||||
method=method,
|
||||
authority=AUTHORITY,
|
||||
point_count=size // 12,
|
||||
pose_count=len(arrays["positions"]),
|
||||
frame_count=len(arrays["frames"]),
|
||||
path_m=float(arrays["distance_m"][-1]),
|
||||
artifacts=artifacts,
|
||||
)
|
||||
payload = json_bytes(doc)
|
||||
generation = hashlib.sha256(payload).hexdigest()
|
||||
target = root / generation
|
||||
(stage / "manifest.json").write_bytes(payload)
|
||||
if target.exists():
|
||||
MapVersion(target, generation).verify()
|
||||
else:
|
||||
# Renaming a complete directory makes partial candidates undiscoverable.
|
||||
stage.rename(target)
|
||||
return MapVersion(target, generation)
|
||||
|
||||
|
||||
class MapVersion:
|
||||
def __init__(self, directory, generation):
|
||||
self.directory = Path(directory)
|
||||
self.generation = _digest(generation)
|
||||
self.document = self._manifest()
|
||||
|
||||
def _manifest(self):
|
||||
path = _plain_file(self.directory / "manifest.json")
|
||||
payload = path.read_bytes()
|
||||
if hashlib.sha256(payload).hexdigest() != self.generation:
|
||||
raise ValueError("Map manifest identity changed.")
|
||||
doc = json.loads(payload)
|
||||
if (
|
||||
doc.get("schema_version") != SCHEMA
|
||||
or doc.get("units") != "m"
|
||||
or doc.get("authority") != AUTHORITY
|
||||
or doc.get("kind") != "corrected-map-candidate"
|
||||
or not {POINTS, TRAJECTORY}.issubset(doc.get("artifacts", {}))
|
||||
):
|
||||
raise ValueError("Unsupported map version contract or authority.")
|
||||
for name, artifact in doc["artifacts"].items():
|
||||
if (
|
||||
not re.fullmatch(r"[a-zA-Z0-9][a-zA-Z0-9._-]*", name)
|
||||
or name == "manifest.json"
|
||||
or type(artifact["bytes"]) is not int
|
||||
or artifact["bytes"] < 0
|
||||
):
|
||||
raise ValueError("Invalid map artifact metadata.")
|
||||
_digest(artifact["sha256"])
|
||||
return doc
|
||||
|
||||
def verify(self):
|
||||
doc = self._manifest()
|
||||
for name, meta in doc["artifacts"].items():
|
||||
path = _plain_file(self.directory / name)
|
||||
if path.stat().st_size != meta["bytes"] or sha256(path) != meta["sha256"]:
|
||||
raise ValueError("Map artifact identity changed: " + name)
|
||||
return doc
|
||||
|
||||
def planning_source(self, original):
|
||||
original = deepcopy(original)
|
||||
doc = self.verify()
|
||||
if _source_identity(original) != doc["source"]:
|
||||
raise ValueError("Map version belongs to another source generation.")
|
||||
arrays = _trajectory(self.directory / TRAJECTORY, doc["point_count"])
|
||||
self.verify()
|
||||
if len(original["poses"]) != len(arrays["positions"]):
|
||||
raise ValueError("Map and original pose ownership differ.")
|
||||
poses = [
|
||||
{**pose, "position": xyz.tolist(), "distance_m": float(distance)}
|
||||
for pose, xyz, distance in zip(
|
||||
original["poses"], arrays["positions"], arrays["distance_m"], strict=True
|
||||
)
|
||||
]
|
||||
return {
|
||||
**original,
|
||||
"poses": poses,
|
||||
"generation": self.generation,
|
||||
"frame_id": "map/" + self.generation,
|
||||
"label": doc["label"],
|
||||
"path_m": float(arrays["distance_m"][-1]),
|
||||
"reference_version": dict(
|
||||
schema_version=SCHEMA,
|
||||
source=doc["source"],
|
||||
authority=doc["authority"],
|
||||
method=doc["method"],
|
||||
),
|
||||
}
|
||||
|
||||
def arrays(self):
|
||||
"""Only use within a verified private snapshot for a multi-tile preparation."""
|
||||
return _trajectory(self.directory / TRAJECTORY, self.document["point_count"])
|
||||
@@ -0,0 +1,58 @@
|
||||
"""Read-only projection of complete, sealed per-frame map geometry.
|
||||
|
||||
The capture clock stays untouched. Map receipt times are relative to the first
|
||||
raw message, NOT to recording start or the first pose. No fitting occurs here.
|
||||
"""
|
||||
|
||||
import hashlib
|
||||
import json
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .map_version import POINTS, TRAJECTORY, _trajectory, sha256
|
||||
|
||||
|
||||
class RecordedMapGeometry:
|
||||
def __init__(self, artifacts):
|
||||
manifest = Path(artifacts["map-version-manifest.json"])
|
||||
self.generation = sha256(manifest)
|
||||
self.document = json.loads(manifest.read_text())
|
||||
paths = {name: Path(artifacts["map-version-" + name]) for name in (POINTS, TRAJECTORY)}
|
||||
for name, path in paths.items():
|
||||
meta = self.document["artifacts"][name]
|
||||
if path.stat().st_size != meta["bytes"] or sha256(path) != meta["sha256"]:
|
||||
raise ValueError("Corrected recording geometry failed integrity validation.")
|
||||
self.arrays = _trajectory(paths[TRAJECTORY], self.document["point_count"])
|
||||
self._points = np.memmap(paths[POINTS], dtype="<f4", mode="r").reshape(-1, 3)
|
||||
|
||||
@classmethod
|
||||
def optional(cls, artifacts):
|
||||
if not artifacts or not any(key.startswith("map-version-") for key in artifacts):
|
||||
return None
|
||||
return cls(artifacts)
|
||||
|
||||
def recording_id(self, original_id):
|
||||
return hashlib.sha256((original_id + ":map:" + self.generation).encode()).hexdigest()
|
||||
|
||||
def points(self, index, receipt_time_s, count):
|
||||
t, _sequence, offset, length = self.arrays["frames"][index]
|
||||
self._check_time(t, receipt_time_s)
|
||||
if length != count:
|
||||
raise ValueError("Corrected cloud point ownership mismatch.")
|
||||
return self._points[int(offset) : int(offset + length)]
|
||||
|
||||
def pose(self, index, receipt_time_s):
|
||||
self._check_time(self.arrays["receipt_time_s"][index], receipt_time_s)
|
||||
return tuple(self.arrays["positions"][index]), tuple(
|
||||
self.arrays["orientations_xyzw"][index]
|
||||
)
|
||||
|
||||
def complete(self, point_frames, poses):
|
||||
if point_frames != len(self.arrays["frames"]) or poses != len(self.arrays["positions"]):
|
||||
raise ValueError("Corrected recording does not cover all native frames.")
|
||||
|
||||
@staticmethod
|
||||
def _check_time(expected, actual):
|
||||
if not np.isfinite(actual) or abs(expected - actual) > 1e-6:
|
||||
raise ValueError("Corrected frame receipt clock mismatch.")
|
||||
@@ -0,0 +1,118 @@
|
||||
"""One explicit default map per physical session, shared by playback and planning.
|
||||
|
||||
Candidate publication is not activation. Activation copies an immutable reviewed
|
||||
bundle into durable application storage, then replaces a small selection pointer.
|
||||
This admits laboratory reference use, not autonomous vehicle control.
|
||||
"""
|
||||
|
||||
import json
|
||||
import os
|
||||
import re
|
||||
import shutil
|
||||
from dataclasses import replace
|
||||
from pathlib import Path
|
||||
from tempfile import TemporaryDirectory
|
||||
from uuid import uuid4
|
||||
|
||||
from .map_version import MapVersion, _digest, json_bytes
|
||||
|
||||
SCHEMA = "missioncore.session-map-default/v1"
|
||||
|
||||
|
||||
class SessionMapVersions:
|
||||
def __init__(self, data_dir):
|
||||
self.root = Path(data_dir) / "session-map-versions"
|
||||
|
||||
def _selection(self, session_id):
|
||||
if not re.fullmatch(r"[A-Za-z0-9][A-Za-z0-9._-]{0,127}", session_id):
|
||||
raise ValueError("Invalid session identity.")
|
||||
return self.root / "selected" / (session_id + ".json")
|
||||
|
||||
def version(self, session_id, generation):
|
||||
directory = self.root / "versions" / _digest(generation)
|
||||
if not directory.exists():
|
||||
return None
|
||||
if directory.is_symlink():
|
||||
raise ValueError("Map version directory may not be a link.")
|
||||
version = MapVersion(directory, generation)
|
||||
if version.document["source"]["session_id"] != session_id:
|
||||
raise ValueError("Map belongs to another session.")
|
||||
return version
|
||||
|
||||
def selected(self, session_id):
|
||||
path = self._selection(session_id)
|
||||
if not path.exists() and not path.is_symlink():
|
||||
return None
|
||||
if path.is_symlink() or path.stat().st_size > 8192:
|
||||
raise ValueError("Invalid map selection.")
|
||||
doc = json.loads(path.read_text())
|
||||
if doc.get("schema_version") != SCHEMA or doc.get("session_id") != session_id:
|
||||
raise ValueError("Invalid map selection identity.")
|
||||
version = self.version(session_id, doc["generation"])
|
||||
if version is None:
|
||||
raise ValueError("Selected corrected map is unavailable; original was not substituted.")
|
||||
return version
|
||||
|
||||
def activate(self, version, original_sources):
|
||||
"""Explicit operator admission; keep original captures and old map versions."""
|
||||
doc = version.verify()
|
||||
session_id = doc["source"]["session_id"]
|
||||
parent = original_sources.verify(session_id, doc["source"]["generation"])
|
||||
version.planning_source(parent)
|
||||
root = self.root / "versions"
|
||||
root.mkdir(parents=True, exist_ok=True)
|
||||
target = root / version.generation
|
||||
if not target.exists():
|
||||
with TemporaryDirectory(prefix=".admit-", dir=root) as temporary:
|
||||
stage = Path(temporary) / version.generation
|
||||
stage.mkdir()
|
||||
for name in ["manifest.json", *doc["artifacts"]]:
|
||||
shutil.copyfile(version.directory / name, stage / name)
|
||||
with (stage / name).open("rb") as stream:
|
||||
os.fsync(stream.fileno())
|
||||
MapVersion(stage, version.generation).verify()
|
||||
stage.rename(target)
|
||||
_sync_directory(root)
|
||||
self.version(session_id, version.generation).verify()
|
||||
original_sources.verify(session_id, doc["source"]["generation"])
|
||||
path = self._selection(session_id)
|
||||
path.parent.mkdir(parents=True, exist_ok=True)
|
||||
candidate = path.with_name("." + uuid4().hex + ".json")
|
||||
try:
|
||||
candidate.write_bytes(
|
||||
json_bytes(
|
||||
dict(
|
||||
schema_version=SCHEMA,
|
||||
session_id=session_id,
|
||||
generation=version.generation,
|
||||
uses=["recorded-playback", "laboratory-reference"],
|
||||
vehicle_control=False,
|
||||
)
|
||||
)
|
||||
)
|
||||
with candidate.open("rb") as stream:
|
||||
os.fsync(stream.fileno())
|
||||
os.replace(candidate, path)
|
||||
_sync_directory(path.parent)
|
||||
finally:
|
||||
candidate.unlink(missing_ok=True)
|
||||
return self.selected(session_id)
|
||||
|
||||
def resolve_replay(self, command):
|
||||
from k1link.sessions.models import ReplayMapVersion
|
||||
|
||||
# An already pinned launch remains pinned even if the default changes.
|
||||
if command.map_version is not None:
|
||||
return command
|
||||
version = self.selected(command.session_id)
|
||||
if version is None:
|
||||
return command
|
||||
return replace(command, map_version=ReplayMapVersion(version.directory, version.generation))
|
||||
|
||||
|
||||
def _sync_directory(path):
|
||||
descriptor = os.open(path, os.O_RDONLY)
|
||||
try:
|
||||
os.fsync(descriptor)
|
||||
finally:
|
||||
os.close(descriptor)
|
||||
@@ -0,0 +1,239 @@
|
||||
"""Experimental smooth correction of already mapped, provenance-bearing frames.
|
||||
|
||||
This is NOT a replacement LiDAR odometer or an automatic loop detector. A caller
|
||||
must supply independently checked registrations between disjoint source windows.
|
||||
The unknown C(s) maps original-map coordinates into a corrected map. Its six
|
||||
parameters are cubic splines over traveled distance: translation and a rotation
|
||||
vector. Every individual frame receives ONE rigid C(s), preserving its geometry.
|
||||
The original map/trajectory is never mutated. The first knot fixes the gauge;
|
||||
no constraint equates the first and last scanner positions.
|
||||
|
||||
Weights below are declared engineering regularizers, NOT calibrated covariance.
|
||||
The small-rotation chart is appropriate for the measured first experiment; this
|
||||
is not a qualified solution for arbitrary large drift or an arbitrary loop graph.
|
||||
"""
|
||||
|
||||
from dataclasses import asdict, dataclass
|
||||
|
||||
import numpy as np
|
||||
from scipy.interpolate import CubicSpline
|
||||
from scipy.optimize import least_squares
|
||||
from scipy.spatial.transform import Rotation
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class CorrectionPolicy:
|
||||
version: str = "smooth-map-correction-experiment/v2"
|
||||
knot_spacing_m: float = 20.0
|
||||
smoothness_length_m: float = 30.0
|
||||
rotation_lever_m: float = 20.0
|
||||
strain_weight: float = 1.0
|
||||
maximum_evaluations: int = 100
|
||||
|
||||
def __post_init__(self):
|
||||
values = [
|
||||
self.knot_spacing_m,
|
||||
self.smoothness_length_m,
|
||||
self.rotation_lever_m,
|
||||
self.strain_weight,
|
||||
self.maximum_evaluations,
|
||||
]
|
||||
if not np.isfinite(values).all() or min(values) <= 0:
|
||||
raise ValueError("Correction policy values must be finite and positive.")
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class SurfaceLink:
|
||||
reference_distance_m: float
|
||||
query_distance_m: float
|
||||
T_reference_query: np.ndarray
|
||||
query_center: np.ndarray
|
||||
translation_weight_m: float
|
||||
rotation_weight_deg: float
|
||||
identity: str
|
||||
|
||||
def __post_init__(self):
|
||||
t = np.asarray(self.T_reference_query, dtype=float)
|
||||
c = np.asarray(self.query_center, dtype=float)
|
||||
if (
|
||||
t.shape != (4, 4)
|
||||
or not np.isfinite(t).all()
|
||||
or not np.allclose(t[3], [0, 0, 0, 1])
|
||||
or not np.allclose(t[:3, :3].T @ t[:3, :3], np.eye(3), atol=1e-7)
|
||||
or not np.isclose(np.linalg.det(t[:3, :3]), 1)
|
||||
or c.shape != (3,)
|
||||
or not np.isfinite(c).all()
|
||||
):
|
||||
raise ValueError("A surface link requires a rigid transform and finite center.")
|
||||
values = [
|
||||
self.reference_distance_m,
|
||||
self.query_distance_m,
|
||||
self.translation_weight_m,
|
||||
self.rotation_weight_deg,
|
||||
]
|
||||
if (
|
||||
not np.isfinite(values).all()
|
||||
or min(values[:2]) < 0
|
||||
or self.reference_distance_m == self.query_distance_m
|
||||
or min(values[2:]) <= 0
|
||||
or not self.identity
|
||||
):
|
||||
raise ValueError("Invalid link distances, weights or identity.")
|
||||
t, c = t.copy(), c.copy()
|
||||
t.setflags(write=False)
|
||||
c.setflags(write=False)
|
||||
object.__setattr__(self, "T_reference_query", t)
|
||||
object.__setattr__(self, "query_center", c)
|
||||
|
||||
|
||||
class CorrectionField:
|
||||
def __init__(self, knots, parameters, origin=None):
|
||||
self.origin = np.asarray(np.zeros(3) if origin is None else origin, dtype=float).copy()
|
||||
if self.origin.shape != (3,) or not np.isfinite(self.origin).all():
|
||||
raise ValueError("Correction origin must be a finite 3D point.")
|
||||
self.knots = np.asarray(knots, dtype=float).copy()
|
||||
self.parameters = np.asarray(parameters, dtype=float).copy()
|
||||
if (
|
||||
self.knots.ndim != 1
|
||||
or len(self.knots) < 2
|
||||
or not np.isfinite(self.knots).all()
|
||||
or (np.diff(self.knots) <= 0).any()
|
||||
or self.parameters.shape != (len(self.knots), 6)
|
||||
or not np.isfinite(self.parameters).all()
|
||||
):
|
||||
raise ValueError("Invalid correction knots or parameters.")
|
||||
self.spline = CubicSpline(self.knots, self.parameters, bc_type="natural")
|
||||
|
||||
def matrices(self, distance):
|
||||
s = np.atleast_1d(np.asarray(distance, dtype=float))
|
||||
if (
|
||||
s.ndim != 1
|
||||
or not np.isfinite(s).all()
|
||||
or (s < self.knots[0] - 1e-8).any()
|
||||
or (s > self.knots[-1] + 1e-8).any()
|
||||
):
|
||||
raise ValueError("Correction cannot extrapolate beyond its source route.")
|
||||
p = self.spline(np.clip(s, self.knots[0], self.knots[-1]))
|
||||
# Avoid silently wrapping the rotation-vector chart through pi.
|
||||
if (np.linalg.norm(p[:, 3:], axis=1) >= np.pi / 2).any():
|
||||
raise ValueError("Correction exceeds the experimental small-rotation chart.")
|
||||
result = np.repeat(np.eye(4)[None], len(s), axis=0)
|
||||
result[:, :3, :3] = Rotation.from_rotvec(p[:, 3:]).as_matrix()
|
||||
result[:, :3, 3] = (
|
||||
p[:, :3] + self.origin - np.einsum("nij,j->ni", result[:, :3, :3], self.origin)
|
||||
)
|
||||
return result
|
||||
|
||||
def points(self, points, distance):
|
||||
p = np.asarray(points, dtype=float)
|
||||
if p.ndim != 2 or p.shape[1] != 3 or not np.isfinite(p).all():
|
||||
raise ValueError("Expected finite Nx3 points.")
|
||||
c = self.matrices(distance)
|
||||
if len(c) not in (1, len(p)):
|
||||
raise ValueError("One correction per frame or per point is required.")
|
||||
return np.einsum("nij,nj->ni", c[:, :3, :3], p) + c[:, :3, 3]
|
||||
|
||||
def poses(self, positions, orientations_xyzw, distance):
|
||||
c = self.matrices(distance)
|
||||
q = np.asarray(orientations_xyzw, dtype=float)
|
||||
if q.shape != (len(c), 4) or not np.isfinite(q).all():
|
||||
raise ValueError("Pose orientations must match correction coordinates.")
|
||||
return (
|
||||
self.points(positions, distance),
|
||||
(Rotation.from_matrix(c[:, :3, :3]) * Rotation.from_quat(q)).as_quat(),
|
||||
)
|
||||
|
||||
|
||||
def fit_correction(length_m, links, policy=None):
|
||||
"""Fit one separately reviewed candidate; absence of constraints is an error."""
|
||||
policy = policy or CorrectionPolicy()
|
||||
if not np.isfinite(length_m) or length_m <= 0 or not links:
|
||||
raise ValueError("A positive route length and verified links are required.")
|
||||
if any(max(e.reference_distance_m, e.query_distance_m) > length_m for e in links):
|
||||
raise ValueError("Surface link lies outside the source route.")
|
||||
knots = np.linspace(0, length_m, max(2, int(np.ceil(length_m / policy.knot_spacing_m)) + 1))
|
||||
# Three-point quadrature exactly integrates the squared cubic derivatives.
|
||||
h = np.diff(knots)
|
||||
mid = (knots[:-1] + knots[1:]) / 2
|
||||
abscissa, weight = np.polynomial.legendre.leggauss(3)
|
||||
grid = (mid[:, None] + h[:, None] * abscissa / 2).ravel()
|
||||
root_weight = np.sqrt((h[:, None] * weight / 2).ravel())[:, None]
|
||||
unit_scale = np.array([1, 1, 1, *([policy.rotation_lever_m] * 3)])
|
||||
edge_s = np.array([[e.reference_distance_m, e.query_distance_m] for e in links])
|
||||
measured_r = Rotation.from_matrix(np.stack([e.T_reference_query[:3, :3] for e in links]))
|
||||
centers = np.stack([e.query_center for e in links])
|
||||
destinations = np.stack(
|
||||
[e.T_reference_query[:3, :3] @ e.query_center + e.T_reference_query[:3, 3] for e in links]
|
||||
)
|
||||
# Geometry-bound pivot: a change of world origin must not change regularization.
|
||||
origin = destinations[0].copy()
|
||||
centers, destinations = centers - origin, destinations - origin
|
||||
tw = np.array([e.translation_weight_m for e in links])[:, None]
|
||||
rw = np.deg2rad([e.rotation_weight_deg for e in links])[:, None]
|
||||
|
||||
def field_of(x):
|
||||
return CorrectionField(knots, np.vstack([np.zeros(6), x.reshape(-1, 6)]), origin)
|
||||
|
||||
def residual(x):
|
||||
field = field_of(x)
|
||||
values = field.spline(edge_s)
|
||||
a = Rotation.from_rotvec(values[:, 0, 3:])
|
||||
b = Rotation.from_rotvec(values[:, 1, 3:])
|
||||
displacement = (
|
||||
b.apply(centers) + values[:, 1, :3] - a.apply(destinations) - values[:, 0, :3]
|
||||
) / tw
|
||||
angular = ((a * measured_r).inv() * b).as_rotvec() / rw
|
||||
first = field.spline(grid, 1) * unit_scale * root_weight * policy.strain_weight
|
||||
second = (
|
||||
field.spline(grid, 2)
|
||||
* unit_scale
|
||||
* root_weight
|
||||
* policy.strain_weight
|
||||
* policy.smoothness_length_m
|
||||
)
|
||||
return np.r_[displacement.ravel(), angular.ravel(), first.ravel(), second.ravel()]
|
||||
|
||||
result = least_squares(
|
||||
residual,
|
||||
np.zeros((len(knots) - 1) * 6),
|
||||
max_nfev=policy.maximum_evaluations,
|
||||
x_scale="jac",
|
||||
ftol=1e-8,
|
||||
xtol=1e-8,
|
||||
gtol=1e-8,
|
||||
)
|
||||
field = field_of(result.x)
|
||||
# Check interpolation, not just control points, for forbidden rotations.
|
||||
field.matrices(np.linspace(0, length_m, max(100, len(knots) * 10)))
|
||||
remaining = residual(result.x)[: len(links) * 6]
|
||||
return field, {
|
||||
"schema_version": "missioncore.smooth-map-correction/v2",
|
||||
"origin_m": origin.tolist(),
|
||||
"policy": asdict(policy),
|
||||
"converged": bool(result.success),
|
||||
"solver_message": str(result.message),
|
||||
"evaluations": int(result.nfev),
|
||||
"cost": float(result.cost),
|
||||
"optimality": float(result.optimality),
|
||||
"knots_m": knots.tolist(),
|
||||
"parameters": field.parameters.tolist(),
|
||||
"link_residuals": [
|
||||
dict(
|
||||
identity=e.identity,
|
||||
translation_m=float(np.linalg.norm(remaining[i * 3 : i * 3 + 3]) * tw[i, 0]),
|
||||
rotation_deg=float(
|
||||
np.rad2deg(
|
||||
np.linalg.norm(
|
||||
remaining[len(links) * 3 + i * 3 : len(links) * 3 + i * 3 + 3]
|
||||
)
|
||||
* rw[i, 0]
|
||||
)
|
||||
),
|
||||
)
|
||||
for i, e in enumerate(links)
|
||||
],
|
||||
"weights_are_calibrated_covariances": False,
|
||||
"status": "candidate" if result.success else "solver-failed",
|
||||
"production_promotion": False,
|
||||
"vehicle_control": False,
|
||||
}
|
||||
@@ -0,0 +1,40 @@
|
||||
"""Mutable per-recording presentation metadata, separate from sealed evidence."""
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import os
|
||||
import tempfile
|
||||
from pathlib import Path
|
||||
|
||||
SCHEMA = "missioncore.session-display-profile/v1"
|
||||
|
||||
|
||||
def load_display_profile(root: Path, session_id: str) -> dict | None:
|
||||
path = root / "display-profile.json"
|
||||
if not path.exists():
|
||||
return None
|
||||
if path.is_symlink() or not path.is_file() or path.stat().st_size > 64 * 1024:
|
||||
raise ValueError("invalid display profile file")
|
||||
document = json.loads(path.read_text(encoding="utf-8"))
|
||||
if (not isinstance(document, dict) or document.get("schema_version") != SCHEMA
|
||||
or document.get("session_id") != session_id):
|
||||
raise ValueError("invalid display profile identity")
|
||||
return document
|
||||
|
||||
|
||||
def save_display_profile(root: Path, session_id: str, settings: dict) -> dict:
|
||||
destination = root / "display-profile.json"
|
||||
if destination.is_symlink():
|
||||
raise ValueError("invalid display profile file")
|
||||
document = {"schema_version": SCHEMA, "session_id": session_id, "scene_settings": settings}
|
||||
descriptor, temporary = tempfile.mkstemp(prefix=".display-profile-", dir=root)
|
||||
try:
|
||||
with os.fdopen(descriptor, "w", encoding="utf-8") as stream:
|
||||
json.dump(document, stream, ensure_ascii=False, allow_nan=False)
|
||||
stream.write("\n")
|
||||
stream.flush()
|
||||
os.fsync(stream.fileno())
|
||||
os.replace(temporary, destination)
|
||||
finally:
|
||||
Path(temporary).unlink(missing_ok=True)
|
||||
return document
|
||||
@@ -0,0 +1,80 @@
|
||||
"""Validate a pinned geometry projection without widening native source roots."""
|
||||
|
||||
from k1link.reconstruction.map_version import POINTS, TRAJECTORY, MapVersion
|
||||
|
||||
|
||||
class MapReplaySessionStore:
|
||||
"""Operator playback facade. Catalog, media and raw evidence stay in the store.
|
||||
|
||||
Scientific/native consumers receive the original store, not this facade.
|
||||
"""
|
||||
|
||||
def __init__(self, original, versions):
|
||||
self.original, self.versions = original, versions
|
||||
|
||||
def __getattr__(self, name):
|
||||
return getattr(self.original, name)
|
||||
|
||||
def prepare_replay(self, *args, **kwargs):
|
||||
from .models import SessionIntegrityError
|
||||
from .recording import RecordingMaterializationError, _validate_source
|
||||
|
||||
try:
|
||||
command = self.versions.resolve_replay(self.original.prepare_replay(*args, **kwargs))
|
||||
if command.map_version is not None:
|
||||
_validate_source(command)
|
||||
return command
|
||||
except (OSError, ValueError, RecordingMaterializationError) as exc:
|
||||
raise SessionIntegrityError(
|
||||
"Исправленная версия записи недоступна или изменилась."
|
||||
) from exc
|
||||
|
||||
|
||||
def map_recording_inputs(command, source):
|
||||
from .recording import (
|
||||
RecordingMaterializationError,
|
||||
_regular_file_stat_nofollow,
|
||||
_sha256_stable,
|
||||
_validated_artifact_digests,
|
||||
_ValidatedArtifact,
|
||||
)
|
||||
|
||||
binding = command.map_version
|
||||
if binding is None:
|
||||
return ()
|
||||
try:
|
||||
if binding.directory.is_symlink() or binding.directory.name != binding.generation:
|
||||
raise ValueError("Unsafe map directory.")
|
||||
version = MapVersion(binding.directory, binding.generation)
|
||||
parent = version.document["source"]
|
||||
if parent["session_id"] != command.session_id or parent[
|
||||
"source_digests"
|
||||
] != _validated_artifact_digests(source):
|
||||
raise ValueError("Map source identity does not match the recording.")
|
||||
artifacts = []
|
||||
names = {
|
||||
"manifest.json": binding.generation,
|
||||
**{
|
||||
name: version.document["artifacts"][name]["sha256"] for name in (POINTS, TRAJECTORY)
|
||||
},
|
||||
}
|
||||
for name, expected in names.items():
|
||||
path = binding.directory / name
|
||||
stat = _regular_file_stat_nofollow(path, "map recording input")
|
||||
if _sha256_stable(path, stat) != expected:
|
||||
raise ValueError("Map recording artifact changed.")
|
||||
artifacts.append(
|
||||
_ValidatedArtifact(
|
||||
"map-version-" + name,
|
||||
path,
|
||||
"application/octet-stream",
|
||||
stat,
|
||||
stat.st_size,
|
||||
expected,
|
||||
)
|
||||
)
|
||||
return tuple(artifacts)
|
||||
except (OSError, ValueError, KeyError) as exc:
|
||||
raise RecordingMaterializationError(
|
||||
"Исправленная версия записи недоступна или изменилась."
|
||||
) from exc
|
||||
@@ -282,6 +282,14 @@ class ReplayArtifact:
|
||||
expected_sha256: str | None
|
||||
|
||||
|
||||
@dataclass(frozen=True, slots=True)
|
||||
class ReplayMapVersion:
|
||||
"""Pinned derived geometry; never expands the native capture confinement."""
|
||||
|
||||
directory: Path
|
||||
generation: str
|
||||
|
||||
|
||||
@dataclass(frozen=True, slots=True)
|
||||
class ReplayCommand:
|
||||
"""Internal replay request containing no vendor format or channel names."""
|
||||
@@ -296,6 +304,7 @@ class ReplayCommand:
|
||||
timeline_origin_monotonic_ns: int
|
||||
speed: float
|
||||
loop: bool
|
||||
map_version: ReplayMapVersion | None = None
|
||||
|
||||
@property
|
||||
def primary_artifact(self) -> ReplayArtifact:
|
||||
|
||||
+111
-40
@@ -1,4 +1,5 @@
|
||||
"""On-demand, source-bound overview cache. No catalog-wide decoding."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import hashlib
|
||||
@@ -7,26 +8,34 @@ import logging
|
||||
import os
|
||||
import shutil
|
||||
import threading
|
||||
from collections.abc import Mapping
|
||||
from concurrent.futures import ThreadPoolExecutor
|
||||
from pathlib import Path
|
||||
from typing import Mapping
|
||||
from uuid import uuid4
|
||||
|
||||
from .store import SessionStore
|
||||
from .overview_comparison import load_comparison
|
||||
from .plugin_contract import RecordingExporter
|
||||
from .recording import (_validate_source, _validate_source_state,
|
||||
_validated_artifact_digests, _stage_replay_prefix)
|
||||
from .recording import (
|
||||
_stage_replay_prefix,
|
||||
_validate_source,
|
||||
_validate_source_state,
|
||||
_validated_artifact_digests,
|
||||
)
|
||||
from .store import SessionStore
|
||||
|
||||
SCHEMA = 'missioncore.session-overview/v1'
|
||||
SCHEMA = "missioncore.session-overview/v1"
|
||||
|
||||
|
||||
class SessionOverviewService:
|
||||
def __init__(self, store: SessionStore, exporters: Mapping[str, RecordingExporter]):
|
||||
def __init__(
|
||||
self, store: SessionStore, exporters: Mapping[str, RecordingExporter], *, map_versions=None
|
||||
):
|
||||
self.store = store
|
||||
self.map_versions = map_versions
|
||||
self.exporters = exporters
|
||||
self.root = store.data_dir / 'session-overviews'
|
||||
self.root = store.data_dir / "session-overviews"
|
||||
self.root.mkdir(parents=True, exist_ok=True)
|
||||
self.executor = ThreadPoolExecutor(max_workers=1, thread_name_prefix='session-overview')
|
||||
self.executor = ThreadPoolExecutor(max_workers=1, thread_name_prefix="session-overview")
|
||||
self.guard = threading.RLock()
|
||||
self.cancel = threading.Event()
|
||||
self.jobs: dict[str, dict] = {}
|
||||
@@ -37,25 +46,35 @@ class SessionOverviewService:
|
||||
|
||||
def get(self, session_id: str, *, start: bool = True) -> dict:
|
||||
detail = self.store.get_session(session_id)
|
||||
base = {'schema_version': SCHEMA, 'session': detail.as_dict()}
|
||||
base = {"schema_version": SCHEMA, "session": detail.as_dict()}
|
||||
exporter = self.exporters.get(detail.plugin_id)
|
||||
if not detail.summary.replayable or detail.summary.lab is not None or exporter is None:
|
||||
return {**base, 'state': 'ready', 'metrics': None, 'scene_url': None}
|
||||
return {**base, "state": "ready", "metrics": None, "scene_url": None}
|
||||
command = self.store.prepare_replay(session_id)
|
||||
source = _validate_source(command)
|
||||
identity = hashlib.sha256(json.dumps([SCHEMA, session_id, source.identity], default=str).encode()).hexdigest()
|
||||
identity = hashlib.sha256(
|
||||
json.dumps([SCHEMA, session_id, source.identity], default=str).encode()
|
||||
).hexdigest()
|
||||
directory = self.root / identity
|
||||
cached = self._cached(directory)
|
||||
if cached:
|
||||
return {**base, **cached, 'generation': identity, 'scene_url': f'/api/v1/observation-sessions/{session_id}/overview/scene.rrd?generation={identity}'}
|
||||
return {
|
||||
**base,
|
||||
**cached,
|
||||
"generation": identity,
|
||||
"scene_url": (
|
||||
f"/api/v1/observation-sessions/{session_id}/overview/scene.rrd"
|
||||
f"?generation={identity}"
|
||||
),
|
||||
}
|
||||
with self.guard:
|
||||
if identity in self.jobs:
|
||||
return {**base, **self.jobs[identity]}
|
||||
if not start:
|
||||
return {**base, 'state': 'missing'}
|
||||
if sum(j['state'] in {'queued', 'preparing'} for j in self.jobs.values()) >= 8:
|
||||
return {**base, 'state': 'error', 'message': 'Подготовка занята. Повторите позже.'}
|
||||
self.jobs[identity] = {'state': 'queued', 'messages_processed': 0}
|
||||
return {**base, "state": "missing"}
|
||||
if sum(j["state"] in {"queued", "preparing"} for j in self.jobs.values()) >= 8:
|
||||
return {**base, "state": "error", "message": "Подготовка занята. Повторите позже."}
|
||||
self.jobs[identity] = {"state": "queued", "messages_processed": 0}
|
||||
self.executor.submit(self._build, identity, source, exporter)
|
||||
return {**base, **self.jobs[identity]}
|
||||
|
||||
@@ -63,65 +82,117 @@ class SessionOverviewService:
|
||||
detail = self.store.get_session(session_id)
|
||||
if detail.summary.replayable and detail.summary.lab is None:
|
||||
source = _validate_source(self.store.prepare_replay(session_id))
|
||||
identity = hashlib.sha256(json.dumps([SCHEMA, session_id, source.identity], default=str).encode()).hexdigest()
|
||||
identity = hashlib.sha256(
|
||||
json.dumps([SCHEMA, session_id, source.identity], default=str).encode()
|
||||
).hexdigest()
|
||||
with self.guard:
|
||||
if self.jobs.get(identity, {}).get('state') == 'error':
|
||||
if self.jobs.get(identity, {}).get("state") == "error":
|
||||
self.jobs.pop(identity, None)
|
||||
return self.get(session_id)
|
||||
|
||||
def scene(self, session_id: str, generation: str) -> Path:
|
||||
current = self.get(session_id, start=False)
|
||||
if current.get('state') != 'ready' or current.get('generation') != generation:
|
||||
raise ValueError('overview generation is unavailable')
|
||||
return self.root / generation / 'scene.rrd'
|
||||
if current.get("state") != "ready" or current.get("generation") != generation:
|
||||
raise ValueError("overview generation is unavailable")
|
||||
return self.root / generation / "scene.rrd"
|
||||
|
||||
def comparison(
|
||||
self, session_id: str, generation: str, comparison_generation: str | None = None
|
||||
):
|
||||
# scene() rechecks current source identity, not merely the cached filename.
|
||||
scene = self.scene(session_id, generation)
|
||||
report = json.loads((scene.parent / "overview.json").read_text())
|
||||
return load_comparison(
|
||||
self.store.data_dir / "session-map-previews",
|
||||
generation,
|
||||
session_id,
|
||||
report["source_digests"],
|
||||
comparison_generation,
|
||||
)
|
||||
|
||||
def default_representation(self, session_id, comparison, reference_generation=None):
|
||||
if self.map_versions is None:
|
||||
return "original"
|
||||
version = (
|
||||
self.map_versions.selected(session_id)
|
||||
if reference_generation is None
|
||||
else self.map_versions.version(session_id, reference_generation)
|
||||
)
|
||||
if version is None:
|
||||
return "original"
|
||||
version.verify()
|
||||
if comparison is None or comparison.document["map_generation"] != version.generation:
|
||||
raise ValueError("Corrected overview for the pinned map is unavailable.")
|
||||
if version.document["source"]["source_digests"] != comparison.document["source_digests"]:
|
||||
raise ValueError("Corrected overview source mismatch.")
|
||||
return "corrected"
|
||||
|
||||
def _cached(self, directory: Path) -> dict | None:
|
||||
try:
|
||||
report = directory / 'overview.json'
|
||||
report = directory / "overview.json"
|
||||
if report.stat().st_size > 2 * 1024 * 1024:
|
||||
return None
|
||||
doc = json.loads(report.read_text())
|
||||
stat = (directory / 'scene.rrd').stat()
|
||||
if doc['schema_version'] != SCHEMA or [stat.st_size, stat.st_mtime_ns] != doc['scene_stat']:
|
||||
stat = (directory / "scene.rrd").stat()
|
||||
if (
|
||||
doc["schema_version"] != SCHEMA
|
||||
or [stat.st_size, stat.st_mtime_ns] != doc["scene_stat"]
|
||||
):
|
||||
return None
|
||||
return {'state': 'ready', 'metrics': doc['metrics'], 'scene_sha256': doc['scene_sha256']}
|
||||
return {
|
||||
"state": "ready",
|
||||
"metrics": doc["metrics"],
|
||||
"scene_sha256": doc["scene_sha256"],
|
||||
}
|
||||
except (OSError, ValueError, KeyError, TypeError):
|
||||
return None
|
||||
|
||||
def _build(self, identity: str, source, exporter: RecordingExporter) -> None:
|
||||
directory = self.root / identity
|
||||
directory.mkdir(exist_ok=True)
|
||||
candidate = directory / ('.' + uuid4().hex + '.rrd')
|
||||
candidate = directory / ("." + uuid4().hex + ".rrd")
|
||||
staged = None
|
||||
try:
|
||||
with self.guard:
|
||||
self.jobs[identity] = {'state': 'preparing', 'messages_processed': 0}
|
||||
self.jobs[identity] = {"state": "preparing", "messages_processed": 0}
|
||||
digests = _validated_artifact_digests(source)
|
||||
staged, primary, _ = _stage_replay_prefix(directory, source, cancel_event=self.cancel)
|
||||
|
||||
def pulse():
|
||||
with self.guard:
|
||||
self.jobs[identity]['messages_processed'] += 100
|
||||
metrics = dict(exporter(primary, candidate, cancel_event=self.cancel, activity_callback=pulse))
|
||||
self.jobs[identity]["messages_processed"] += 100
|
||||
|
||||
metrics = dict(
|
||||
exporter(primary, candidate, cancel_event=self.cancel, activity_callback=pulse)
|
||||
)
|
||||
if self.cancel.is_set() or source.identity != _validate_source_state(source).identity:
|
||||
raise ValueError('overview source changed')
|
||||
raise ValueError("overview source changed")
|
||||
if digests != _validated_artifact_digests(source):
|
||||
raise ValueError('overview source changed')
|
||||
raise ValueError("overview source changed")
|
||||
if candidate.stat().st_size > 32 * 1024 * 1024:
|
||||
raise ValueError('overview exceeded display budget')
|
||||
raise ValueError("overview exceeded display budget")
|
||||
digest = hashlib.sha256(candidate.read_bytes()).hexdigest()
|
||||
os.replace(candidate, directory / 'scene.rrd')
|
||||
stat = (directory / 'scene.rrd').stat()
|
||||
document = {'schema_version': SCHEMA, 'metrics': metrics, 'source_digests': digests,
|
||||
'scene_sha256': digest, 'scene_stat': [stat.st_size, stat.st_mtime_ns]}
|
||||
temporary = directory / '.overview.json'
|
||||
os.replace(candidate, directory / "scene.rrd")
|
||||
stat = (directory / "scene.rrd").stat()
|
||||
document = {
|
||||
"schema_version": SCHEMA,
|
||||
"metrics": metrics,
|
||||
"source_digests": digests,
|
||||
"scene_sha256": digest,
|
||||
"scene_stat": [stat.st_size, stat.st_mtime_ns],
|
||||
}
|
||||
temporary = directory / ".overview.json"
|
||||
temporary.write_text(json.dumps(document, allow_nan=False))
|
||||
os.replace(temporary, directory / 'overview.json')
|
||||
os.replace(temporary, directory / "overview.json")
|
||||
with self.guard:
|
||||
self.jobs.pop(identity, None)
|
||||
except Exception:
|
||||
logging.getLogger(__name__).exception('Session overview preparation failed')
|
||||
logging.getLogger(__name__).exception("Session overview preparation failed")
|
||||
with self.guard:
|
||||
self.jobs[identity] = {'state': 'error', 'message': 'Не удалось подготовить обзор записи.'}
|
||||
self.jobs[identity] = {
|
||||
"state": "error",
|
||||
"message": "Не удалось подготовить обзор записи.",
|
||||
}
|
||||
finally:
|
||||
candidate.unlink(missing_ok=True)
|
||||
if staged is not None:
|
||||
|
||||
@@ -0,0 +1,211 @@
|
||||
"""Content-addressed, paired display derivatives, never planning references.
|
||||
|
||||
Only an offline publisher may attach an explicitly reviewed version to a source
|
||||
overview. HTTP callers select a digest, not filesystem paths or a mutable latest
|
||||
map. Both representations preserve point/pose correspondence and share colors.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import hashlib
|
||||
import io
|
||||
import json
|
||||
import os
|
||||
import re
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from tempfile import TemporaryDirectory
|
||||
from uuid import uuid4
|
||||
|
||||
import numpy as np
|
||||
|
||||
from k1link.reconstruction.map_version import json_bytes
|
||||
|
||||
SCHEMA = "missioncore.overview-comparison/v1"
|
||||
MAX_POINTS = 180_000
|
||||
MAX_POSES = 20_000
|
||||
MAX_BYTES = 8 * 1024 * 1024
|
||||
|
||||
|
||||
def _digest(value):
|
||||
if not isinstance(value, str) or not re.fullmatch(r"[a-f0-9]{64}", value):
|
||||
raise ValueError("Invalid comparison identity.")
|
||||
return value
|
||||
|
||||
|
||||
def _read(path, maximum):
|
||||
if path.is_symlink() or path.stat().st_size > maximum:
|
||||
raise ValueError("Invalid comparison artifact.")
|
||||
with path.open("rb") as stream:
|
||||
value = stream.read(maximum + 1)
|
||||
if len(value) > maximum:
|
||||
raise ValueError("Comparison exceeds display budget.")
|
||||
return value
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class OverviewComparison:
|
||||
generation: str
|
||||
document: dict
|
||||
original: np.ndarray
|
||||
corrected: np.ndarray
|
||||
original_route: np.ndarray
|
||||
corrected_route: np.ndarray
|
||||
|
||||
@property
|
||||
def bounds(self):
|
||||
# Only bounds are combined; there is no correspondence or fitting here.
|
||||
return np.array(
|
||||
[
|
||||
self.original.min(axis=0),
|
||||
self.original.max(axis=0),
|
||||
self.corrected.min(axis=0),
|
||||
self.corrected.max(axis=0),
|
||||
]
|
||||
)
|
||||
|
||||
def metadata(self):
|
||||
return dict(
|
||||
generation=self.generation,
|
||||
map_generation=self.document["map_generation"],
|
||||
sample_points=len(self.original),
|
||||
source_points=self.document["source_points"],
|
||||
height_min_m=min(-3.0, float(self.bounds[:, 2].min())),
|
||||
height_max_m=max(80.0, float(self.bounds[:, 2].max())),
|
||||
original_path_m=self.document["original_path_m"],
|
||||
corrected_path_m=self.document["corrected_path_m"],
|
||||
)
|
||||
|
||||
|
||||
def _arrays(payload):
|
||||
# np.savez (uncompressed) is deliberate: reject compressed archive bombs.
|
||||
import zipfile
|
||||
|
||||
with zipfile.ZipFile(io.BytesIO(payload)) as archive:
|
||||
if len(archive.infolist()) != 4 or any(
|
||||
x.compress_type != zipfile.ZIP_STORED for x in archive.infolist()
|
||||
):
|
||||
raise ValueError("Invalid comparison array archive.")
|
||||
with np.load(io.BytesIO(payload), allow_pickle=False) as data:
|
||||
result = [
|
||||
np.array(data[k])
|
||||
for k in ("original", "corrected", "original_route", "corrected_route")
|
||||
]
|
||||
a, b, p, q = result
|
||||
if (
|
||||
a.shape != b.shape
|
||||
or p.shape != q.shape
|
||||
or not 1 <= len(a) <= MAX_POINTS
|
||||
or not 2 <= len(p) <= MAX_POSES
|
||||
or any(
|
||||
x.ndim != 2
|
||||
or x.shape[1:] != (3,)
|
||||
or x.dtype != np.dtype("<f4")
|
||||
or not np.isfinite(x).all()
|
||||
for x in result
|
||||
)
|
||||
):
|
||||
raise ValueError("Invalid paired comparison geometry.")
|
||||
return result
|
||||
|
||||
|
||||
def load_comparison(
|
||||
root: Path,
|
||||
overview_generation: str,
|
||||
session_id: str,
|
||||
source_digests: dict,
|
||||
generation: str | None = None,
|
||||
):
|
||||
root = Path(root)
|
||||
_digest(overview_generation)
|
||||
pointer = root / "selected" / f"{overview_generation}.json"
|
||||
if generation is None:
|
||||
if not pointer.exists():
|
||||
return None
|
||||
generation = json.loads(_read(pointer, 4096))["generation"]
|
||||
_digest(generation)
|
||||
directory = root / generation
|
||||
if directory.is_symlink():
|
||||
raise ValueError("Comparison directory must not be a link.")
|
||||
payload = _read(directory / "manifest.json", 32_768)
|
||||
if hashlib.sha256(payload).hexdigest() != generation:
|
||||
raise ValueError("Comparison manifest changed.")
|
||||
doc = json.loads(payload)
|
||||
if (
|
||||
doc.get("schema_version") != SCHEMA
|
||||
or doc.get("session_id") != session_id
|
||||
or doc.get("overview_generation") != overview_generation
|
||||
or doc.get("source_digests") != source_digests
|
||||
or doc.get("view_only") is not True
|
||||
):
|
||||
raise ValueError("Comparison belongs to another recording generation.")
|
||||
_digest(doc["map_generation"])
|
||||
arrays = _read(directory / "geometry.npz", MAX_BYTES)
|
||||
if hashlib.sha256(arrays).hexdigest() != doc["geometry_sha256"]:
|
||||
raise ValueError("Comparison geometry changed.")
|
||||
return OverviewComparison(generation, doc, *_arrays(arrays))
|
||||
|
||||
|
||||
def publish_comparison(
|
||||
root,
|
||||
*,
|
||||
session_id,
|
||||
overview_generation,
|
||||
source_digests,
|
||||
map_generation,
|
||||
source_points,
|
||||
original_path_m,
|
||||
corrected_path_m,
|
||||
original,
|
||||
corrected,
|
||||
original_route,
|
||||
corrected_route,
|
||||
):
|
||||
"""Publish a view-only pair; no catalog or mission mutation, no implicit promotion."""
|
||||
root = Path(root)
|
||||
root.mkdir(parents=True, exist_ok=True)
|
||||
_digest(overview_generation)
|
||||
_digest(map_generation)
|
||||
with TemporaryDirectory(prefix=".comparison-", dir=root) as temporary:
|
||||
stage = Path(temporary)
|
||||
np.savez(
|
||||
stage / "geometry.npz",
|
||||
**{
|
||||
name: np.asarray(value, dtype="<f4")
|
||||
for name, value in (
|
||||
("original", original),
|
||||
("corrected", corrected),
|
||||
("original_route", original_route),
|
||||
("corrected_route", corrected_route),
|
||||
)
|
||||
},
|
||||
)
|
||||
payload = _read(stage / "geometry.npz", MAX_BYTES)
|
||||
_arrays(payload)
|
||||
doc = dict(
|
||||
schema_version=SCHEMA,
|
||||
session_id=session_id,
|
||||
overview_generation=overview_generation,
|
||||
source_digests=source_digests,
|
||||
map_generation=map_generation,
|
||||
source_points=source_points,
|
||||
original_path_m=original_path_m,
|
||||
corrected_path_m=corrected_path_m,
|
||||
view_only=True,
|
||||
geometry_sha256=hashlib.sha256(payload).hexdigest(),
|
||||
)
|
||||
manifest = json_bytes(doc)
|
||||
generation = hashlib.sha256(manifest).hexdigest()
|
||||
(stage / "manifest.json").write_bytes(manifest)
|
||||
if not (root / generation).exists():
|
||||
stage.rename(root / generation)
|
||||
load_comparison(root, overview_generation, session_id, source_digests, generation)
|
||||
pointers = root / "selected"
|
||||
pointers.mkdir(exist_ok=True)
|
||||
temporary = pointers / f".{uuid4().hex}.json"
|
||||
try:
|
||||
temporary.write_bytes(json_bytes(dict(generation=generation)))
|
||||
os.replace(temporary, pointers / f"{overview_generation}.json")
|
||||
finally:
|
||||
temporary.unlink(missing_ok=True)
|
||||
return generation
|
||||
@@ -1,4 +1,5 @@
|
||||
"""View-only height clipping and camera presets for bounded overview RRDs."""
|
||||
|
||||
from functools import lru_cache
|
||||
from pathlib import Path
|
||||
from typing import Literal
|
||||
@@ -8,22 +9,24 @@ import rerun as rr
|
||||
from rerun import blueprint as rrb
|
||||
from rerun.experimental import RrdReader
|
||||
|
||||
from .overview_comparison import OverviewComparison
|
||||
|
||||
|
||||
@lru_cache(maxsize=1)
|
||||
def _geometry(path: Path, size: int, modified: int):
|
||||
if size > 32 * 1024 * 1024:
|
||||
raise ValueError('overview exceeds display budget')
|
||||
raise ValueError("overview exceeds display budget")
|
||||
reader = RrdReader(path)
|
||||
entry = reader.recordings()[0]
|
||||
xyz = np.empty((0, 3), dtype=np.float32)
|
||||
colors = np.empty(0, dtype=np.uint32)
|
||||
for chunk in reader.stream():
|
||||
if chunk.entity_path == '/world/cloud':
|
||||
if chunk.entity_path == "/world/cloud":
|
||||
batch = chunk.to_record_batch()
|
||||
xyz = batch.column('Points3D:positions')[0].values.values.to_numpy().reshape(-1, 3)
|
||||
colors = batch.column('Points3D:colors')[0].values.to_numpy()
|
||||
xyz = batch.column("Points3D:positions")[0].values.values.to_numpy().reshape(-1, 3)
|
||||
colors = batch.column("Points3D:colors")[0].values.to_numpy()
|
||||
if len(xyz) > 180_000 or not np.isfinite(xyz).all():
|
||||
raise ValueError('overview geometry is invalid')
|
||||
raise ValueError("overview geometry is invalid")
|
||||
return entry.application_id, entry.recording_id, xyz, colors
|
||||
|
||||
|
||||
@@ -32,15 +35,18 @@ def geometry(path: Path):
|
||||
return _geometry(path, stat.st_size, stat.st_mtime_ns)
|
||||
|
||||
|
||||
def spatial_metadata(path: Path) -> dict:
|
||||
def spatial_metadata(path: Path, comparison: OverviewComparison | None = None) -> dict:
|
||||
_, _, xyz, _ = geometry(path)
|
||||
return {'height_min_m': float(xyz[:, 2].min()) if len(xyz) else None,
|
||||
'height_max_m': float(xyz[:, 2].max()) if len(xyz) else None,
|
||||
'sample_points': len(xyz)}
|
||||
return {
|
||||
"height_min_m": float(xyz[:, 2].min()) if len(xyz) else None,
|
||||
"height_max_m": float(xyz[:, 2].max()) if len(xyz) else None,
|
||||
"sample_points": len(xyz),
|
||||
**({"comparison": comparison.metadata()} if comparison else {}),
|
||||
}
|
||||
|
||||
|
||||
def _camera_eye(xyz: np.ndarray, mode: Literal['3d', 'top'], aspect: float) -> dict:
|
||||
points = xyz.astype(np.float64) if len(xyz) else np.array([[-1., -1., -1.], [1., 1., 1.]])
|
||||
def _camera_eye(xyz: np.ndarray, mode: Literal["3d", "top"], aspect: float) -> dict:
|
||||
points = xyz.astype(np.float64) if len(xyz) else np.array([[-1.0, -1.0, -1.0], [1.0, 1.0, 1.0]])
|
||||
center = (points.min(axis=0) + points.max(axis=0)) / 2
|
||||
centered = points - center
|
||||
# Align an elongated survey with the width of the viewport, regardless of K1's initial yaw.
|
||||
@@ -48,38 +54,104 @@ def _camera_eye(xyz: np.ndarray, mode: Literal['3d', 'top'], aspect: float) -> d
|
||||
along = axes[:, -1]
|
||||
if along[np.argmax(np.abs(along))] < 0:
|
||||
along = -along
|
||||
side = np.array([-along[1], along[0], 0.])
|
||||
direction = np.array([0., 0., 1.]) if mode == 'top' else side * .8 + np.array([0., 0., .75])
|
||||
side = np.array([-along[1], along[0], 0.0])
|
||||
direction = (
|
||||
np.array([0.0, 0.0, 1.0]) if mode == "top" else side * 0.8 + np.array([0.0, 0.0, 0.75])
|
||||
)
|
||||
direction /= np.linalg.norm(direction)
|
||||
up = side if mode == 'top' else np.array([0., 0., 1.])
|
||||
up = side if mode == "top" else np.array([0.0, 0.0, 1.0])
|
||||
right = np.cross(-direction, up)
|
||||
right /= np.linalg.norm(right)
|
||||
screen_up = np.cross(right, -direction)
|
||||
# Conservative 45-degree vertical field of view, with space around the cloud.
|
||||
tangent = np.tan(np.pi / 8)
|
||||
depth = centered @ direction
|
||||
required = np.maximum(np.abs(centered @ right) / (aspect * tangent),
|
||||
np.abs(centered @ screen_up) / tangent) + depth
|
||||
distance = max(2., float(required.max()) * 1.15)
|
||||
return {'position': (center + direction * distance).tolist(),
|
||||
'lookTarget': center.tolist(), 'eyeUp': up.tolist()}
|
||||
required = (
|
||||
np.maximum(
|
||||
np.abs(centered @ right) / (aspect * tangent), np.abs(centered @ screen_up) / tangent
|
||||
)
|
||||
+ depth
|
||||
)
|
||||
distance = max(2.0, float(required.max()) * 1.15)
|
||||
return {
|
||||
"position": (center + direction * distance).tolist(),
|
||||
"lookTarget": center.tolist(),
|
||||
"eyeUp": up.tolist(),
|
||||
}
|
||||
|
||||
|
||||
def render_spatial_update(path: Path, ceiling_m: float | None, mode: Literal['3d', 'top'] | None, aspect: float = 1.5):
|
||||
def render_spatial_update(
|
||||
path: Path,
|
||||
ceiling_m: float | None,
|
||||
mode: Literal["3d", "top"] | None,
|
||||
aspect: float = 1.5,
|
||||
comparison: OverviewComparison | None = None,
|
||||
representation: Literal["original", "corrected"] = "original",
|
||||
):
|
||||
app_id, recording_id, xyz, colors = geometry(path)
|
||||
if representation == "corrected" and comparison is None:
|
||||
raise ValueError("Corrected representation requires a pinned comparison.")
|
||||
camera_points = xyz
|
||||
route = None
|
||||
if comparison is not None:
|
||||
xyz = comparison.corrected if representation == "corrected" else comparison.original
|
||||
route = (
|
||||
comparison.corrected_route
|
||||
if representation == "corrected"
|
||||
else comparison.original_route
|
||||
)
|
||||
camera_points = comparison.bounds
|
||||
# Identical point colors in both representations: only geometry changes.
|
||||
height = comparison.original[:, 2]
|
||||
low, high = np.quantile(height, [0.05, 0.95])
|
||||
normalized = np.clip((height - low) / max(high - low, 0.01), 0, 1)
|
||||
colors = np.column_stack(
|
||||
[70 + 100 * normalized, 135 + 80 * normalized, 220 - 90 * normalized]
|
||||
).astype(np.uint8)
|
||||
selected = xyz[:, 2] <= ceiling_m if ceiling_m is not None else np.ones(len(xyz), dtype=bool)
|
||||
recording = rr.RecordingStream(app_id, recording_id=recording_id, send_properties=False)
|
||||
sink = rr.binary_stream(recording)
|
||||
eye = None
|
||||
try:
|
||||
# Static replacement changes only the display derivative; poses and source metrics are untouched.
|
||||
recording.log('world/cloud', rr.Points3D(xyz[selected], colors=colors[selected], radii=rr.Radius.ui_points(1.5)), static=True)
|
||||
# Replace only display geometry. Original recorded poses/metrics remain untouched.
|
||||
recording.log(
|
||||
"world/cloud",
|
||||
rr.Points3D(xyz[selected], colors=colors[selected], radii=rr.Radius.ui_points(1.5)),
|
||||
static=True,
|
||||
)
|
||||
if route is not None:
|
||||
recording.log(
|
||||
"world/route",
|
||||
rr.LineStrips3D([route], colors=[180, 240, 90], radii=rr.Radius.ui_points(2)),
|
||||
static=True,
|
||||
)
|
||||
recording.log(
|
||||
"world/endpoints",
|
||||
rr.Points3D(
|
||||
route[[0, -1]],
|
||||
labels=["Старт", "Финиш"],
|
||||
colors=[245, 248, 240],
|
||||
radii=rr.Radius.ui_points(5),
|
||||
),
|
||||
static=True,
|
||||
)
|
||||
if mode is not None:
|
||||
eye = _camera_eye(xyz, mode, aspect)
|
||||
view = rrb.Spatial3DView(name='Облако и траектория', origin='/world', contents=['/world/**'],
|
||||
background=[9, 10, 12, 255], eye_controls=rrb.EyeControls3D(kind=rrb.Eye3DKind.Orbital,
|
||||
position=eye['position'], look_target=eye['lookTarget'], eye_up=eye['eyeUp']))
|
||||
recording.send_blueprint(rrb.Blueprint(view, auto_layout=False, auto_views=False, collapse_panels=True))
|
||||
eye = _camera_eye(camera_points, mode, aspect)
|
||||
view = rrb.Spatial3DView(
|
||||
name="Облако и траектория",
|
||||
origin="/world",
|
||||
contents=["/world/**"],
|
||||
background=[9, 10, 12, 255],
|
||||
eye_controls=rrb.EyeControls3D(
|
||||
kind=rrb.Eye3DKind.Orbital,
|
||||
position=eye["position"],
|
||||
look_target=eye["lookTarget"],
|
||||
eye_up=eye["eyeUp"],
|
||||
),
|
||||
)
|
||||
recording.send_blueprint(
|
||||
rrb.Blueprint(view, auto_layout=False, auto_views=False, collapse_panels=True)
|
||||
)
|
||||
data = sink.read(flush=True)
|
||||
return data, int(selected.sum()), eye
|
||||
finally:
|
||||
|
||||
@@ -8,7 +8,7 @@ evidence and export it into the host's canonical recorded-viewer artifact.
|
||||
from __future__ import annotations
|
||||
|
||||
import threading
|
||||
from collections.abc import Callable, Mapping
|
||||
from collections.abc import Callable, Iterator, Mapping
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import Literal, Protocol
|
||||
@@ -53,6 +53,13 @@ class RecordedPointColorRenderer(Protocol):
|
||||
) -> bytes: ...
|
||||
|
||||
|
||||
class RecordedPointDisplayRenderer(Protocol):
|
||||
def __call__(self, command: ReplayCommand, *, application_id: str, recording_id: str,
|
||||
color_mode: str, palette: str, custom_color: str,
|
||||
point_decimation_percent: float, display_bank: str,
|
||||
prepared_recording_path: Path | None = None) -> Iterator[bytes]: ...
|
||||
|
||||
|
||||
ObservationArchiveDiscovery = Callable[
|
||||
[Path],
|
||||
tuple[ObservationSessionCandidate, ...],
|
||||
@@ -82,6 +89,7 @@ class ObservationRuntimeContribution:
|
||||
archives: tuple[ObservationArchiveSource, ...]
|
||||
recording_exporter: RecordingExporter
|
||||
point_color_renderer: RecordedPointColorRenderer | None = None
|
||||
point_display_renderer: RecordedPointDisplayRenderer | None = None
|
||||
overview_exporter: RecordingExporter | None = None
|
||||
planning_exporter: RecordingExporter | None = None
|
||||
submap_extractor: SubmapExtractor | None = None
|
||||
|
||||
@@ -756,6 +756,7 @@ def _source_identity(command: ReplayCommand) -> tuple[object, ...]:
|
||||
command.session_id,
|
||||
command.plugin_id,
|
||||
command.primary_artifact_id,
|
||||
command.map_version,
|
||||
]
|
||||
for artifact in command.artifacts:
|
||||
path = artifact.path
|
||||
|
||||
@@ -30,14 +30,12 @@ from .plugin_contract import (
|
||||
RecordingExporter,
|
||||
)
|
||||
|
||||
# v12 publishes a 2 Hz point-cloud operator projection while retaining complete
|
||||
# normal K1 point batches. v11 also spatially thinned every ~2.4k-point frame;
|
||||
# the latest-at AI view therefore looked visibly bald even though the raw
|
||||
# capture was complete. Frames above the explicit emergency threshold remain
|
||||
# bounded, and the native capture stays the source of record and AI input.
|
||||
CACHE_SCHEMA = "missioncore.derived-rerun-recording-cache/v12"
|
||||
# v13 preserves every point and frame. Older projections silently discarded
|
||||
# four out of five frames, losing fine geometry even in accumulated playback.
|
||||
# Invalidate only the derived RRD; raw captures and corrected maps are unchanged.
|
||||
CACHE_SCHEMA = "missioncore.derived-rerun-recording-cache/v13"
|
||||
COMPATIBLE_CACHE_SCHEMAS = frozenset({CACHE_SCHEMA})
|
||||
RECORDING_CACHE_FILENAME = "scene.operator-v12.rrd"
|
||||
RECORDING_CACHE_FILENAME = "scene.operator-v13.rrd"
|
||||
RECORDING_CACHE_SIDECAR_FILENAME = f"{RECORDING_CACHE_FILENAME}.cache.json"
|
||||
RERUN_RECORDING_MEDIA_TYPE = "application/vnd.rerun.rrd"
|
||||
RERUN_SESSION_TIMELINE = "session_time"
|
||||
@@ -812,6 +810,10 @@ class SessionRecordingMaterializer:
|
||||
raise PluginRecordingExportError(
|
||||
f"device plugin has no recording exporter: {plugin_id}"
|
||||
)
|
||||
if any(name.startswith("map-version-") for name in artifacts or {}) and not getattr(
|
||||
selected, "supports_map_versions", False
|
||||
):
|
||||
raise RecordingMaterializationError("Device exporter cannot project corrected geometry")
|
||||
exporter = cast(RrdExporter, selected)
|
||||
kwargs: dict[str, object] = {}
|
||||
if _callable_accepts_keyword(exporter, "artifacts"):
|
||||
@@ -1085,11 +1087,19 @@ def _validate_source(command: ReplayCommand) -> _ValidatedSource:
|
||||
)
|
||||
if sum(artifact.artifact_id == primary_artifact_id for artifact in validated) != 1:
|
||||
raise RecordingMaterializationError("recording primary artifact is unavailable")
|
||||
return _ValidatedSource(
|
||||
result = _ValidatedSource(
|
||||
plugin_id=plugin_id,
|
||||
primary_artifact_id=primary_artifact_id,
|
||||
artifacts=tuple(validated),
|
||||
)
|
||||
if command.map_version is not None:
|
||||
from .map_recording import map_recording_inputs
|
||||
|
||||
projected = map_recording_inputs(command, result)
|
||||
if seen_names.intersection(item.path.name for item in projected):
|
||||
raise RecordingMaterializationError("map artifact filenames conflict with native inputs")
|
||||
result = _ValidatedSource(plugin_id, primary_artifact_id, (*result.artifacts, *projected))
|
||||
return result
|
||||
|
||||
|
||||
def _validate_source_state(source: _ValidatedSource) -> _ValidatedSource:
|
||||
|
||||
@@ -56,7 +56,7 @@ LAB_ORIGIN = "missioncore.lab-instance/v1"
|
||||
LAB_ID_PATTERN = re.compile(r"^LAB [A-Z][A-Z0-9._-]{0,31}$")
|
||||
SHA256_PATTERN = re.compile(r"^[a-f0-9]{64}$")
|
||||
LAB_METHOD_SCHEMA = "missioncore.laboratory-method/v1"
|
||||
SessionScope = Literal["all", "source", "laboratory"]
|
||||
SessionScope = Literal["all", "source", "standalone", "laboratory"]
|
||||
|
||||
SCHEMA_SQL = """
|
||||
CREATE TABLE IF NOT EXISTS observation_sessions (
|
||||
@@ -95,6 +95,16 @@ CREATE TABLE IF NOT EXISTS observation_sessions (
|
||||
CREATE INDEX IF NOT EXISTS observation_sessions_recent
|
||||
ON observation_sessions(started_at_utc DESC, session_id DESC);
|
||||
|
||||
-- Acquisition provenance, not a relabeling of physical evidence as a LAB.
|
||||
-- May be recorded before capture finalization/catalog ingestion. Deliberately
|
||||
-- survives catalog reconciliation and removal of a planner project.
|
||||
CREATE TABLE IF NOT EXISTS observation_planning_captures (
|
||||
session_id TEXT NOT NULL,
|
||||
run_id TEXT NOT NULL,
|
||||
recorded_at_utc TEXT NOT NULL,
|
||||
PRIMARY KEY (session_id, run_id)
|
||||
);
|
||||
|
||||
CREATE TABLE IF NOT EXISTS observation_session_artifacts (
|
||||
session_id TEXT NOT NULL REFERENCES observation_sessions(session_id) ON DELETE CASCADE,
|
||||
artifact_id TEXT NOT NULL,
|
||||
@@ -237,6 +247,19 @@ class SessionStore:
|
||||
connection.commit()
|
||||
return tuple(imported)
|
||||
|
||||
def record_planning_capture(self, session_id: str, run_id: str) -> None:
|
||||
"""Remember an exact capture bound by a live planner, never by its name."""
|
||||
from uuid import UUID
|
||||
|
||||
_validate_identifier(session_id, "planning capture")
|
||||
run_id = str(UUID(run_id))
|
||||
with self._lock, self._connect() as connection:
|
||||
connection.execute(
|
||||
"INSERT OR IGNORE INTO observation_planning_captures VALUES (?, ?, ?)",
|
||||
(session_id, run_id, utc_now_iso()),
|
||||
)
|
||||
connection.commit()
|
||||
|
||||
def list_recent(
|
||||
self,
|
||||
*,
|
||||
@@ -249,6 +272,8 @@ class SessionStore:
|
||||
raise ValueError("limit must be within 1..100")
|
||||
if not isinstance(include_capability_projections, bool):
|
||||
raise ValueError("capability projection policy must be boolean")
|
||||
standalone = scope == "standalone"
|
||||
effective_scope = "source" if standalone else scope
|
||||
scope_clause = ({
|
||||
"all": "1 = 1",
|
||||
"source": (
|
||||
@@ -274,9 +299,14 @@ class SessionStore:
|
||||
"WHERE lab.session_id = sessions.session_id "
|
||||
"AND lab.replay_capability_json IS NULL)"
|
||||
),
|
||||
}).get(scope)
|
||||
}).get(effective_scope)
|
||||
if scope_clause is None:
|
||||
raise ValueError("scope must be all, source, or laboratory")
|
||||
raise ValueError("scope must be all, source, standalone, or laboratory")
|
||||
if standalone:
|
||||
scope_clause += (
|
||||
" AND NOT EXISTS (SELECT 1 FROM observation_planning_captures AS planning "
|
||||
"WHERE planning.session_id = sessions.session_id)"
|
||||
)
|
||||
parameters: list[object] = []
|
||||
where = f"WHERE {scope_clause}" # noqa: S608 - closed static scope clauses
|
||||
with self._connect() as connection:
|
||||
@@ -341,6 +371,22 @@ class SessionStore:
|
||||
detail, _snapshot_sha256 = self.get_session_with_catalog_snapshot(session_id)
|
||||
return detail
|
||||
|
||||
def display_profile_root(self, session_id: str) -> Path:
|
||||
"""Resolve only the catalogued physical directory; never accept a client path."""
|
||||
_validate_identifier(session_id, "session id")
|
||||
with self._connect() as connection:
|
||||
row = connection.execute(
|
||||
"SELECT allowed_root, session_root FROM observation_sessions WHERE session_id = ?",
|
||||
(session_id,),
|
||||
).fetchone()
|
||||
if row is None:
|
||||
raise SessionNotFoundError("observation session was not found")
|
||||
root = Path(row["session_root"])
|
||||
allowed = Path(row["allowed_root"]).resolve(strict=True)
|
||||
if root.is_symlink() or not root.is_dir() or not root.resolve().is_relative_to(allowed):
|
||||
raise SessionIntegrityError("display profile escapes session directory")
|
||||
return root.resolve()
|
||||
|
||||
def get_session_with_catalog_snapshot(
|
||||
self,
|
||||
session_id: str,
|
||||
|
||||
@@ -152,6 +152,7 @@ recorded_blueprint_sessions = RecordedBlueprintSessions[_RecordedBlueprintStream
|
||||
def recorded_blueprint(
|
||||
settings: RerunSceneSettings,
|
||||
*,
|
||||
display_point_bank: str | None = None,
|
||||
include_initial_playback_state: bool = True,
|
||||
active_view: RecordedView = "spatial",
|
||||
view_reset_generation: Literal[0, 1] = 0,
|
||||
@@ -238,6 +239,9 @@ def recorded_blueprint(
|
||||
spatial_view = rrb.Spatial3DView(
|
||||
origin="/world",
|
||||
name="Мир · LiDAR и объекты" if unified_perception else "Пространственная сцена",
|
||||
contents=["+ /world/**", "- /world/display_points/**"] + (
|
||||
["- /world/points", f"+ /world/display_points/{display_point_bank}"]
|
||||
if display_point_bank is not None else []),
|
||||
background=[7, 8, 10, 255],
|
||||
line_grid=rrb.LineGrid3D(
|
||||
visible=settings.show_grid,
|
||||
@@ -249,6 +253,8 @@ def recorded_blueprint(
|
||||
# inherits the view's latest-at query and can never turn into an
|
||||
# object-history trail when the operator widens the cloud window.
|
||||
"/world/points": point_overrides,
|
||||
**({f"/world/display_points/{display_point_bank}": point_overrides}
|
||||
if display_point_bank is not None else {}),
|
||||
"/world/costmap": rrb.EntityBehavior(visible=show_costmap),
|
||||
"/world/trajectory": trajectory_overrides,
|
||||
"/world/perception": rrb.EntityBehavior(visible=show_cuboids_3d),
|
||||
@@ -429,6 +435,7 @@ def recorded_blueprint(
|
||||
def recorded_blueprint_rrd(
|
||||
settings: RerunSceneSettings,
|
||||
*,
|
||||
display_point_bank: str | None = None,
|
||||
application_id: str = APPLICATION_ID,
|
||||
recording_id: str,
|
||||
blueprint_session_id: str | None = None,
|
||||
@@ -458,6 +465,7 @@ def recorded_blueprint_rrd(
|
||||
) -> rrb.Blueprint:
|
||||
return recorded_blueprint(
|
||||
settings,
|
||||
display_point_bank=display_point_bank,
|
||||
include_initial_playback_state=False,
|
||||
active_view=active_view,
|
||||
view_reset_generation=view_reset_generation,
|
||||
|
||||
+23
-9
@@ -12,7 +12,6 @@ from fastapi import FastAPI, HTTPException, Request, WebSocket, WebSocketDisconn
|
||||
from fastapi.exceptions import RequestValidationError
|
||||
from fastapi.responses import JSONResponse
|
||||
from pydantic import ValidationError
|
||||
from starlette.middleware.gzip import GZipMiddleware
|
||||
|
||||
from k1link import __version__
|
||||
from k1link.artifact_gateway import configured_artifact_gateway
|
||||
@@ -146,6 +145,7 @@ from k1link.web.e46j_raw_fisheye_realtime_api import (
|
||||
from k1link.web.e47_semantic_slam_api import build_e47_semantic_slam_router
|
||||
from k1link.web.environment_api import build_environment_router
|
||||
from k1link.web.frontend_assets import ControlStationStaticFiles, frontend_build_id
|
||||
from k1link.web.response_compression import ResponseCompressionMiddleware
|
||||
from k1link.web.l3_pointpillars_visual_api import (
|
||||
build_l3_pointpillars_visual_router,
|
||||
)
|
||||
@@ -470,14 +470,25 @@ session_recording_materializer = SessionRecordingMaterializer(
|
||||
exporters=plugin_environment.recording_exporters,
|
||||
artifact_gateway=session_artifact_gateway,
|
||||
)
|
||||
session_overview_service = SessionOverviewService(session_store, plugin_environment.overview_exporters)
|
||||
mission_drafts = MissionDrafts(session_store.data_dir / 'missions', PlanningSources(
|
||||
from k1link.reconstruction.session_versions import SessionMapVersions
|
||||
from k1link.sessions.map_recording import MapReplaySessionStore
|
||||
from k1link.missions.default_sources import DefaultPlanningSources
|
||||
|
||||
session_map_versions = SessionMapVersions(session_store.data_dir)
|
||||
operator_session_store = MapReplaySessionStore(session_store, session_map_versions)
|
||||
session_overview_service = SessionOverviewService(session_store, plugin_environment.overview_exporters,
|
||||
map_versions=session_map_versions)
|
||||
original_planning_sources = PlanningSources(
|
||||
session_store, plugin_environment.planning_exporters, plugin_environment.submap_extractors,
|
||||
plugin_environment.scene_submap_extractors))
|
||||
plugin_environment.scene_submap_extractors)
|
||||
mission_drafts = MissionDrafts(session_store.data_dir / 'missions',
|
||||
DefaultPlanningSources(original_planning_sources, session_map_versions))
|
||||
mission_registration_runs = RegistrationRuns(mission_drafts)
|
||||
from k1link.missions.live_tests import PlanningLiveTests
|
||||
from k1link.web.planning_live_api import build_planning_live_router
|
||||
planning_live_tests = PlanningLiveTests(mission_drafts, plugin_environment.live_planning_sources, mission_registration_runs.lock)
|
||||
planning_live_tests = PlanningLiveTests(mission_drafts, plugin_environment.live_planning_sources,
|
||||
mission_registration_runs.lock,
|
||||
capture_recorder=session_store.record_planning_capture)
|
||||
session_recorded_media_inspector = RecordedMediaInspector(
|
||||
session_store.data_dir / "recorded-media-preparations"
|
||||
)
|
||||
@@ -744,7 +755,7 @@ def _m48_recorded_camera_playback_source(
|
||||
|
||||
if session_recorded_camera_frame_service is None:
|
||||
raise RuntimeError("recorded camera playback is unavailable")
|
||||
command = session_store.prepare_replay(session_id, speed=1.0, loop=False)
|
||||
command = operator_session_store.prepare_replay(session_id, speed=1.0, loop=False)
|
||||
snapshot = session_recording_preparation_manager.restore_published(command)
|
||||
if snapshot is None or snapshot.state != "ready" or snapshot.recorded_media is None:
|
||||
raise RuntimeError("recorded camera playback package is not published")
|
||||
@@ -757,6 +768,8 @@ def _canonical_lab_recording_source(session_id: str) -> tuple[Path, str] | None:
|
||||
snapshot = session_recording_preparation_manager.status(session_id)
|
||||
if snapshot is None or snapshot.state != "ready" or snapshot.recording is None:
|
||||
return None
|
||||
if snapshot.command.map_version is not None:
|
||||
return None # Original-frame LAB overlays cannot consume corrected operator geometry.
|
||||
return snapshot.recording.path, snapshot.recording.sha256
|
||||
|
||||
|
||||
@@ -798,7 +811,7 @@ def enqueue_replayable_recordings(session_ids: Iterable[str]) -> tuple[str, ...]
|
||||
enqueued: list[str] = []
|
||||
for session_id in dict.fromkeys(session_ids):
|
||||
try:
|
||||
command = session_store.prepare_replay(session_id)
|
||||
command = operator_session_store.prepare_replay(session_id)
|
||||
session_recording_preparation_manager.enqueue(
|
||||
command,
|
||||
retry_interrupted=True,
|
||||
@@ -986,7 +999,7 @@ app = FastAPI(
|
||||
openapi_url="/api/openapi.json",
|
||||
lifespan=app_lifespan,
|
||||
)
|
||||
app.add_middleware(GZipMiddleware, minimum_size=1_024, compresslevel=5)
|
||||
app.add_middleware(ResponseCompressionMiddleware, minimum_size=1_024, compresslevel=5)
|
||||
|
||||
|
||||
app.include_router(fleet_router)
|
||||
@@ -1153,7 +1166,7 @@ app.include_router(build_mission_registration_router(mission_registration_runs,
|
||||
|
||||
app.include_router(
|
||||
build_session_router(
|
||||
session_store,
|
||||
operator_session_store,
|
||||
# Production discovery belongs to the startup/background reconciler.
|
||||
# HTTP list/replay paths must never rescan evidence roots inline.
|
||||
catalog_refresher=None,
|
||||
@@ -1163,6 +1176,7 @@ app.include_router(
|
||||
perception_overlay_provider=session_perception_overlay_store,
|
||||
perception_media_provider=session_perception_epoch_store,
|
||||
point_color_renderers=plugin_environment.point_color_renderers,
|
||||
point_display_renderers=plugin_environment.point_display_renderers,
|
||||
lab_calculation_profile_resolver=(
|
||||
None
|
||||
if (
|
||||
|
||||
@@ -13,6 +13,7 @@ from missioncore_plugin_sdk.v0alpha2 import RuntimeHandshakeRequest
|
||||
from k1link.sessions.plugin_contract import (
|
||||
ObservationArchiveSource,
|
||||
RecordedPointColorRenderer,
|
||||
RecordedPointDisplayRenderer,
|
||||
RecordingExporter,
|
||||
SubmapExtractor,
|
||||
)
|
||||
@@ -103,6 +104,12 @@ class InstalledDevicePluginEnvironment:
|
||||
if (renderer := contribution.observation.point_color_renderer) is not None
|
||||
}
|
||||
|
||||
@property
|
||||
def point_display_renderers(self) -> dict[str, RecordedPointDisplayRenderer]:
|
||||
return {c.runtime.descriptor.plugin_id: c.observation.point_display_renderer
|
||||
for c in self._contributions if c.observation is not None
|
||||
and c.observation.point_display_renderer is not None}
|
||||
|
||||
@property
|
||||
def runtime_health(self) -> tuple[dict[str, Any], ...]:
|
||||
return tuple(
|
||||
|
||||
@@ -1,7 +1,7 @@
|
||||
"""Mission planning API: immutable recorded sources, draft persistence, data checks."""
|
||||
from uuid import UUID
|
||||
from typing import Literal
|
||||
from fastapi import APIRouter, HTTPException
|
||||
from fastapi import APIRouter, HTTPException, Query
|
||||
from pydantic import BaseModel, ConfigDict, Field
|
||||
from starlette.concurrency import run_in_threadpool
|
||||
from k1link.sessions.models import SessionNotFoundError, SessionStoreError
|
||||
@@ -16,8 +16,11 @@ class DraftRequest(BaseModel):
|
||||
name: str = Field(min_length=1, max_length=120)
|
||||
session_id: str = Field(pattern=r'^[A-Za-z0-9][A-Za-z0-9._-]{0,127}$')
|
||||
generation: str = Field(pattern='^[a-f0-9]{64}$')
|
||||
start_index: int = Field(ge=0, strict=True)
|
||||
end_index: int = Field(ge=1, strict=True)
|
||||
# Legacy bounded clients remain readable; the product now asks the server
|
||||
# to resolve the full immutable trajectory instead of sending crop indices.
|
||||
whole_recording: bool = Field(default=False, strict=True)
|
||||
start_index: int | None = Field(default=None, ge=0, strict=True)
|
||||
end_index: int | None = Field(default=None, ge=1, strict=True)
|
||||
direction: Literal['forward', 'reverse'] = 'forward'
|
||||
|
||||
|
||||
@@ -40,7 +43,9 @@ def build_mission_planner_router(drafts: MissionDrafts) -> APIRouter:
|
||||
raise HTTPException(409, str(exc)) from exc
|
||||
|
||||
@router.get('/sources/{session_id}')
|
||||
async def source(session_id: str):
|
||||
async def source(session_id: str, generation: str | None = Query(default=None, pattern='^[a-f0-9]{64}$')):
|
||||
if generation is not None:
|
||||
return await call(drafts.sources.bound, session_id, generation)
|
||||
return await call(drafts.sources.get, session_id)
|
||||
|
||||
@router.get('/drafts')
|
||||
|
||||
@@ -0,0 +1,27 @@
|
||||
"""HTTP compression without recompressing native, seekable Rerun streams."""
|
||||
|
||||
from starlette.middleware.gzip import GZipMiddleware
|
||||
from starlette.types import ASGIApp, Receive, Scope, Send
|
||||
|
||||
|
||||
class ResponseCompressionMiddleware:
|
||||
"""Leave RRD bytes/ranges intact; retain gzip for JSON and text assets.
|
||||
|
||||
RRD already compresses its chunks. Gzipping the entire response again burns
|
||||
the event-loop CPU and removes its byte length, delaying native admission.
|
||||
Route suffixes cover recordings, blueprints and point-color overlays alike.
|
||||
"""
|
||||
|
||||
def __init__(
|
||||
self, app: ASGIApp, minimum_size: int = 1024, compresslevel: int = 5
|
||||
) -> None:
|
||||
self.app = app
|
||||
self.compressed_app = GZipMiddleware(
|
||||
app, minimum_size=minimum_size, compresslevel=compresslevel
|
||||
)
|
||||
|
||||
async def __call__(self, scope: Scope, receive: Receive, send: Send) -> None:
|
||||
if scope["type"] == "http" and scope["path"].endswith(".rrd"):
|
||||
await self.app(scope, receive, send)
|
||||
else:
|
||||
await self.compressed_app(scope, receive, send)
|
||||
@@ -9,10 +9,11 @@ from threading import Lock
|
||||
from typing import Annotated, Any, Literal, Protocol
|
||||
from urllib.parse import quote
|
||||
|
||||
from anyio import CancelScope
|
||||
from fastapi import APIRouter, Body, Header, HTTPException, Query, Request, Response
|
||||
from fastapi.responses import FileResponse, JSONResponse, StreamingResponse
|
||||
from pydantic import BaseModel, ConfigDict, Field, StrictBool, field_validator, model_validator
|
||||
from starlette.concurrency import run_in_threadpool
|
||||
from starlette.concurrency import iterate_in_threadpool, run_in_threadpool
|
||||
from starlette.types import Receive, Scope, Send
|
||||
|
||||
from k1link.compute import (
|
||||
@@ -42,7 +43,7 @@ from k1link.sessions.canonical_lab_spatial import (
|
||||
canonical_lab_spatial_frame,
|
||||
)
|
||||
from k1link.sessions.models import SessionSummary
|
||||
from k1link.sessions.plugin_contract import RecordedPointColorRenderer
|
||||
from k1link.sessions.plugin_contract import RecordedPointColorRenderer, RecordedPointDisplayRenderer
|
||||
from k1link.viewer.recorded import (
|
||||
APPLICATION_ID as RECORDED_APPLICATION_ID,
|
||||
)
|
||||
@@ -71,6 +72,12 @@ IMMUTABLE_RECORDING_CACHE_CONTROL = "private, max-age=31536000, immutable, no-tr
|
||||
class _ReleasingFileResponse(FileResponse):
|
||||
"""Release a cache pin exactly once after every ASGI completion path."""
|
||||
|
||||
# A full-fidelity recording can contain hundreds of MB. Starlette's 64 KiB
|
||||
# default incurs thousands of worker/event-loop handoffs, contending with
|
||||
# telemetry and color preparation. Bound each read to 1 MiB, preserving
|
||||
# backpressure, range handling and disconnect pin release.
|
||||
chunk_size = 1024 * 1024
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
*args: Any,
|
||||
@@ -131,6 +138,7 @@ EyeVector = tuple[EyeCoordinate, EyeCoordinate, EyeCoordinate]
|
||||
|
||||
|
||||
class RecordedBlueprintRequest(RecordedBlueprintIdentity):
|
||||
display_point_bank: str | None = Field(default=None, pattern=r"^[a-f0-9]{32}$")
|
||||
accumulation_seconds: float = Field(strict=True, ge=0.0, allow_inf_nan=False)
|
||||
show_points: StrictBool
|
||||
show_trajectory: StrictBool
|
||||
@@ -217,6 +225,12 @@ class RecordedPointColorsRequest(StrictApiModel):
|
||||
custom_color: str = Field(pattern=r"^#[0-9A-Fa-f]{6}$")
|
||||
|
||||
|
||||
class RecordedPointDisplayRequest(RecordedPointColorsRequest):
|
||||
point_decimation_percent: float = Field(gt=0.0, lt=100.0, allow_inf_nan=False)
|
||||
display_bank: str = Field(pattern=r"^[a-f0-9]{32}$")
|
||||
source_generation: str | None = Field(default=None, pattern=r"^[a-f0-9]{64}$")
|
||||
|
||||
|
||||
class SceneSettingsDocument(StrictApiModel):
|
||||
projection: Literal["3d", "2d", "map"]
|
||||
point_size: float = Field(ge=0.1, le=32.0)
|
||||
@@ -231,6 +245,16 @@ class SceneSettingsDocument(StrictApiModel):
|
||||
show_camera_frustums: bool
|
||||
|
||||
|
||||
class SessionSceneSettingsDocument(SceneSettingsDocument):
|
||||
point_decimation_percent: float = Field(default=0.0, ge=0.0, le=100.0, allow_inf_nan=False)
|
||||
accumulation_max_seconds: float = Field(default=180.0, ge=1.0, allow_inf_nan=False)
|
||||
accumulation_seconds: float = Field(ge=0.0, allow_inf_nan=False)
|
||||
|
||||
|
||||
class SessionDisplayRequest(StrictApiModel):
|
||||
scene_settings: SessionSceneSettingsDocument
|
||||
|
||||
|
||||
class LegacyToolWindowsDocument(StrictApiModel):
|
||||
sources_open: bool
|
||||
display_open: bool
|
||||
@@ -375,6 +399,7 @@ def build_session_router(
|
||||
perception_overlay_provider: RecordedPerceptionOverlayProvider | None = None,
|
||||
perception_media_provider: RecordedPerceptionMediaProvider | None = None,
|
||||
point_color_renderers: Mapping[str, RecordedPointColorRenderer] | None = None,
|
||||
point_display_renderers: Mapping[str, RecordedPointDisplayRenderer] | None = None,
|
||||
lab_calculation_profile_resolver: (
|
||||
Callable[[SessionSummary], Mapping[str, object] | None] | None
|
||||
) = None,
|
||||
@@ -386,6 +411,33 @@ def build_session_router(
|
||||
router = APIRouter(tags=["observation-sessions"])
|
||||
recorded_media_inspector = media_inspector or RecordedMediaInspector()
|
||||
|
||||
@router.get("/api/v1/observation-sessions/{session_id}/display-profile")
|
||||
def get_session_display_profile(session_id: str):
|
||||
from k1link.sessions.display_profile import load_display_profile
|
||||
try:
|
||||
document = load_display_profile(store.display_profile_root(session_id), session_id)
|
||||
if document is not None:
|
||||
document["scene_settings"] = SessionSceneSettingsDocument.model_validate(
|
||||
document["scene_settings"]).model_dump()
|
||||
return JSONResponse(document, headers={"Cache-Control": "no-store"})
|
||||
except SessionNotFoundError as exc:
|
||||
raise HTTPException(404, str(exc)) from exc
|
||||
except (OSError, ValueError, KeyError, SessionIntegrityError) as exc:
|
||||
raise HTTPException(409, "Настройки записи недоступны.") from exc
|
||||
|
||||
@router.put("/api/v1/observation-sessions/{session_id}/display-profile")
|
||||
def put_session_display_profile(session_id: str, request: SessionDisplayRequest):
|
||||
from k1link.sessions.display_profile import save_display_profile
|
||||
try:
|
||||
settings = request.scene_settings.model_dump()
|
||||
settings["accumulation_max_seconds"] = max(
|
||||
settings["accumulation_max_seconds"], settings["accumulation_seconds"])
|
||||
return save_display_profile(store.display_profile_root(session_id), session_id, settings)
|
||||
except SessionNotFoundError as exc:
|
||||
raise HTTPException(404, str(exc)) from exc
|
||||
except (OSError, ValueError, SessionIntegrityError) as exc:
|
||||
raise HTTPException(409, "Не удалось сохранить настройки записи.") from exc
|
||||
|
||||
def lab_catalog_document(
|
||||
summary: SessionSummary,
|
||||
contract: Literal["v1", "v2", "v3"],
|
||||
@@ -407,7 +459,7 @@ def build_session_router(
|
||||
def list_observation_sessions(
|
||||
limit: int = Query(default=20, ge=1, le=100),
|
||||
cursor: str | None = Query(default=None, max_length=128),
|
||||
scope: Literal["all", "source", "laboratory"] = "all",
|
||||
scope: Literal["all", "source", "standalone", "laboratory"] = "all",
|
||||
lab_contract: Literal["v1", "v2", "v3"] = "v1",
|
||||
pagination: Literal["cursor-v1"] | None = None,
|
||||
) -> dict[str, Any]:
|
||||
@@ -586,7 +638,7 @@ def build_session_router(
|
||||
if recording_preparation_manager is not None:
|
||||
try:
|
||||
snapshot = recording_preparation_manager.status(command.session_id)
|
||||
if snapshot is None:
|
||||
if snapshot is None or snapshot.command.map_version != command.map_version:
|
||||
snapshot = await run_in_threadpool(
|
||||
recording_preparation_manager.restore_published,
|
||||
command,
|
||||
@@ -1088,6 +1140,7 @@ def build_session_router(
|
||||
show_grid=request.show_grid,
|
||||
),
|
||||
application_id=RECORDED_APPLICATION_ID,
|
||||
display_point_bank=request.display_point_bank,
|
||||
recording_id=request.recording_id,
|
||||
blueprint_session_id=request.blueprint_session_id,
|
||||
active_view=request.active_view,
|
||||
@@ -1160,7 +1213,7 @@ def build_session_router(
|
||||
if perception_overlay_provider is None:
|
||||
return Response(status_code=204, headers={"Cache-Control": "no-store"})
|
||||
try:
|
||||
await run_in_threadpool(
|
||||
command = await run_in_threadpool(
|
||||
_prepare_replay,
|
||||
store,
|
||||
catalog_refresher,
|
||||
@@ -1169,6 +1222,8 @@ def build_session_router(
|
||||
False,
|
||||
False,
|
||||
)
|
||||
if command.map_version is not None:
|
||||
raise HTTPException(409, "Слой распознавания относится к исходной геометрии записи.")
|
||||
materializer = getattr(perception_overlay_provider, "materialize", None)
|
||||
payload = await run_in_threadpool(
|
||||
materializer if callable(materializer) else perception_overlay_provider.render,
|
||||
@@ -1339,6 +1394,70 @@ def build_session_router(
|
||||
},
|
||||
)
|
||||
|
||||
@router.post("/api/v1/observation-sessions/{session_id}/point-display.rrd")
|
||||
async def get_observation_session_point_display(session_id: str, request: RecordedPointDisplayRequest):
|
||||
release = None
|
||||
iterator = None
|
||||
try:
|
||||
command = await run_in_threadpool(_prepare_replay, store, catalog_refresher,
|
||||
session_id, 1.0, False, False)
|
||||
prepared_path = None
|
||||
if request.source_generation is not None:
|
||||
manager = recording_preparation_manager
|
||||
if manager is None:
|
||||
raise HTTPException(409, "Подготовленная запись недоступна.")
|
||||
snapshot = manager.status(session_id)
|
||||
if snapshot is None:
|
||||
snapshot = await run_in_threadpool(manager.restore_published, command)
|
||||
if snapshot is None or snapshot.state != "ready" or snapshot.recording is None:
|
||||
raise HTTPException(409, "Подготовленная запись недоступна.")
|
||||
_require_matching_recording_generation(snapshot.recording.sha256, request.source_generation)
|
||||
pinned = manager.pin_ready(session_id, preparation_id=snapshot.preparation_id)
|
||||
if pinned is None:
|
||||
raise HTTPException(412, "Подготовленная запись была заменена.")
|
||||
snapshot, release = pinned
|
||||
if snapshot.recording is None:
|
||||
raise HTTPException(409, "Подготовленная запись недоступна.")
|
||||
_require_matching_recording_generation(snapshot.recording.sha256, request.source_generation)
|
||||
command = snapshot.command
|
||||
prepared_path = snapshot.recording.path
|
||||
renderer = (point_display_renderers or {}).get(command.plugin_id)
|
||||
if renderer is None:
|
||||
raise HTTPException(409, "Прореживание этой записи недоступно.")
|
||||
options = request.model_dump(exclude={"source_generation"})
|
||||
if prepared_path is not None:
|
||||
options["prepared_recording_path"] = prepared_path
|
||||
iterator = renderer(command, **options)
|
||||
first = await run_in_threadpool(next, iterator)
|
||||
except BaseException as exc:
|
||||
with CancelScope(shield=True):
|
||||
try:
|
||||
if iterator is not None:
|
||||
await run_in_threadpool(iterator.close)
|
||||
finally:
|
||||
if release is not None:
|
||||
await run_in_threadpool(release)
|
||||
if isinstance(exc, SessionNotFoundError):
|
||||
raise HTTPException(404, str(exc)) from exc
|
||||
if isinstance(exc, (SessionNotReplayableError, SessionIntegrityError, ValueError, RuntimeError)):
|
||||
raise HTTPException(409, "Не удалось подготовить прореживание записи.") from exc
|
||||
raise
|
||||
|
||||
async def chunks():
|
||||
try:
|
||||
yield first
|
||||
async for chunk in iterate_in_threadpool(iterator):
|
||||
yield chunk
|
||||
finally:
|
||||
with CancelScope(shield=True):
|
||||
try:
|
||||
await run_in_threadpool(iterator.close)
|
||||
finally:
|
||||
if release is not None:
|
||||
await run_in_threadpool(release)
|
||||
return StreamingResponse(chunks(), media_type="application/vnd.nodedc.point-display-stream",
|
||||
headers={"Cache-Control": "no-store, no-transform"})
|
||||
|
||||
@router.get("/api/v1/observation-sessions/{session_id}/perception-media/{result_id}/manifest")
|
||||
def get_recorded_perception_media_manifest(
|
||||
session_id: str,
|
||||
@@ -1860,6 +1979,7 @@ def _recording_launch_document(
|
||||
"seekable": True,
|
||||
"byte_length": recording.byte_length,
|
||||
"sha256": recording.sha256,
|
||||
**({"map_generation": command.map_version.generation} if command.map_version is not None else {}),
|
||||
"playback": {
|
||||
"speed": command.speed,
|
||||
"loop": command.loop,
|
||||
|
||||
@@ -1,59 +1,93 @@
|
||||
import json
|
||||
from typing import Literal
|
||||
|
||||
from fastapi import APIRouter, HTTPException, Query
|
||||
from fastapi.responses import FileResponse, Response
|
||||
from pydantic import BaseModel, ConfigDict, Field
|
||||
from typing import Literal
|
||||
import json
|
||||
from starlette.concurrency import run_in_threadpool
|
||||
|
||||
from k1link.sessions.models import SessionNotFoundError, SessionStoreError
|
||||
from k1link.sessions.recording import RecordingMaterializationError
|
||||
from k1link.sessions.overview import SessionOverviewService
|
||||
from k1link.sessions.overview_spatial import spatial_metadata, render_spatial_update
|
||||
from k1link.sessions.overview_spatial import render_spatial_update, spatial_metadata
|
||||
from k1link.sessions.recording import RecordingMaterializationError
|
||||
|
||||
|
||||
class OverviewSpatialRequest(BaseModel):
|
||||
model_config = ConfigDict(extra='forbid')
|
||||
generation: str = Field(pattern='^[a-f0-9]{64}$')
|
||||
model_config = ConfigDict(extra="forbid")
|
||||
generation: str = Field(pattern="^[a-f0-9]{64}$")
|
||||
ceiling_m: float | None = Field(default=None, allow_inf_nan=False)
|
||||
mode: Literal['3d', 'top'] | None = None
|
||||
aspect: float = Field(default=1.5, ge=.1, le=20, allow_inf_nan=False)
|
||||
mode: Literal["3d", "top"] | None = None
|
||||
comparison_generation: str | None = Field(default=None, pattern="^[a-f0-9]{64}$")
|
||||
representation: Literal["original", "corrected"] = "original"
|
||||
aspect: float = Field(default=1.5, ge=0.1, le=20, allow_inf_nan=False)
|
||||
|
||||
|
||||
def build_session_overview_router(service: SessionOverviewService) -> APIRouter:
|
||||
router = APIRouter(prefix='/api/v1/observation-sessions')
|
||||
router = APIRouter(prefix="/api/v1/observation-sessions")
|
||||
|
||||
async def call(operation, *args):
|
||||
try:
|
||||
return await run_in_threadpool(operation, *args)
|
||||
except SessionNotFoundError as exc:
|
||||
raise HTTPException(404, 'Запись не найдена.') from exc
|
||||
raise HTTPException(404, "Запись не найдена.") from exc
|
||||
except (ValueError, OSError, SessionStoreError, RecordingMaterializationError) as exc:
|
||||
raise HTTPException(409, 'Исходные данные записи недоступны или изменились.') from exc
|
||||
raise HTTPException(409, "Исходные данные записи недоступны или изменились.") from exc
|
||||
|
||||
@router.get('/{session_id}/overview')
|
||||
@router.get("/{session_id}/overview")
|
||||
async def overview(session_id: str):
|
||||
return await call(service.get, session_id)
|
||||
|
||||
@router.post('/{session_id}/overview/retry')
|
||||
@router.post("/{session_id}/overview/retry")
|
||||
async def retry(session_id: str):
|
||||
return await call(service.retry, session_id)
|
||||
|
||||
@router.get('/{session_id}/overview/scene.rrd')
|
||||
async def scene(session_id: str, generation: str = Query(pattern='^[a-f0-9]{64}$')):
|
||||
@router.get("/{session_id}/overview/scene.rrd")
|
||||
async def scene(session_id: str, generation: str = Query(pattern="^[a-f0-9]{64}$")):
|
||||
path = await call(service.scene, session_id, generation)
|
||||
return FileResponse(path, media_type='application/octet-stream', headers={'Cache-Control': 'private, no-cache, no-transform'})
|
||||
return FileResponse(
|
||||
path,
|
||||
media_type="application/octet-stream",
|
||||
headers={"Cache-Control": "private, no-cache, no-transform"},
|
||||
)
|
||||
|
||||
@router.get('/{session_id}/overview/spatial')
|
||||
async def spatial(session_id: str, generation: str = Query(pattern='^[a-f0-9]{64}$')):
|
||||
@router.get("/{session_id}/overview/spatial")
|
||||
async def spatial(
|
||||
session_id: str,
|
||||
generation: str = Query(pattern="^[a-f0-9]{64}$"),
|
||||
reference_generation: str | None = Query(default=None, pattern="^[a-f0-9]{64}$"),
|
||||
):
|
||||
path = await call(service.scene, session_id, generation)
|
||||
return await call(spatial_metadata, path)
|
||||
comparison = await call(service.comparison, session_id, generation)
|
||||
representation = await call(
|
||||
service.default_representation, session_id, comparison, reference_generation
|
||||
)
|
||||
return {
|
||||
**await call(spatial_metadata, path, comparison),
|
||||
"default_representation": representation,
|
||||
}
|
||||
|
||||
@router.post('/{session_id}/overview/spatial')
|
||||
@router.post("/{session_id}/overview/spatial")
|
||||
async def spatial_update(session_id: str, request: OverviewSpatialRequest):
|
||||
path = await call(service.scene, session_id, request.generation)
|
||||
data, visible, eye = await call(render_spatial_update, path, request.ceiling_m, request.mode, request.aspect)
|
||||
headers = {'Cache-Control': 'no-store', 'X-Overview-Visible-Points': str(visible)}
|
||||
comparison = (
|
||||
await call(
|
||||
service.comparison, session_id, request.generation, request.comparison_generation
|
||||
)
|
||||
if request.comparison_generation
|
||||
else None
|
||||
)
|
||||
data, visible, eye = await call(
|
||||
render_spatial_update,
|
||||
path,
|
||||
request.ceiling_m,
|
||||
request.mode,
|
||||
request.aspect,
|
||||
comparison,
|
||||
request.representation,
|
||||
)
|
||||
headers = {"Cache-Control": "no-store", "X-Overview-Visible-Points": str(visible)}
|
||||
if eye is not None:
|
||||
headers['X-Overview-Eye'] = json.dumps(eye)
|
||||
return Response(data, media_type='application/octet-stream', headers=headers)
|
||||
headers["X-Overview-Eye"] = json.dumps(eye)
|
||||
return Response(data, media_type="application/octet-stream", headers=headers)
|
||||
|
||||
return router
|
||||
|
||||
Reference in New Issue
Block a user