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

refactor(lab): canonicalize recorded spatial replay

Browse Source
This commit is contained in:
DCCONSTRUCTIONS
2026-08-29 23:38:17 +03:00
parent bd2892140f
commit 74da6437e9
12 changed files with 919 additions and 293 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/k1link/sessions/canonical_lab_spatial.py
+162 -19
View File
@@ -2,9 +2,11 @@
The LAB viewer must not run an independent Rerun transport beside the camera
transport. This adapter reads the immutable recording once, indexes the
recorded source cloud, sensor pose and SLAM trajectory, and returns the latest
source-paced spatial sample in the current body frame. Camera, spatial layers
and the common timeline can therefore be driven by one host clock.
recorded source cloud and sensor pose, estimates the session sensor height from
the initial stationary cloud, and returns both the current increment and a
bounded accumulated local-SLAM cloud in a ground-rebased body frame. Camera,
spatial layers and the common timeline can therefore be driven by one host
clock without a per-LAB coordinate adapter.
"""
from __future__ import annotations
@@ -19,6 +21,7 @@ from typing import Any, Final
import numpy as np
import rerun_bindings as rr_bindings
CANONICAL_LAB_SPATIAL_PROFILE: Final = "source-paced-ground-v2"
_POINT_ENTITY: Final = "/world/points"
_POSE_ENTITY: Final = "/world/sensor_pose"
_TRAJECTORY_ENTITY: Final = "/world/trajectory"
@@ -27,6 +30,16 @@ _POSE_TRANSLATION_COMPONENT: Final = "Transform3D:translation"
_POSE_QUATERNION_COMPONENT: Final = "Transform3D:quaternion"
_TRAJECTORY_COMPONENT: Final = "LineStrips3D:strips"
_INDEX_LOCK: Final = Lock()
_HEIGHT_CALIBRATION_SECONDS: Final = 60.0
_HEIGHT_CALIBRATION_MAX_FRAMES: Final = 120
_HEIGHT_NEAR_MIN_RADIUS_M: Final = 1.0
_HEIGHT_NEAR_MAX_RADIUS_M: Final = 6.0
_HEIGHT_LOWER_QUANTILE: Final = 0.025
_LOCAL_SLAM_HISTORY_SECONDS: Final = 5.0
_LOCAL_SLAM_RADIUS_M: Final = 30.0
_LOCAL_SLAM_VERTICAL_LIMIT_M: Final = 6.0
_LOCAL_SLAM_VOXEL_SIZE_M: Final = 0.12
_LOCAL_SLAM_POINT_LIMIT: Final = 27_000
@dataclass(frozen=True)
@@ -47,6 +60,9 @@ class _CanonicalSpatialIndex:
points: _TimedPoints
poses: _TimedPoses
trajectories: _TimedPoints
sensor_height_m: float
sensor_height_sample_count: int
sensor_height_mad_m: float
def _session_times(batch: Any) -> Any | None:
@@ -159,7 +175,17 @@ def _load_index_cached(
)
if not points.times_ns or not poses.times_ns or not trajectories.times_ns:
raise ValueError("Recorded LAB source has no canonical spatial layers")
return _CanonicalSpatialIndex(points=points, poses=poses, trajectories=trajectories)
sensor_height_m, sensor_height_sample_count, sensor_height_mad_m = (
_estimate_sensor_height(points, poses)
)
return _CanonicalSpatialIndex(
points=points,
poses=poses,
trajectories=trajectories,
sensor_height_m=sensor_height_m,
sensor_height_sample_count=sensor_height_sample_count,
sensor_height_mad_m=sensor_height_mad_m,
)
def _load_index(
@@ -207,12 +233,108 @@ def _map_points_to_body(
return body.astype(np.float32)
def _estimate_sensor_height(points: _TimedPoints, poses: _TimedPoses) -> tuple[float, int, float]:
"""Estimate one session mount height from the initial qualified cloud.
The K1 recording has no explicit physical mount-height entity. The initial
stationary minute is therefore the only admissible automatic calibration
source. A low near-field quantile is measured per source increment and the
session median rejects vegetation/ravine outliers. The result stays
diagnostic and is never promoted to navigation authority by this adapter.
"""
first_time_ns = points.times_ns[0]
calibration_end_ns = first_time_ns + round(_HEIGHT_CALIBRATION_SECONDS * 1_000_000_000)
candidates = [
index
for index, timestamp in enumerate(points.times_ns)
if timestamp <= calibration_end_ns
][:_HEIGHT_CALIBRATION_MAX_FRAMES]
estimates: list[float] = []
for point_index in candidates:
pose_index = _latest_index(poses.times_ns, points.times_ns[point_index])
body = _map_points_to_body(
points.values[point_index],
poses.translations[pose_index],
poses.quaternions_xyzw[pose_index],
)
radius = np.linalg.norm(body[:, :2], axis=1)
eligible = body[
(radius >= _HEIGHT_NEAR_MIN_RADIUS_M)
& (radius <= _HEIGHT_NEAR_MAX_RADIUS_M)
& (body[:, 2] >= -2.0)
& (body[:, 2] <= 0.5)
]
if eligible.shape[0] < 100:
continue
estimate = -float(np.quantile(eligible[:, 2], _HEIGHT_LOWER_QUANTILE))
if 0.08 <= estimate <= 2.5:
estimates.append(estimate)
if len(estimates) < 8:
raise ValueError("Recorded LAB sensor height cannot be estimated from source cloud")
values = np.asarray(estimates, dtype=np.float64)
height = float(np.median(values))
mad = float(np.median(np.abs(values - height)))
return height, len(estimates), mad
def _ground_origin_map(
sensor_origin_map: np.ndarray,
basis_map_from_body: np.ndarray,
sensor_height_m: float,
) -> np.ndarray:
return sensor_origin_map - basis_map_from_body[:, 2] * sensor_height_m
def _map_points_to_ground_body(
points_map: np.ndarray,
ground_origin_map: np.ndarray,
basis_map_from_body: np.ndarray,
) -> np.ndarray:
body = (points_map.astype(np.float64) - ground_origin_map) @ basis_map_from_body
return body.astype(np.float32)
def _bounded_local_slam(
points: _TimedPoints,
target_time_ns: int,
ground_origin_map: np.ndarray,
basis_map_from_body: np.ndarray,
) -> tuple[np.ndarray, int, int]:
start_ns = target_time_ns - round(_LOCAL_SLAM_HISTORY_SECONDS * 1_000_000_000)
first = bisect_right(points.times_ns, start_ns - 1)
last = bisect_right(points.times_ns, target_time_ns)
selected = points.values[first:last]
if not selected:
return np.empty((0, 3), dtype=np.float32), 0, 0
source_count = sum(int(value.shape[0]) for value in selected)
local = _map_points_to_ground_body(
np.concatenate(selected, axis=0),
ground_origin_map,
basis_map_from_body,
)
mask = (
(np.linalg.norm(local[:, :2], axis=1) <= _LOCAL_SLAM_RADIUS_M)
& (np.abs(local[:, 2]) <= _LOCAL_SLAM_VERTICAL_LIMIT_M)
)
local = local[mask]
if local.shape[0] == 0:
return local, len(selected), source_count
voxel = np.floor(local / _LOCAL_SLAM_VOXEL_SIZE_M).astype(np.int32)
_, retained = np.unique(voxel, axis=0, return_index=True)
local = local[np.sort(retained)]
if local.shape[0] > _LOCAL_SLAM_POINT_LIMIT:
stride = int(np.ceil(local.shape[0] / _LOCAL_SLAM_POINT_LIMIT))
local = local[::stride][:_LOCAL_SLAM_POINT_LIMIT]
return np.ascontiguousarray(local, dtype=np.float32), len(selected), source_count
def canonical_lab_spatial_frame(
recording_path: Path,
generation_sha256: str,
target_time_ns: int,
) -> dict[str, object]:
"""Return the latest sealed source cloud and SLAM route on one host time."""
"""Return the current source cloud and bounded Local SLAM on one host time."""
if target_time_ns < 0:
raise ValueError("Recorded LAB target time is invalid")
@@ -229,31 +351,52 @@ def canonical_lab_spatial_frame(
translation = index.poses.translations[pose_index]
quaternion = index.poses.quaternions_xyzw[pose_index]
basis_map_from_body = _rotation_map_from_body(quaternion)
points_body = _map_points_to_body(index.points.values[point_index], translation, quaternion)
trajectory_body = _map_points_to_body(
index.trajectories.values[trajectory_index],
ground_origin = _ground_origin_map(
translation,
quaternion,
basis_map_from_body,
index.sensor_height_m,
)
# The canonical local-SLAM layer is bounded around the vehicle. It must
# never turn into the full world-route "blob" seen in the raw Rerun view.
local_mask = (
(np.abs(trajectory_body[:, 0]) <= 30.0)
& (np.abs(trajectory_body[:, 1]) <= 30.0)
& (np.abs(trajectory_body[:, 2]) <= 6.0)
points_body = _map_points_to_ground_body(
index.points.values[point_index],
ground_origin,
basis_map_from_body,
)
local_slam, local_slam_source_frames, local_slam_source_points = _bounded_local_slam(
index.points,
index.points.times_ns[point_index],
ground_origin,
basis_map_from_body,
)
local_trajectory = trajectory_body[local_mask]
return {
"schema_version": "missioncore.canonical-recorded-lab-spatial-frame/v1",
"schema_version": "missioncore.canonical-recorded-lab-spatial-frame/v2",
"target_time_ns": target_time_ns,
"source_time_ns": index.points.times_ns[point_index],
"pose_time_ns": index.poses.times_ns[pose_index],
"trajectory_time_ns": index.trajectories.times_ns[trajectory_index],
"coordinate_frame": "body-ground",
"sensor_height": {
"meters": index.sensor_height_m,
"source": "initial-source-cloud-lower-quantile-median",
"sample_count": index.sensor_height_sample_count,
"mad_m": index.sensor_height_mad_m,
"authority": "visual-derived",
},
"spatial_profile": {
"profile_id": CANONICAL_LAB_SPATIAL_PROFILE,
"local_slam_history_seconds": _LOCAL_SLAM_HISTORY_SECONDS,
"local_slam_radius_m": _LOCAL_SLAM_RADIUS_M,
"local_slam_voxel_size_m": _LOCAL_SLAM_VOXEL_SIZE_M,
"local_slam_point_limit": _LOCAL_SLAM_POINT_LIMIT,
},
"body_frame": {
"origin_map_xyz_m": translation.tolist(),
"origin_map_xyz_m": ground_origin.tolist(),
"sensor_origin_map_xyz_m": translation.tolist(),
"basis_map_from_body": basis_map_from_body.tolist(),
},
"source_point_count": int(points_body.shape[0]),
"source_points_body_xyz_m": points_body.tolist(),
"local_slam_body_xyz_m": local_trajectory.tolist(),
"local_slam_source_frame_count": local_slam_source_frames,
"local_slam_source_point_count": local_slam_source_points,
"local_slam_point_count": int(local_slam.shape[0]),
"local_slam_body_xyz_m": local_slam.tolist(),
}
src/k1link/web/session_api.py
+9 -2
View File
@@ -37,7 +37,10 @@ from k1link.sessions import (
SessionStore,
validate_recorded_media_timeline,
)
from k1link.sessions.canonical_lab_spatial import canonical_lab_spatial_frame
from k1link.sessions.canonical_lab_spatial import (
CANONICAL_LAB_SPATIAL_PROFILE,
canonical_lab_spatial_frame,
)
from k1link.sessions.plugin_contract import RecordedPointColorRenderer
from k1link.viewer.recorded import (
APPLICATION_ID as RECORDED_APPLICATION_ID,
@@ -832,6 +835,7 @@ def build_session_router(
session_id: str,
generation: Annotated[str, Query(min_length=64, max_length=64)],
time_ns: Annotated[int, Query(ge=0, le=MAX_SAFE_INTEGER)],
profile: Literal["source-paced-ground-v2"],
) -> JSONResponse:
"""Serve one body-frame sample for the canonical recorded-LAB clock.
@@ -890,7 +894,10 @@ def build_session_router(
payload,
headers={
"Cache-Control": "private, max-age=31536000, immutable",
"ETag": f'"{generation}:{payload["source_time_ns"]}"',
"ETag": (
f'"{generation}:{CANONICAL_LAB_SPATIAL_PROFILE}:'
f'{payload["source_time_ns"]}"'
),
"X-Content-Type-Options": "nosniff",
},
)