529 lines
20 KiB
Python
529 lines
20 KiB
Python
from __future__ import annotations
|
|
|
|
import hashlib
|
|
import json
|
|
import os
|
|
import time
|
|
from pathlib import Path
|
|
|
|
import numpy as np
|
|
import pytest
|
|
from fastapi import APIRouter
|
|
from fastapi.routing import APIRoute
|
|
|
|
from k1link.compute import (
|
|
E10_LIDAR_PACK_SCHEMA,
|
|
E10LidarFieldSource,
|
|
K1LocalSurfaceProfile,
|
|
K1LocalSurfaceShadowInput,
|
|
K1LocalSurfaceShadowRuntime,
|
|
K1LocalSurfaceV1,
|
|
build_k1_local_surface,
|
|
)
|
|
from k1link.compute.lidar_local_surface import (
|
|
DEFAULT_K1_LOCAL_SURFACE_PROFILE as REPLAY_LOCAL_SURFACE_PROFILE,
|
|
)
|
|
from k1link.compute.lidar_local_surface_geometry import (
|
|
DEFAULT_K1_LOCAL_SURFACE_PROFILE as WORKER_LOCAL_SURFACE_PROFILE,
|
|
)
|
|
from k1link.compute.lidar_local_surface_geometry import (
|
|
cloud_cell_observations,
|
|
update_cache,
|
|
update_cache_observations,
|
|
)
|
|
from k1link.web.lidar_api import build_lidar_router
|
|
from k1link.web.lidar_local_surface_service import K1LocalSurfaceReadService
|
|
|
|
|
|
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 test_replay_and_minimal_worker_pin_the_same_local_surface_profile() -> None:
|
|
assert WORKER_LOCAL_SURFACE_PROFILE.to_dict() == REPLAY_LOCAL_SURFACE_PROFILE.to_dict()
|
|
|
|
|
|
def test_precomputed_local_surface_observations_preserve_cache_update() -> None:
|
|
cloud = np.asarray(
|
|
[
|
|
[0.1, 0.1, 0.8],
|
|
[0.2, 0.2, 0.2],
|
|
[1.1, 0.1, 0.5],
|
|
[-0.2, -0.2, -0.1],
|
|
],
|
|
dtype=np.float64,
|
|
)
|
|
baseline: dict[tuple[int, int], tuple[float, float]] = {}
|
|
optimized: dict[tuple[int, int], tuple[float, float]] = {}
|
|
|
|
update_cache(baseline, cloud, 4.25, WORKER_LOCAL_SURFACE_PROFILE)
|
|
keys, points = cloud_cell_observations(cloud, WORKER_LOCAL_SURFACE_PROFILE)
|
|
update_cache_observations(optimized, keys, points, 4.25)
|
|
|
|
assert optimized == baseline
|
|
|
|
|
|
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,
|
|
monkeypatch: pytest.MonkeyPatch,
|
|
) -> 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)
|
|
review = model.review_detail(source)
|
|
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
|
|
temporal = model.report["metrics"]["temporal_qualification"]
|
|
assert temporal["prediction"]["current_frame_excluded"] is True
|
|
assert temporal["prediction"]["sample_count"] >= 4
|
|
assert temporal["prediction"]["residual_p95_m"]["p95"] < 0.05
|
|
assert temporal["stability"]["sample_count"] >= 4
|
|
assert temporal["step_candidates"]["is_ground_truth"] is False
|
|
assert temporal["step_candidates"]["frames_with_candidates"] > 0
|
|
assert detail["valid"] is True
|
|
assert detail["surface"]["local_radius_m"] == 5.0
|
|
assert 1.2 < detail["surface"]["sensor_height_m"] < 1.6
|
|
assert detail["counts"]["surface"] > 50
|
|
assert detail["counts"]["occupied"] > 0
|
|
assert detail["counts"]["step_candidate"] > 0
|
|
assert detail["prediction"]["current_frame_excluded"] is True
|
|
assert detail["prediction"]["available"] is True
|
|
evidence = detail["prediction"]["evidence"]
|
|
assert evidence["available"] is True
|
|
assert evidence["current_frame_excluded_from_plane"] is True
|
|
assert evidence["basis"] == "current-lower-cell-observations"
|
|
assert evidence["ground_truth"] is False
|
|
assert len(evidence["cell_points_xyz_m"]) == detail["prediction"]["cell_count"]
|
|
assert len(evidence["cell_signed_residual_m"]) == detail["prediction"]["cell_count"]
|
|
assert len(evidence["cell_inlier"]) == detail["prediction"]["cell_count"]
|
|
assert (
|
|
sum(evidence["cell_inlier"]) / detail["prediction"]["cell_count"]
|
|
== (detail["prediction"]["inlier_fraction"])
|
|
)
|
|
assert detail["temporal"]["compared"] is True
|
|
assert detail["authority"]["commands_enabled"] is False
|
|
assert review["available"] is True
|
|
assert review["ground_truth"] is False
|
|
assert review["criteria"]["prediction_inlier_fraction_floor"] == 0.85
|
|
assert review["summary"]["item_count"] == len(review["items"])
|
|
assert "1.27" not in repr({"identity": model.identity, "report": model.report})
|
|
assert str(tmp_path) not in repr(detail)
|
|
assert str(tmp_path) not in repr(review)
|
|
finally:
|
|
source.close()
|
|
model.close()
|
|
|
|
read_service = K1LocalSurfaceReadService(tmp_path / "validation-cache")
|
|
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,
|
|
local_surface_read_service=read_service,
|
|
)
|
|
catalog_route = _endpoint(router, "/api/v1/lidar/local-surfaces")
|
|
frame_route = _endpoint(
|
|
router,
|
|
"/api/v1/lidar/local-surfaces/{model_id}/frames/{frame_index}",
|
|
)
|
|
timeline_route = _endpoint(
|
|
router,
|
|
"/api/v1/lidar/local-surfaces/{model_id}/timeline",
|
|
)
|
|
review_route = _endpoint(
|
|
router,
|
|
"/api/v1/lidar/local-surfaces/{model_id}/review",
|
|
)
|
|
catalog = catalog_route(limit=10) # type: ignore[operator]
|
|
frame = frame_route(model_id=output.name, frame_index=2) # type: ignore[operator]
|
|
timeline = timeline_route(model_id=output.name) # type: ignore[operator]
|
|
review = review_route(model_id=output.name) # 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"
|
|
assert timeline["frame_count"] == 8
|
|
assert sum(timeline["prediction_available"]) >= 4
|
|
assert len(timeline["temporal_jump"]) == 8
|
|
assert str(tmp_path) not in repr(timeline)
|
|
assert review["review_profile_id"] == "missioncore-local-surface-attention/v1"
|
|
assert review["access"] == "read-only"
|
|
assert str(tmp_path) not in repr(review)
|
|
read_service.close()
|
|
|
|
def forbidden_strict_open(*_args: object, **_kwargs: object) -> None:
|
|
raise AssertionError("an unchanged admitted generation must restore without strict scan")
|
|
|
|
monkeypatch.setattr(K1LocalSurfaceV1, "__init__", forbidden_strict_open)
|
|
monkeypatch.setattr(E10LidarFieldSource, "__init__", forbidden_strict_open)
|
|
restored_service = K1LocalSurfaceReadService(tmp_path / "validation-cache")
|
|
restored_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,
|
|
local_surface_read_service=restored_service,
|
|
)
|
|
restored_catalog = _endpoint(restored_router, "/api/v1/lidar/local-surfaces")
|
|
restored_timeline = _endpoint(
|
|
restored_router,
|
|
"/api/v1/lidar/local-surfaces/{model_id}/timeline",
|
|
)
|
|
try:
|
|
assert restored_catalog(limit=1)["items"][0]["model_id"] == output.name
|
|
assert restored_timeline(model_id=output.name)["frame_count"] == 8
|
|
finally:
|
|
restored_service.close()
|
|
|
|
source_arrays = source_path / "lidar-pack.npz"
|
|
source_metadata = source_arrays.stat()
|
|
os.utime(
|
|
source_arrays,
|
|
ns=(source_metadata.st_atime_ns, source_metadata.st_mtime_ns + 1),
|
|
)
|
|
invalidated_source_service = K1LocalSurfaceReadService(
|
|
tmp_path / "validation-cache"
|
|
)
|
|
invalidated_source_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,
|
|
local_surface_read_service=invalidated_source_service,
|
|
)
|
|
invalidated_timeline = _endpoint(
|
|
invalidated_source_router,
|
|
"/api/v1/lidar/local-surfaces/{model_id}/timeline",
|
|
)
|
|
try:
|
|
with pytest.raises(AssertionError, match="strict scan"):
|
|
invalidated_timeline(model_id=output.name)
|
|
finally:
|
|
invalidated_source_service.close()
|
|
|
|
model_manifest = output / "manifest.json"
|
|
model_metadata = model_manifest.stat()
|
|
os.utime(
|
|
model_manifest,
|
|
ns=(model_metadata.st_atime_ns, model_metadata.st_mtime_ns + 1),
|
|
)
|
|
invalidated_model_service = K1LocalSurfaceReadService(
|
|
tmp_path / "validation-cache"
|
|
)
|
|
invalidated_model_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,
|
|
local_surface_read_service=invalidated_model_service,
|
|
)
|
|
invalidated_catalog = _endpoint(
|
|
invalidated_model_router,
|
|
"/api/v1/lidar/local-surfaces",
|
|
)
|
|
try:
|
|
with pytest.raises(AssertionError, match="strict scan"):
|
|
invalidated_catalog(limit=1)
|
|
finally:
|
|
invalidated_model_service.close()
|
|
|
|
|
|
def _shadow_input(
|
|
source: E10LidarFieldSource,
|
|
frame_index: int,
|
|
) -> K1LocalSurfaceShadowInput:
|
|
offsets = source.arrays["cloud_offsets"]
|
|
start = int(offsets[frame_index])
|
|
end = int(offsets[frame_index + 1])
|
|
points = np.asarray(
|
|
source.arrays["cloud_points_map"][start:end],
|
|
dtype=np.float64,
|
|
).copy()
|
|
position = np.asarray(
|
|
source.arrays["pose_positions_map"][frame_index],
|
|
dtype=np.float64,
|
|
).copy()
|
|
points.flags.writeable = False
|
|
position.flags.writeable = False
|
|
return K1LocalSurfaceShadowInput(
|
|
frame_index=frame_index,
|
|
source_frame_index=int(source.arrays["source_frame_indices"][frame_index]),
|
|
session_seconds=float(source.arrays["session_seconds"][frame_index]),
|
|
pose_binding_age_ms=abs(float(source.arrays["pose_point_delta_ms"][frame_index])),
|
|
points_map=points,
|
|
position_map=position,
|
|
published_monotonic_ns=time.monotonic_ns(),
|
|
)
|
|
|
|
|
|
def test_k1_local_surface_shadow_matches_replay_and_stays_non_authoritative(
|
|
tmp_path: Path,
|
|
) -> None:
|
|
source_path = _source_pack(tmp_path / "source")
|
|
profile = K1LocalSurfaceProfile(
|
|
profile_id="synthetic-shadow-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,
|
|
)
|
|
finally:
|
|
source.close()
|
|
|
|
source = E10LidarFieldSource(source_path)
|
|
model = K1LocalSurfaceV1(output)
|
|
runtime = K1LocalSurfaceShadowRuntime(
|
|
"synthetic-shadow",
|
|
profile=profile,
|
|
queue_capacity=2,
|
|
result_capacity=16,
|
|
)
|
|
try:
|
|
published = []
|
|
for frame_index in range(source.frame_count):
|
|
if not bool(source.arrays["sample_available"][frame_index]):
|
|
continue
|
|
runtime.publish(_shadow_input(source, frame_index))
|
|
assert runtime.wait_until_idle(2.0)
|
|
published.append(frame_index)
|
|
runtime.close()
|
|
results = runtime.results()
|
|
assert [item.frame_index for item in results] == published
|
|
offsets = source.arrays["cloud_offsets"]
|
|
for result in results:
|
|
frame_index = result.frame_index
|
|
start = int(offsets[frame_index])
|
|
end = int(offsets[frame_index + 1])
|
|
if bool(model.arrays["frame_valid"][frame_index]):
|
|
assert result.state == "valid"
|
|
assert result.sensor_height_m == pytest.approx(
|
|
float(model.arrays["sensor_height_m"][frame_index]),
|
|
abs=1e-12,
|
|
)
|
|
assert result.slope_deg == pytest.approx(
|
|
float(model.arrays["slope_deg"][frame_index]),
|
|
abs=1e-12,
|
|
)
|
|
assert result.roughness_m == pytest.approx(
|
|
float(model.arrays["roughness_m"][frame_index]),
|
|
abs=1e-12,
|
|
)
|
|
assert result.confidence == pytest.approx(
|
|
float(model.arrays["confidence"][frame_index]),
|
|
abs=1e-12,
|
|
)
|
|
assert np.array_equal(
|
|
result.point_class,
|
|
model.arrays["point_class"][start:end],
|
|
)
|
|
assert np.array_equal(
|
|
result.point_step_candidate,
|
|
model.arrays["point_step_candidate"][start:end],
|
|
)
|
|
else:
|
|
assert result.state == "pose-stale"
|
|
snapshot = runtime.snapshot()
|
|
assert snapshot["queue"]["capacity"] == 2
|
|
assert snapshot["queue"]["published"] == len(published)
|
|
assert snapshot["queue"]["consumed"] == len(published)
|
|
assert snapshot["queue"]["dropped_overflow"] == 0
|
|
assert snapshot["delivery"]["source_span_seconds"] > 0
|
|
assert snapshot["delivery"]["effective_fps"] > 0
|
|
assert snapshot["results"]["failed"] == 0
|
|
assert snapshot["occupancy_policy"]["absence_of_points_means_free"] is False
|
|
assert snapshot["authority"]["commands_enabled"] is False
|
|
assert snapshot["authority"]["navigation_or_safety_accepted"] is False
|
|
assert snapshot["closed"] is True
|
|
finally:
|
|
runtime.close()
|
|
source.close()
|
|
model.close()
|
|
|
|
|
|
def test_k1_local_surface_shadow_overload_is_bounded_and_latest_wins(
|
|
tmp_path: Path,
|
|
) -> None:
|
|
source_path = _source_pack(tmp_path / "source")
|
|
source = E10LidarFieldSource(source_path)
|
|
runtime = K1LocalSurfaceShadowRuntime(
|
|
"synthetic-overload",
|
|
profile=K1LocalSurfaceProfile(
|
|
profile_id="synthetic-shadow-overload/v1",
|
|
local_radius_m=5.0,
|
|
cell_size_m=0.5,
|
|
minimum_surface_cells=12,
|
|
),
|
|
queue_capacity=1,
|
|
result_capacity=2,
|
|
)
|
|
try:
|
|
template = _shadow_input(source, 0)
|
|
for frame_index in range(200):
|
|
runtime.publish(
|
|
K1LocalSurfaceShadowInput(
|
|
frame_index=frame_index,
|
|
source_frame_index=10_000 + frame_index,
|
|
session_seconds=10.0 + frame_index * 0.1,
|
|
pose_binding_age_ms=4.0,
|
|
points_map=template.points_map,
|
|
position_map=template.position_map,
|
|
published_monotonic_ns=time.monotonic_ns(),
|
|
)
|
|
)
|
|
runtime.close()
|
|
snapshot = runtime.snapshot()
|
|
queue = snapshot["queue"]
|
|
assert queue["maximum_depth"] <= queue["capacity"] == 1
|
|
assert queue["dropped_overflow"] > 0
|
|
assert queue["consumed"] + queue["dropped_overflow"] == queue["published"]
|
|
assert snapshot["results"]["depth"] <= 2
|
|
assert runtime.results()[-1].frame_index == 199
|
|
assert snapshot["results"]["latest"]["frame_index"] == 199
|
|
assert snapshot["authority"]["commands_enabled"] is False
|
|
finally:
|
|
runtime.close()
|
|
source.close()
|