refactor(lab): canonicalize recorded spatial replay

tests/test_canonical_lab_spatial.py

@@ -0,0 +1,71 @@
+from __future__ import annotations
+
+import numpy as np
+import pytest
+
+from k1link.sessions.canonical_lab_spatial import (
+    _TimedPoints,
+    _TimedPoses,
+    _bounded_local_slam,
+    _estimate_sensor_height,
+    _ground_origin_map,
+)
+
+
+def _calibration_cloud(height_m: float, seed: int) -> np.ndarray:
+    rng = np.random.default_rng(seed)
+    xy = rng.uniform(-5.5, 5.5, size=(500, 2)).astype(np.float32)
+    radius = np.linalg.norm(xy, axis=1)
+    xy = xy[(radius >= 1.0) & (radius <= 5.5)][:360]
+    ground = np.column_stack((
+        xy,
+        rng.normal(-height_m, 0.006, size=xy.shape[0]),
+    )).astype(np.float32)
+    vegetation = np.column_stack((
+        rng.uniform(-5, 5, size=(300, 2)),
+        rng.uniform(0.0, 1.2, size=300),
+    )).astype(np.float32)
+    return np.concatenate((ground, vegetation), axis=0)
+
+
+def test_session_sensor_height_is_derived_from_initial_source_cloud() -> None:
+    times = tuple(index * 500_000_000 for index in range(12))
+    points = _TimedPoints(
+        times_ns=times,
+        values=tuple(_calibration_cloud(0.32, index) for index in range(12)),
+    )
+    poses = _TimedPoses(
+        times_ns=times,
+        translations=tuple(np.zeros(3) for _ in times),
+        quaternions_xyzw=tuple(np.asarray([0.0, 0.0, 0.0, 1.0]) for _ in times),
+    )
+
+    height, sample_count, mad = _estimate_sensor_height(points, poses)
+
+    assert height == pytest.approx(0.32, abs=0.02)
+    assert sample_count == 12
+    assert mad < 0.02
+
+
+def test_local_slam_accumulates_source_increments_in_ground_body_frame() -> None:
+    points = _TimedPoints(
+        times_ns=(0, 1_000_000_000, 2_000_000_000),
+        values=(
+            np.asarray([[1.0, 0.0, -0.32]], dtype=np.float32),
+            np.asarray([[2.0, 0.0, -0.32]], dtype=np.float32),
+            np.asarray([[3.0, 0.0, -0.32]], dtype=np.float32),
+        ),
+    )
+    basis = np.eye(3)
+    ground_origin = _ground_origin_map(np.asarray([0.0, 0.0, 0.0]), basis, 0.32)
+
+    local, frame_count, source_count = _bounded_local_slam(
+        points,
+        2_000_000_000,
+        ground_origin,
+        basis,
+    )
+
+    assert frame_count == 3
+    assert source_count == 3
+    assert local[:, 2].tolist() == pytest.approx([0.0, 0.0, 0.0], abs=1e-6)

tests/test_session_api.py

@@ -489,17 +489,36 @@ def test_canonical_lab_spatial_frame_uses_ready_immutable_recording(
     assert resolved is not None and resolved.recording is not None
     generation = hashlib.sha256(payload).hexdigest()
     expected = {
-        "schema_version": "missioncore.canonical-recorded-lab-spatial-frame/v1",
+        "schema_version": "missioncore.canonical-recorded-lab-spatial-frame/v2",
         "target_time_ns": 500_000_000,
         "source_time_ns": 499_000_000,
         "pose_time_ns": 499_000_000,
         "trajectory_time_ns": 490_000_000,
+        "coordinate_frame": "body-ground",
+        "sensor_height": {
+            "meters": 0.32,
+            "source": "initial-source-cloud-lower-quantile-median",
+            "sample_count": 20,
+            "mad_m": 0.03,
+            "authority": "visual-derived",
+        },
+        "spatial_profile": {
+            "profile_id": "source-paced-ground-v2",
+            "local_slam_history_seconds": 5.0,
+            "local_slam_radius_m": 30.0,
+            "local_slam_voxel_size_m": 0.12,
+            "local_slam_point_limit": 27000,
+        },
         "body_frame": {
             "origin_map_xyz_m": [0.0, 0.0, 0.0],
+            "sensor_origin_map_xyz_m": [0.0, 0.0, 0.32],
             "basis_map_from_body": [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]],
         },
         "source_point_count": 1,
         "source_points_body_xyz_m": [[1.0, 2.0, 3.0]],
+        "local_slam_source_frame_count": 1,
+        "local_slam_source_point_count": 1,
+        "local_slam_point_count": 1,
         "local_slam_body_xyz_m": [[0.0, 0.0, 0.0]],
     }
 
@@ -525,9 +544,12 @@ def test_canonical_lab_spatial_frame_uses_ready_immutable_recording(
             session_id=session.name,
             generation=generation,
             time_ns=500_000_000,
+            profile="source-paced-ground-v2",
         ))
         assert json.loads(response.body) == expected
-        assert response.headers["etag"] == f'"{generation}:499000000"'
+        assert response.headers["etag"] == (
+            f'"{generation}:source-paced-ground-v2:499000000"'
+        )
         assert response.headers["cache-control"].endswith("immutable")
     finally:
         manager.close()