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

fix(lab): stabilize replay and densify LiDAR overlay

Browse Source
This commit is contained in:
DCCONSTRUCTIONS
2026-08-25 17:51:50 +03:00
parent 15be698097
commit b5080671c9
8 changed files with 520 additions and 52 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/k1link/perception/m48s_replay_timeline.py
+161 -13
View File
@@ -6,10 +6,12 @@ import copy
import json
import math
import statistics
from bisect import bisect_left
from collections import OrderedDict
from dataclasses import dataclass
from itertools import pairwise
from pathlib import Path
from threading import RLock
from threading import Lock
from typing import Final
import numpy as np
@@ -37,6 +39,14 @@ from .threat_timeline import (
FRAME_EVIDENCE_SCHEMA: Final = "missioncore.m48s-reference-graph-frame-evidence/v0"
EXPECTED_FRAME_COUNT: Final = 4_489
CAMERA_ACCUMULATION_WINDOW_SECONDS: Final = 2.0
CAMERA_ACCUMULATION_POINT_LIMIT: Final = 20_000
CAMERA_POINT_OVERLAY_SCHEMA: Final = "missioncore.m48s-camera-point-overlay/v1"
_FRAME_EVIDENCE_SCHEMA_MARKER: Final = (
b'"schema_version":"missioncore.m48s-reference-graph-frame-evidence/v0"'
)
_SOURCE_ENVELOPE_MARKER: Final = b'"source_envelope":'
_JSON_DECODER: Final = json.JSONDecoder()
class M48sReplayTimelineError(RuntimeError):
@@ -87,7 +97,9 @@ class M48sReplayTimeline:
worker = _object(json.loads(self.worker_path.read_text("utf-8")), "worker result")
self.outcomes = _terminal_outcomes(worker)
self.index = _index_ledger(self.frames_path, self.source_times_ns, self.outcomes)
self._lock = RLock()
self._cache_lock = Lock()
self._chunk_json_cache: OrderedDict[tuple[int, int], bytes] = OrderedDict()
self._camera_point_json_cache: OrderedDict[int, bytes] = OrderedDict()
def metadata(self) -> dict[str, object]:
intervals = [
@@ -114,11 +126,14 @@ class M48sReplayTimeline:
"point_sample_limit": RECORDED_SPATIAL_POINT_LIMIT,
"maximum_source_points_per_frame": self.store.maximum_current_point_count,
"point_delivery": "exact-current-increment",
"camera_point_delivery": "factory-kb4-projected-current-increment",
"camera_point_sample_limit": RECORDED_SPATIAL_POINT_LIMIT,
"camera_point_delivery": "factory-kb4-causal-registered-accumulation",
"camera_point_window_seconds": CAMERA_ACCUMULATION_WINDOW_SECONDS,
"camera_point_sample_limit": CAMERA_ACCUMULATION_POINT_LIMIT,
"world_state_delivery": "source-paced-latest-wins",
"world_state_frame_count": len(self.index.offsets_by_sequence),
"superseded_frame_count": sum(value == "superseded" for value in self.outcomes.values()),
"superseded_frame_count": sum(
value == "superseded" for value in self.outcomes.values()
),
"local_surface_visualization": {
"derivation": "bounded-registered-increment-accumulation",
"window_seconds": RECORDED_LOCAL_SURFACE_WINDOW_SECONDS,
@@ -155,8 +170,7 @@ class M48sReplayTimeline:
if not 1 <= frame_count <= RECORDED_SPATIAL_MAX_CHUNK_FRAMES:
raise M48sReplayTimelineError("M4.8S timeline chunk size is invalid")
stop = min(EXPECTED_FRAME_COUNT, start_sequence + frame_count)
with self._lock:
frames = [self._project_frame(sequence) for sequence in range(start_sequence, stop)]
frames = [self._project_frame(sequence) for sequence in range(start_sequence, stop)]
return {
"schema_version": RECORDED_SPATIAL_CHUNK_SCHEMA,
"result_id": self.result_id,
@@ -169,6 +183,110 @@ class M48sReplayTimeline:
"access": "read-only-bounded-recorded-replay",
}
def chunk_json(self, *, start_sequence: int, frame_count: int) -> bytes:
"""Return one bounded immutable chunk without repeating JSON encoding."""
key = (start_sequence, frame_count)
with self._cache_lock:
cached = self._chunk_json_cache.get(key)
if cached is not None:
self._chunk_json_cache.move_to_end(key)
return cached
content = json.dumps(
self.chunk(start_sequence=start_sequence, frame_count=frame_count),
ensure_ascii=False,
separators=(",", ":"),
).encode("utf-8")
with self._cache_lock:
self._chunk_json_cache[key] = content
self._chunk_json_cache.move_to_end(key)
while len(self._chunk_json_cache) > 12:
self._chunk_json_cache.popitem(last=False)
return content
def camera_point_overlay_json(self, *, sequence: int) -> bytes:
"""Project causal registered LiDAR increments into the current camera.
Every rendered point comes from a sealed map-frame increment at or before
``sequence``. Accumulation is visualization-only: it increases static
surface density but can leave short trails behind moving objects.
"""
if not 0 <= sequence < EXPECTED_FRAME_COUNT:
raise M48sReplayTimelineError("M4.8S camera point sequence is invalid")
with self._cache_lock:
cached = self._camera_point_json_cache.get(sequence)
if cached is not None:
self._camera_point_json_cache.move_to_end(sequence)
return cached
current = self.store.frame_for_index(sequence)
source_time_ns = self.source_times_ns[sequence]
window_ns = round(CAMERA_ACCUMULATION_WINDOW_SECONDS * 1_000_000_000)
first_sequence = bisect_left(self.source_times_ns, source_time_ns - window_ns)
source_frames = []
source_point_count = 0
if current is not None:
for source_sequence in range(first_sequence, sequence + 1):
source = self.store.frame_for_index(source_sequence)
if source is None or source.points_map.size == 0:
continue
source_frames.append(source.points_map)
source_point_count += source.source_point_count
points: list[list[float]] = []
front_point_count = 0
projected_point_count = 0
if current is not None and source_frames:
accumulated = np.concatenate(source_frames, axis=0)
projected = project_map_points_kb4(
accumulated,
position_map_xyz=current.sensor_position_map,
orientation_map_from_lidar_xyzw=current.sensor_orientation_xyzw,
profile=current.projection,
)
front_point_count = projected.camera_front_point_count
projected_point_count = projected.projected_point_count
sample_count = min(projected_point_count, CAMERA_ACCUMULATION_POINT_LIMIT)
indices = np.linspace(
0,
projected_point_count - 1,
num=sample_count,
dtype=np.int64,
)
if indices.size:
xy = projected.pixels_xy[indices]
depth = projected.depths_m[indices, None]
points = np.round(np.concatenate((xy, depth), axis=1), 2).tolist()
payload = {
"schema_version": CAMERA_POINT_OVERLAY_SCHEMA,
"result_id": self.result_id,
"sequence": sequence,
"source_time_ns": source_time_ns,
"points_xyd": points,
"source_frame_count": len(source_frames),
"source_point_count": source_point_count,
"front_point_count": front_point_count,
"projected_point_count": projected_point_count,
"sample_count": len(points),
"window_seconds": CAMERA_ACCUMULATION_WINDOW_SECONDS,
"projection": "factory-kb4-causal-registered-accumulation",
"ground_truth": False,
"authority": "visual-derived",
"access": "read-only-bounded-recorded-replay",
}
content = json.dumps(
payload,
ensure_ascii=False,
separators=(",", ":"),
).encode("utf-8")
with self._cache_lock:
self._camera_point_json_cache[sequence] = content
self._camera_point_json_cache.move_to_end(sequence)
while len(self._camera_point_json_cache) > 32:
self._camera_point_json_cache.popitem(last=False)
return content
def _project_frame(self, sequence: int) -> dict[str, object]:
terminal_outcome = self.outcomes[sequence]
row = self._row(sequence)
@@ -273,7 +391,9 @@ class M48sReplayTimeline:
"semantic_hint": proposal.get("semantic_hint"),
"occupied_support": proposal_id in associated,
"range_m": None,
"threat_decision": None if assessment is None else assessment.get("decision"),
"threat_decision": None
if assessment is None
else assessment.get("decision"),
"threat_reason_codes": []
if assessment is None
else assessment.get("reason_codes"),
@@ -344,10 +464,7 @@ def _index_ledger(
line = stream.readline()
if not line:
break
row = json.loads(line)
if not isinstance(row, dict) or row.get("schema_version") != FRAME_EVIDENCE_SCHEMA:
raise M48sReplayTimelineError("M4.8S ledger schema changed")
envelope = _object(row.get("source_envelope"), "source envelope")
envelope = _ledger_source_envelope(line)
timestamps = _object(envelope.get("timestamps"), "source timestamps")
sequence = envelope.get("sequence")
if (
@@ -366,6 +483,31 @@ def _index_ledger(
return _LedgerIndex(offsets)
def _ledger_source_envelope(line: bytes) -> dict[str, object]:
"""Validate a ledger row while decoding only its small trailing envelope.
The full row can exceed 100 KiB because it contains the delivered world
state. Indexing needs only the sealed top-level schema and source binding;
decoding the complete 473 MiB ledger on every backend start needlessly holds
the GIL for many seconds.
"""
if (
line.count(_FRAME_EVIDENCE_SCHEMA_MARKER) != 1
or line.count(_SOURCE_ENVELOPE_MARKER) != 1
):
raise M48sReplayTimelineError("M4.8S ledger schema changed")
start = line.find(_SOURCE_ENVELOPE_MARKER) + len(_SOURCE_ENVELOPE_MARKER)
try:
tail = line[start:].decode("utf-8")
value, end = _JSON_DECODER.raw_decode(tail)
except (UnicodeDecodeError, json.JSONDecodeError):
raise M48sReplayTimelineError("M4.8S ledger source envelope is invalid") from None
if tail[end:].strip() != "}":
raise M48sReplayTimelineError("M4.8S ledger source envelope moved")
return _object(value, "source envelope")
def _terminal_outcomes(worker: dict[str, object]) -> dict[int, str]:
execution = _object(worker.get("execution"), "execution")
loops = execution.get("loops")
@@ -423,4 +565,10 @@ def _text(value: object, label: str) -> str:
return value
__all__ = ["M48sReplayTimeline", "M48sReplayTimelineError"]
__all__ = [
"CAMERA_ACCUMULATION_POINT_LIMIT",
"CAMERA_ACCUMULATION_WINDOW_SECONDS",
"CAMERA_POINT_OVERLAY_SCHEMA",
"M48sReplayTimeline",
"M48sReplayTimelineError",
]