feat: add passive K1 local surface replay

2026-07-25 22:47:12 +03:00
parent cfe17e49bb
commit 04a658b218
16 changed files with 2715 additions and 21 deletions
@@ -0,0 +1,195 @@
+from __future__ import annotations
+
+import hashlib
+import json
+from pathlib import Path
+
+import numpy as np
+from fastapi import APIRouter
+from fastapi.routing import APIRoute
+
+from k1link.compute import (
+    E10_LIDAR_PACK_SCHEMA,
+    E10LidarFieldSource,
+    K1LocalSurfaceProfile,
+    K1LocalSurfaceV1,
+    build_k1_local_surface,
+)
+from k1link.web.lidar_api import build_lidar_router
+
+
+def _canonical_json(value: object) -> bytes:
+    return json.dumps(
+        value,
+        ensure_ascii=False,
+        sort_keys=True,
+        separators=(",", ":"),
+        allow_nan=False,
+    ).encode()
+
+
+def _sha256(path: Path) -> str:
+    return hashlib.sha256(path.read_bytes()).hexdigest()
+
+
+def _source_pack(root: Path) -> Path:
+    frame_count = 8
+    available = np.asarray([True, True, True, True, True, True, True, False])
+    poses: list[list[float]] = []
+    clouds: list[np.ndarray] = []
+    offsets = [0]
+    for frame_index in range(frame_count):
+        pose_x = frame_index * 0.35
+        pose_y = 0.1 * np.sin(frame_index)
+        ground_z = 0.04 * pose_x - 0.015 * pose_y
+        poses.append([pose_x, pose_y, ground_z + 1.42])
+        if not available[frame_index]:
+            offsets.append(offsets[-1])
+            continue
+        axis = np.linspace(-3.5, 3.5, 12)
+        xx, yy = np.meshgrid(axis + pose_x, axis + pose_y)
+        zz = 0.04 * xx - 0.015 * yy + 0.008 * np.sin(xx * 2 + frame_index)
+        ground = np.column_stack((xx.ravel(), yy.ravel(), zz.ravel()))
+        obstacle_xy = ground[::13, :2]
+        obstacle_z = (
+            0.04 * obstacle_xy[:, 0] - 0.015 * obstacle_xy[:, 1] + 0.75
+        )
+        obstacle = np.column_stack((obstacle_xy, obstacle_z))
+        cloud = np.concatenate((ground, obstacle)).astype("<f4")
+        clouds.append(cloud)
+        offsets.append(offsets[-1] + cloud.shape[0])
+    points = np.concatenate(clouds).astype("<f4")
+    arrays = {
+        "frame_indices": np.arange(frame_count, dtype="<i8"),
+        "source_frame_indices": np.arange(100, 100 + frame_count * 10, 10, dtype="<i8"),
+        "session_seconds": np.arange(frame_count, dtype="<f8") * 0.1 + 10.0,
+        "sample_available": available.astype("?"),
+        "cloud_offsets": np.asarray(offsets, dtype="<i8"),
+        "cloud_points_map": points,
+        "pose_positions_map": np.asarray(poses, dtype="<f8"),
+        "pose_quaternions_map_from_lidar": np.tile(
+            np.asarray([0.0, 0.0, 0.0, 1.0], dtype="<f8"),
+            (frame_count, 1),
+        ),
+        "lidar_camera_delta_ms": np.zeros(frame_count, dtype="<f8"),
+        "pose_point_delta_ms": np.asarray(
+            [4.0, 5.0, 6.0, 7.0, 130.0, 5.0, 6.0, 0.0],
+            dtype="<f8",
+        ),
+        "intrinsic_fx_fy_cx_cy": np.asarray(
+            [100.0, 100.0, 50.0, 50.0],
+            dtype="<f8",
+        ),
+        "distortion_kb4": np.zeros(4, dtype="<f8"),
+        "t_camera_from_lidar": np.eye(4, dtype="<f8"),
+    }
+    identity = {
+        "schema_version": E10_LIDAR_PACK_SCHEMA,
+        "session_id": "synthetic-ravnoves00-local-surface",
+        "frame_count": frame_count,
+        "available_lidar_frames": int(np.count_nonzero(available)),
+        "point_count": int(points.shape[0]),
+        "timeline_start_seconds": 10.0,
+        "timeline_end_seconds": 10.7,
+    }
+    identity_sha256 = hashlib.sha256(_canonical_json(identity)).hexdigest()
+    pack = root / f"e10-lidar-pack-{identity_sha256}"
+    pack.mkdir(parents=True)
+    arrays_path = pack / "lidar-pack.npz"
+    np.savez_compressed(arrays_path, **arrays)  # type: ignore[arg-type]
+    manifest = {
+        "schema_version": E10_LIDAR_PACK_SCHEMA,
+        "pack_id": pack.name,
+        "identity_sha256": identity_sha256,
+        "identity": identity,
+        "artifact": {
+            "path": arrays_path.name,
+            "media_type": "application/x-npz",
+            "byte_length": arrays_path.stat().st_size,
+            "sha256": _sha256(arrays_path),
+        },
+    }
+    (pack / "manifest.json").write_bytes(_canonical_json(manifest))
+    return pack
+
+
+def _endpoint(router: APIRouter, path: str) -> object:
+    for route in router.routes:
+        if (
+            isinstance(route, APIRoute)
+            and route.path == path
+            and route.methods is not None
+            and "GET" in route.methods
+        ):
+            return route.endpoint
+    raise AssertionError(f"GET {path} route is missing")
+
+
+def test_k1_local_surface_is_dynamic_source_bound_and_read_only(
+    tmp_path: Path,
+) -> None:
+    source_path = _source_pack(tmp_path / "source")
+    source_artifact = source_path / "lidar-pack.npz"
+    source_sha256 = _sha256(source_artifact)
+    profile = K1LocalSurfaceProfile(
+        profile_id="synthetic-dynamic-local-surface/v1",
+        local_radius_m=5.0,
+        cell_size_m=0.5,
+        surface_ttl_s=0.5,
+        minimum_surface_cells=12,
+    )
+    source = E10LidarFieldSource(source_path)
+    try:
+        output = build_k1_local_surface(
+            source,
+            tmp_path / "models",
+            profile=profile,
+        )
+        duplicate = build_k1_local_surface(
+            source,
+            tmp_path / "models",
+            profile=profile,
+        )
+    finally:
+        source.close()
+    assert duplicate == output
+    assert _sha256(source_artifact) == source_sha256
+
+    model = K1LocalSurfaceV1(output)
+    source = E10LidarFieldSource(source_path)
+    try:
+        detail = model.frame_detail(source, 2)
+        assert model.report["source"]["passive_processing_only"] is True
+        assert model.report["source"]["firmware_or_device_commands_used"] is False
+        assert model.report["surface_model"]["hardcoded_height_m"] is None
+        assert model.report["occupancy_policy"]["absence_of_points_means_free"] is False
+        assert model.report["metrics"]["frames"]["valid"] >= 5
+        assert detail["valid"] is True
+        assert 1.2 < detail["surface"]["sensor_height_m"] < 1.6
+        assert detail["counts"]["surface"] > 50
+        assert detail["counts"]["occupied"] > 0
+        assert detail["authority"]["commands_enabled"] is False
+        assert "1.27" not in repr({"identity": model.identity, "report": model.report})
+        assert str(tmp_path) not in repr(detail)
+    finally:
+        source.close()
+        model.close()
+
+    router = build_lidar_router(
+        root_provider=lambda: None,
+        ground_root_provider=lambda: None,
+        field_review_root_provider=lambda: None,
+        local_surface_root_provider=lambda: output.parent,
+        e10_source_root_provider=lambda: source_path.parent,
+    )
+    catalog_route = _endpoint(router, "/api/v1/lidar/local-surfaces")
+    frame_route = _endpoint(
+        router,
+        "/api/v1/lidar/local-surfaces/{model_id}/frames/{frame_index}",
+    )
+    catalog = catalog_route(limit=10)  # type: ignore[operator]
+    frame = frame_route(model_id=output.name, frame_index=2)  # type: ignore[operator]
+    assert catalog["valid_total"] == 1
+    assert catalog["items"][0]["status"] == "diagnostic-only"
+    assert frame["model_id"] == output.name
+    assert frame["access"] == "read-only"