From a1e2cb523f8b8ab29be4f78deebf5380fc0f8a26 Mon Sep 17 00:00:00 2001 From: DCCONSTRUCTIONS Date: Sun, 26 Jul 2026 01:30:53 +0300 Subject: [PATCH] feat: wire physical K1 surface shadow --- README.md | 12 + docs/13_LIDAR_WORKER_PRODUCT_AND_ROADMAP.md | 33 +- docs/14_LIDAR_DATASET_GATEWAY.md | 8 +- .../perception/prepare_e15_worker_package.py | 4 + .../worker/Invoke-E15ShadowInference.ps1 | 19 +- ...e28_physical_k1_local_surface_profile.json | 71 +++ .../worker/run_e15_shadow_inference.py | 300 ++++++++++++- src/k1link/compute/__init__.py | 8 + .../compute/lidar_local_surface_geometry.py | 403 ++++++++++++++++++ .../compute/lidar_local_surface_shadow.py | 370 +++++++++++++++- tests/test_e15_shadow_inference.py | 132 ++++++ tests/test_e15_worker_package.py | 27 +- tests/test_lidar_local_surface.py | 10 + tests/test_live_perception_synchronizer.py | 88 +++- 14 files changed, 1444 insertions(+), 41 deletions(-) create mode 100644 experiments/perception/worker/e28_physical_k1_local_surface_profile.json create mode 100644 src/k1link/compute/lidar_local_surface_geometry.py diff --git a/README.md b/README.md index 7ea2667..4478685 100644 --- a/README.md +++ b/README.md @@ -168,6 +168,18 @@ and scalar results matched the immutable replay derivative exactly. This is a recorded-source-paced execution gate only; physical K1 worker binding, free-space, commands, navigation and safety authority remain unavailable. +The physical shadow seam is now implemented but not yet physically qualified. +The authenticated external worker can bind decoded map-frame `lio_pcl` and +`lio_pose` in either arrival order through +`missioncore.k1-local-surface-pose-binder/v1`, then feed the accepted estimator +without blocking camera cadence. Point input and estimator work are +capacity-two latest-wins queues; pose history and result history are bounded. +The pinned `e28_physical_k1_local_surface_profile.json` rejects the run unless +at least 100 point frames bind and exact accounting, pose misses, queue drops, +p95 result age, profile identity and shadow-only authority all pass. No +physical acceptance result is claimed until that profile is executed against +an attached K1 on the NVIDIA worker. + The complete RELLIS-3D v1.1 release is now admitted there and its full `2,413`-frame validation split is available in **Полигон → Датасеты**. The sealed Current/Patchwork++ comparison rejected Patchwork++ for navigation: diff --git a/docs/13_LIDAR_WORKER_PRODUCT_AND_ROADMAP.md b/docs/13_LIDAR_WORKER_PRODUCT_AND_ROADMAP.md index 68c9262..001d74e 100644 --- a/docs/13_LIDAR_WORKER_PRODUCT_AND_ROADMAP.md +++ b/docs/13_LIDAR_WORKER_PRODUCT_AND_ROADMAP.md @@ -517,10 +517,28 @@ immutable replay derivative: zero state, point-class or step-candidate mismatches and zero scalar delta. The accepted result is `k1-local-surface-shadow-04f14d8c580f74cbd5b0a452867563ebc6b3ef93d872129e4918680932253ab7`. -The next gate is not another replay tuning pass. It is an explicit bounded -LiDAR↔pose binder on the authenticated external worker stream followed by a -physical K1 shadow run, still without commands, free-space, navigation or -safety authority. +The authenticated external worker now contains the next bounded seam. +`missioncore.k1-local-surface-pose-binder/v1` accepts decoded map-frame +`lio_pcl` and `lio_pose` in either arrival order, binds by bounded worker +host-arrival time, and publishes only matched pairs to the capacity-two +latest-wins estimator queue. Point backlog is bounded to two, pose history to +16 and the diagnostic result ring to eight. Shutdown flushes or explicitly +counts every point frame; the final report records pose-binding misses, +binder/runtime overflow, exact queue accounting, processing failures and p95 +result age. + +The pinned physical profile is +`experiments/perception/worker/e28_physical_k1_local_surface_profile.json`. +It requires at least `100` bound frames, at most `5%` pose misses, `1%` binder +point replacement, `1%` runtime replacement and `80 ms` p95 worker-local +result age. The existing D-only PowerShell worker launcher accepts this +physical profile and verifies that the minimal hash-addressed package contains +the local-surface geometry, binder/runtime and ground primitives. + +This is implementation readiness, not a physical result. The next gate is to +build and stage the new content-addressed worker package on D, execute the +profile against an attached K1 and retain the generated report. Commands, +free-space, navigation and safety authority remain disabled. Exit: one immutable K1 session yields both a persistent reconstruction and a bounded local world state without hard-coded terrain height or scanner-side @@ -545,6 +563,9 @@ independent gate without increasing unsafe false-free or false-dynamic output. - [x] Add a provider-neutral bounded LiDAR local-surface queue independent of camera cadence and qualify it at recorded 1× source pace. +- [x] Bind authenticated decoded physical `lio_pcl + lio_pose` to that queue + with bounded buffers, exact accounting and a pinned physical acceptance + profile. - [ ] Run the accepted K1 local-surface/local-map profile on the NVIDIA worker. - [ ] Fuse K1 geometric evidence with E26 camera evidence as independent sources; a LiDAR-native detector remains optional. @@ -612,7 +633,7 @@ large for a free-space claim. Deterministic triage has reduced the first manual inspection set to four high-priority frames in two episodes. Prior-plane residual explainability now shows that episode `09` is a localized positive structure rather than symmetric plane drift. The highest-value immediate work -is therefore a bounded live-shadow queue, followed by a separate -dynamic-observation layer. Nvblox, +is therefore the physical K1 run through the now-implemented bounded +live-shadow seam, followed by a separate dynamic-observation layer. Nvblox, raw-scan detectors and alternative SLAM remain optional later gates because the current report contract does not carry their required ray/timing semantics. diff --git a/docs/14_LIDAR_DATASET_GATEWAY.md b/docs/14_LIDAR_DATASET_GATEWAY.md index b59e0cf..8d41f2f 100644 --- a/docs/14_LIDAR_DATASET_GATEWAY.md +++ b/docs/14_LIDAR_DATASET_GATEWAY.md @@ -92,10 +92,12 @@ Not implemented: - no model training or production promotion; - no RELLIS ROS bag admission, continuous synchronized playback or production promotion; -- no ray-cleared free-space or planner-authoritative rolling occupancy map. +- no ray-cleared free-space or planner-authoritative rolling occupancy map; - the accepted K1 local-surface profile now passes a 15-second - recorded-source-paced bounded shadow gate; physical K1 worker binding is not - implemented yet, and the residual overlay remains non-authoritative. + recorded-source-paced bounded shadow gate; authenticated physical + `lio_pcl + lio_pose` binding and its pinned acceptance profile are + implemented, but no attached-K1 run has qualified them yet. The residual + overlay remains non-authoritative. ## Product surface boundary diff --git a/experiments/perception/prepare_e15_worker_package.py b/experiments/perception/prepare_e15_worker_package.py index cf44696..ddb848f 100644 --- a/experiments/perception/prepare_e15_worker_package.py +++ b/experiments/perception/prepare_e15_worker_package.py @@ -17,9 +17,12 @@ SCHEMA = "missioncore.e15-worker-package/v1" COPIED_FILES = ( "k1link/__init__.py", "k1link/compute/inline_temporal.py", + "k1link/compute/lidar_local_surface_geometry.py", + "k1link/compute/lidar_local_surface_shadow.py", "k1link/compute/live_perception.py", "k1link/data_plane/__init__.py", "k1link/data_plane/views.py", + "k1link/ground_segmentation.py", "k1link/device_plugins/__init__.py", "k1link/device_plugins/xgrids_k1/protocol/__init__.py", "k1link/device_plugins/xgrids_k1/protocol/normalizer.py", @@ -71,6 +74,7 @@ def _identity(source_root: Path) -> dict[str, Any]: "imports": [ "k1link.compute.inline_temporal.TemporalStabilizer", "k1link.compute.inline_temporal.StreamingSemanticStabilizer", + "k1link.compute.lidar_local_surface_shadow.K1LocalSurfaceShadowCoordinator", "k1link.compute.live_perception.LiveSensorSynchronizer", "k1link.data_plane.DecodedPointCloudView", "k1link.data_plane.DecodedPoseView", diff --git a/experiments/perception/worker/Invoke-E15ShadowInference.ps1 b/experiments/perception/worker/Invoke-E15ShadowInference.ps1 index 722dd6b..fedd94f 100644 --- a/experiments/perception/worker/Invoke-E15ShadowInference.ps1 +++ b/experiments/perception/worker/Invoke-E15ShadowInference.ps1 @@ -136,11 +136,26 @@ if ( $live = Get-Content -LiteralPath $liveProfile -Raw | ConvertFrom-Json if ( $live.schema_version -ne "missioncore.e15-shadow-inference-profile/v1" -or - $live.mode -ne "replay-shadow-gate" -or + $live.mode -notin @("replay-shadow-gate", "physical-shadow-gate") -or [bool]$live.authority.commands_enabled -or [bool]$live.authority.navigation_or_safety_accepted -or $live.transport.pyav_version -ne "18.0.0" -) { throw "LAB E15 replay-shadow authority contract changed" } +) { throw "LAB E15/E28 shadow authority contract changed" } +if ($live.mode -eq "physical-shadow-gate") { + foreach ($relative in @( + "k1link\compute\lidar_local_surface_geometry.py", + "k1link\compute\lidar_local_surface_shadow.py", + "k1link\ground_segmentation.py" + )) { + if (-not (Test-Path -LiteralPath (Join-Path $package $relative) -PathType Leaf)) { + throw "LAB E28 worker package lacks local-surface runtime" + } + } + if ( + -not [bool]$live.local_surface.enabled -or + $live.local_surface.profile_id -ne "k1-vendor-map-dynamic-local-surface/v1" + ) { throw "LAB E28 local-surface profile contract changed" } +} if ($stabilityProfile) { $stability = Get-Content -LiteralPath $stabilityProfile -Raw | ConvertFrom-Json if ( diff --git a/experiments/perception/worker/e28_physical_k1_local_surface_profile.json b/experiments/perception/worker/e28_physical_k1_local_surface_profile.json new file mode 100644 index 0000000..3d3b5c4 --- /dev/null +++ b/experiments/perception/worker/e28_physical_k1_local_surface_profile.json @@ -0,0 +1,71 @@ +{ + "schema_version": "missioncore.e15-shadow-inference-profile/v1", + "mode": "physical-shadow-gate", + "authority": { + "commands_enabled": false, + "navigation_or_safety_accepted": false + }, + "source": { + "source_id": "sensor.camera.right", + "resolution": [ + 800, + 600 + ], + "calibration_slot": "camera_1", + "calibration_sha256": "05f3ad9b38b3a4fc95388a8ec83da83c745e217709e51787b3d5aad0969f6fa9" + }, + "transport": { + "wire_schema": "missioncore.live-perception-wire/v1", + "camera_media": "persistent-fmp4-pyav", + "pyav_version": "18.0.0", + "maximum_media_buffer_bytes": 8388608, + "camera_metadata_capacity": 16 + }, + "scheduling": { + "detector_queue_capacity": 2, + "semantic_queue_capacity": 1, + "semantic_sample_every_frames": 5, + "semantic_ttl_ms": 750.0, + "sensor_wait_ms": 90.0 + }, + "temporal": { + "binding": "nearest-recorded-host-arrival-best-effort", + "maximum_lidar_camera_delta_ms": 100.0, + "maximum_pose_point_delta_ms": 100.0, + "buffer_capacity_per_modality": 32, + "retention_seconds": 3.0, + "clock_qualification": "not-hardware-synchronized" + }, + "local_surface": { + "enabled": true, + "profile_id": "k1-vendor-map-dynamic-local-surface/v1", + "profile_sha256": "7a59edc8404d0177a39175578743589bfb6b7822837170cded38ca2b6698cc26", + "point_queue_capacity": 2, + "pose_buffer_capacity": 16, + "future_pose_wait_ms": 25.0, + "retention_seconds": 3.0, + "result_capacity": 8, + "acceptance": { + "minimum_bound_frames": 100, + "maximum_pose_miss_fraction": 0.05, + "maximum_point_drop_fraction": 0.01, + "maximum_runtime_drop_fraction": 0.01, + "maximum_p95_result_age_ms": 80.0 + } + }, + "acceptance": { + "minimum_camera_frames": 140, + "detector_minimum_effective_fps": 9.5, + "detector_maximum_drop_fraction": 0.01, + "semantic_minimum_effective_fps": 1.8, + "semantic_maximum_drop_fraction": 0.05, + "semantic_maximum_p95_completion_age_ms": 400.0, + "minimum_fresh_semantic_coverage": 0.9, + "minimum_fused_fraction": 0.85, + "maximum_p95_decode_age_ms": 80.0, + "maximum_p95_world_state_age_ms": 200.0, + "require_zero_transport_gaps": true, + "require_zero_camera_sequence_gaps": true, + "require_zero_failures": true + } +} diff --git a/experiments/perception/worker/run_e15_shadow_inference.py b/experiments/perception/worker/run_e15_shadow_inference.py index f47703e..cca5574 100644 --- a/experiments/perception/worker/run_e15_shadow_inference.py +++ b/experiments/perception/worker/run_e15_shadow_inference.py @@ -176,6 +176,7 @@ def read_live_profile(path: Path) -> tuple[dict[str, Any], str]: transport = profile.get("transport") scheduling = profile.get("scheduling") temporal = profile.get("temporal") + local_surface = profile.get("local_surface") acceptance = profile.get("acceptance") if ( profile.get("schema_version") != PROFILE_SCHEMA @@ -212,6 +213,78 @@ def read_live_profile(path: Path) -> tuple[dict[str, Any], str]: or not 1 <= float(temporal.get("maximum_pose_point_delta_ms", 0)) <= 1000 ): raise RuntimeError("LAB E15 bounded runtime contract is invalid") + if local_surface is not None: + from k1link.compute.lidar_local_surface_geometry import ( + DEFAULT_K1_LOCAL_SURFACE_PROFILE, + ) + + local_acceptance = ( + local_surface.get("acceptance") + if isinstance(local_surface, dict) + else None + ) + expected_profile_sha256 = hashlib.sha256( + canonical_json(DEFAULT_K1_LOCAL_SURFACE_PROFILE.to_dict()) + ).hexdigest() + local_fractions = ( + "maximum_pose_miss_fraction", + "maximum_point_drop_fraction", + "maximum_runtime_drop_fraction", + ) + point_capacity = ( + local_surface.get("point_queue_capacity") + if isinstance(local_surface, dict) + else None + ) + pose_capacity = ( + local_surface.get("pose_buffer_capacity") + if isinstance(local_surface, dict) + else None + ) + result_capacity = ( + local_surface.get("result_capacity") + if isinstance(local_surface, dict) + else None + ) + if ( + profile.get("mode") != "physical-shadow-gate" + or not isinstance(local_surface, dict) + or local_surface.get("enabled") is not True + or local_surface.get("profile_id") + != DEFAULT_K1_LOCAL_SURFACE_PROFILE.profile_id + or local_surface.get("profile_sha256") + != expected_profile_sha256 + or not isinstance(point_capacity, int) + or isinstance(point_capacity, bool) + or point_capacity not in range(1, 9) + or not isinstance(pose_capacity, int) + or isinstance(pose_capacity, bool) + or pose_capacity not in range(2, 257) + or not isinstance(result_capacity, int) + or isinstance(result_capacity, bool) + or result_capacity not in range(1, 257) + or not 0 + <= float(local_surface.get("future_pose_wait_ms", -1)) + <= DEFAULT_K1_LOCAL_SURFACE_PROFILE.maximum_pose_binding_ms + or not 0.1 + <= float(local_surface.get("retention_seconds", 0)) + <= 30 + or float(temporal.get("maximum_pose_point_delta_ms", 0)) + != DEFAULT_K1_LOCAL_SURFACE_PROFILE.maximum_pose_binding_ms + or not isinstance(local_acceptance, dict) + or int(local_acceptance.get("minimum_bound_frames", 0)) < 2 + or any( + not 0 <= float(local_acceptance.get(key, -1)) <= 1 + for key in local_fractions + ) + or float( + local_acceptance.get("maximum_p95_result_age_ms", 0) + ) + <= 0 + ): + raise RuntimeError( + "LAB E28 physical local-surface profile contract is invalid" + ) fractions = ( "detector_maximum_drop_fraction", "semantic_maximum_drop_fraction", @@ -244,6 +317,125 @@ def read_live_profile(path: Path) -> tuple[dict[str, Any], str]: return profile, sha256(resolved) +def _local_surface_acceptance_checks( + snapshot: dict[str, object], + config: dict[str, Any], +) -> dict[str, bool]: + binder = snapshot.get("binder") + runtime = snapshot.get("runtime") + acceptance = config["acceptance"] + if not isinstance(binder, dict): + binder = {} + if not isinstance(runtime, dict): + runtime = {} + points = binder.get("points") + poses = binder.get("poses") + queue_state = runtime.get("queue") + results = runtime.get("results") + runtime_profile = runtime.get("profile") + result_age = results.get("result_age_ms") if isinstance(results, dict) else None + if not isinstance(points, dict): + points = {} + if not isinstance(poses, dict): + poses = {} + if not isinstance(queue_state, dict): + queue_state = {} + if not isinstance(results, dict): + results = {} + if not isinstance(runtime_profile, dict): + runtime_profile = {} + if not isinstance(result_age, dict): + result_age = {} + + point_published = int(points.get("published", 0)) + point_bound = int(points.get("bound", 0)) + point_missed = int(points.get("missed", 0)) + point_dropped = int(points.get("dropped_overflow", 0)) + point_depth = int(points.get("depth", 0)) + runtime_published = int(queue_state.get("published", 0)) + runtime_consumed = int(queue_state.get("consumed", 0)) + runtime_dropped = int(queue_state.get("dropped_overflow", 0)) + runtime_depth = int(queue_state.get("depth", 0)) + result_published = int(results.get("published", 0)) + result_failed = int(results.get("failed", 0)) + p95_result_age = result_age.get("p95") + + return { + "local_surface_session_initialized": bool(runtime), + "local_surface_closed": snapshot.get("closed") is True + and runtime.get("closed") is True, + "local_surface_minimum_bound_frames": point_bound + >= int(acceptance["minimum_bound_frames"]), + "local_surface_binder_accounting": point_bound + + point_missed + + point_dropped + + point_depth + == point_published, + "local_surface_binder_to_runtime_accounting": point_bound + == runtime_published, + "local_surface_point_buffer_bound": ( + int(points.get("capacity", 0)) == int(config["point_queue_capacity"]) + and int(points.get("maximum_depth", 0)) + <= int(points.get("capacity", 0)) + and point_depth == 0 + ), + "local_surface_pose_buffer_bound": ( + int(poses.get("capacity", 0)) == int(config["pose_buffer_capacity"]) + and int(poses.get("maximum_depth", 0)) + <= int(poses.get("capacity", 0)) + ), + "local_surface_maximum_pose_miss_fraction": point_missed + / max(1, point_published) + <= float(acceptance["maximum_pose_miss_fraction"]), + "local_surface_maximum_point_drop_fraction": point_dropped + / max(1, point_published) + <= float(acceptance["maximum_point_drop_fraction"]), + "local_surface_runtime_accounting": runtime_consumed + + runtime_dropped + + runtime_depth + == runtime_published, + "local_surface_runtime_result_accounting": result_published + + result_failed + == runtime_consumed, + "local_surface_runtime_queue_bound": ( + int(queue_state.get("capacity", 0)) == int(config["point_queue_capacity"]) + and int(queue_state.get("maximum_depth", 0)) + <= int(queue_state.get("capacity", 0)) + and runtime_depth == 0 + ), + "local_surface_maximum_runtime_drop_fraction": runtime_dropped + / max(1, runtime_published) + <= float(acceptance["maximum_runtime_drop_fraction"]), + "local_surface_zero_runtime_failures": result_failed == 0, + "local_surface_maximum_p95_result_age_ms": ( + isinstance(p95_result_age, (int, float)) + and not isinstance(p95_result_age, bool) + and float(p95_result_age) + <= float(acceptance["maximum_p95_result_age_ms"]) + ), + "local_surface_profile_pinned": ( + runtime_profile.get("profile_id") == config["profile_id"] + ), + "local_surface_shadow_authority_only": ( + snapshot.get("authority") + == { + "commands_enabled": False, + "navigation_or_safety_accepted": False, + } + and runtime.get("authority") + == { + "commands_enabled": False, + "navigation_or_safety_accepted": False, + } + ), + "local_surface_unknown_never_free": runtime.get("occupancy_policy") + == { + "absence_of_points_means_free": False, + "unknown_is_traversable": False, + }, + } + + def read_projection_pack( root: Path, expected_calibration_sha256: str ) -> tuple[ProjectionProfile, dict[str, Any]]: @@ -312,7 +504,7 @@ def read_worker_package(root: Path) -> dict[str, Any]: or manifest.get("package_id") != f"e15-worker-package-{identity_sha256}" or resolved.name != manifest.get("package_id") or not isinstance(artifacts, list) - or len(artifacts) != 12 + or len(artifacts) not in {12, 15} ): raise RuntimeError("LAB E15 worker package identity is invalid") expected = set() @@ -568,6 +760,7 @@ def _receiver( max_duration_seconds: float, decoder: PersistentFmp4Decoder, synchronizer: LiveSensorSynchronizer, + local_surface: Any | None, lidar_quality: LidarQualityMonitor, state: _TransportState, sensor_decode_ms: dict[str, list[float]], @@ -646,6 +839,8 @@ def _receiver( session_id = str(header["session_id"]) if state.session_id is None: state.session_id = session_id + if local_surface is not None: + local_surface.begin_session(session_id) elif state.session_id != session_id: raise ShadowRuntimeError("shadow session identity changed") modality = str(header["modality"]) @@ -698,8 +893,12 @@ def _receiver( if modality == "lidar" and isinstance(normalized, DecodedPointCloudView): lidar_quality.observe(normalized) synchronizer.publish_point_cloud(normalized) + if local_surface is not None: + local_surface.publish_point_cloud(normalized) elif modality == "pose" and isinstance(normalized, DecodedPoseView): synchronizer.publish_pose(normalized) + if local_surface is not None: + local_surface.publish_pose(normalized) else: raise ShadowRuntimeError("known sensor modality did not normalize") else: @@ -709,6 +908,12 @@ def _receiver( state.failures.append(exc) finally: decoder.finish_input() + if local_surface is not None: + try: + local_surface.close(timeout_seconds=30) + except BaseException as exc: + assert state.failures is not None + state.failures.append(exc) sender_stop.set() if sender_thread is not None: sender_thread.join(timeout=5) @@ -765,6 +970,21 @@ def _common(args: argparse.Namespace) -> dict[str, Any]: if dependency["identity"]["profile_sha256"] != semantic_sha256: raise RuntimeError("LAB E15 semantic dependency identity changed") worker_package = read_worker_package(args.worker_package) + if live.get("local_surface") is not None: + worker_sources = { + str(item.get("path")) + for item in worker_package["identity"]["source_files"] + if isinstance(item, dict) + } + required_surface_sources = { + "k1link/compute/lidar_local_surface_geometry.py", + "k1link/compute/lidar_local_surface_shadow.py", + "k1link/ground_segmentation.py", + } + if not required_surface_sources <= worker_sources: + raise RuntimeError( + "LAB E28 worker package lacks local-surface runtime" + ) stability = None stability_sha256 = None if args.stability_profile is not None: @@ -926,6 +1146,22 @@ def run(args: argparse.Namespace, loaded: _LoadedModels | None = None) -> int: capacity_per_modality=int(temporal["buffer_capacity_per_modality"]), retention_seconds=float(temporal["retention_seconds"]), ) + local_surface_config = live.get("local_surface") + local_surface = None + if isinstance(local_surface_config, dict): + from k1link.compute.lidar_local_surface_shadow import ( + K1LocalSurfaceShadowCoordinator, + ) + + local_surface = K1LocalSurfaceShadowCoordinator( + point_capacity=int(local_surface_config["point_queue_capacity"]), + pose_capacity=int(local_surface_config["pose_buffer_capacity"]), + future_pose_wait_ms=float( + local_surface_config["future_pose_wait_ms"] + ), + retention_seconds=float(local_surface_config["retention_seconds"]), + result_capacity=int(local_surface_config["result_capacity"]), + ) lidar_quality = LidarQualityMonitor(K1_LIVE_LIDAR_PROFILE) first_camera_epoch_ns: list[int] = [] last_camera_epoch_ns: list[int] = [] @@ -1025,6 +1261,21 @@ def run(args: argparse.Namespace, loaded: _LoadedModels | None = None) -> int: runtime_path = output / "runtime-telemetry.jsonl" run_started = time.perf_counter() process_cpu_started = time.process_time() + runtime_snapshotters = { + "detector": detector_queue.snapshot, + "semantic": semantic_queue.snapshot, + "decoder": decoder.snapshot, + "synchronizer": synchronizer.snapshot, + "lidar_quality": lidar_quality.snapshot, + "result": lambda: { + "capacity": result_queue.maxsize, + "depth": result_queue.qsize(), + "dropped_overflow": transport.results_dropped, + "published": transport.results_published, + }, + } + if local_surface is not None: + runtime_snapshotters["local_surface"] = local_surface.snapshot with ( semantic_path.open("x", encoding="utf-8", newline="\n") as semantic_stream, @@ -1037,19 +1288,7 @@ def run(args: argparse.Namespace, loaded: _LoadedModels | None = None) -> int: _RuntimeTelemetry( runtime_stream, interval_seconds=1.0, - snapshotters={ - "detector": detector_queue.snapshot, - "semantic": semantic_queue.snapshot, - "decoder": decoder.snapshot, - "synchronizer": synchronizer.snapshot, - "lidar_quality": lidar_quality.snapshot, - "result": lambda: { - "capacity": result_queue.maxsize, - "depth": result_queue.qsize(), - "dropped_overflow": transport.results_dropped, - "published": transport.results_published, - }, - }, + snapshotters=runtime_snapshotters, ) as runtime_telemetry, ): @@ -1111,6 +1350,7 @@ def run(args: argparse.Namespace, loaded: _LoadedModels | None = None) -> int: "max_duration_seconds": args.max_duration_seconds, "decoder": decoder, "synchronizer": synchronizer, + "local_surface": local_surface, "lidar_quality": lidar_quality, "state": transport, "sensor_decode_ms": sensor_decode_ms, @@ -1400,6 +1640,9 @@ def run(args: argparse.Namespace, loaded: _LoadedModels | None = None) -> int: temporal_semantic_summary = ( None if semantic_stabilizer is None else semantic_stabilizer.snapshot() ) + local_surface_snapshot = ( + None if local_surface is None else local_surface.snapshot() + ) acceptance = live["acceptance"] checks = { "minimum_camera_frames": decoded_frame_count >= int(acceptance["minimum_camera_frames"]), @@ -1454,6 +1697,14 @@ def run(args: argparse.Namespace, loaded: _LoadedModels | None = None) -> int: "authority_remains_shadow_only": live["authority"] == {"commands_enabled": False, "navigation_or_safety_accepted": False}, } + if isinstance(local_surface_config, dict): + assert local_surface_snapshot is not None + checks.update( + _local_surface_acceptance_checks( + local_surface_snapshot, + local_surface_config, + ) + ) if stability is not None: temporal_acceptance = stability["acceptance"] assert temporal_track_summary is not None @@ -1502,6 +1753,16 @@ def run(args: argparse.Namespace, loaded: _LoadedModels | None = None) -> int: "identity_sha256": common["projection_manifest"]["identity_sha256"], }, "lidar_evidence": lidar_readiness_document(K1_LIVE_LIDAR_PROFILE), + "local_surface": ( + None + if local_surface_config is None + else { + "profile_id": local_surface_config["profile_id"], + "profile_sha256": local_surface_config["profile_sha256"], + "binder_schema": "missioncore.k1-local-surface-pose-binder/v1", + "runtime_schema": "missioncore.k1-local-surface-shadow-runtime/v1", + } + ), "worker_package": { "id": common["worker_package"]["package_id"], "identity_sha256": common["worker_package"]["identity_sha256"], @@ -1566,6 +1827,7 @@ def run(args: argparse.Namespace, loaded: _LoadedModels | None = None) -> int: "sensor_decode_ms": sensor_decode_summary, }, "lidar_quality": lidar_quality.snapshot(), + "local_surface": local_surface_snapshot, "latency_ms": latency_summary, "temporal_stability": { "enabled": stability is not None, @@ -1616,6 +1878,16 @@ def run(args: argparse.Namespace, loaded: _LoadedModels | None = None) -> int: "Camera/LiDAR matching uses recorded host arrival time, not a hardware clock.", "K1 LiDAR is a vendor map increment, not an admitted raw sensor sweep.", "K1 LiDAR has no admitted per-point time, ring, scan geometry or IMU stream.", + *( + [ + "K1 local-surface pose binding uses bounded worker host-arrival " + "time, not hardware synchronization.", + "K1 local-surface outputs are diagnostic observed-surface evidence; " + "they do not infer free or traversable unknown space.", + ] + if local_surface_snapshot is not None + else [] + ), "Cross-host source epoch age is diagnostic and excluded from acceptance.", "COCO and Cityscapes models are not forest-domain or safety validated.", "Amodal cuboids infer unobserved volume from class priors.", diff --git a/src/k1link/compute/__init__.py b/src/k1link/compute/__init__.py index 01cf183..7f5ad50 100644 --- a/src/k1link/compute/__init__.py +++ b/src/k1link/compute/__init__.py @@ -119,8 +119,12 @@ from .lidar_local_surface import ( k1_local_surface_catalog_item, ) from .lidar_local_surface_shadow import ( + K1_LOCAL_SURFACE_BINDER_SCHEMA, K1_LOCAL_SURFACE_SHADOW_FRAME_SCHEMA, K1_LOCAL_SURFACE_SHADOW_SCHEMA, + K1LocalSurfaceBoundViews, + K1LocalSurfacePoseBinder, + K1LocalSurfaceShadowCoordinator, K1LocalSurfaceShadowEstimator, K1LocalSurfaceShadowInput, K1LocalSurfaceShadowResult, @@ -217,6 +221,7 @@ __all__ = [ "LIDAR_GROUND_BENCHMARK_SCHEMA", "LIDAR_GROUND_FRAME_SCHEMA", "K1_LOCAL_SURFACE_FRAME_SCHEMA", + "K1_LOCAL_SURFACE_BINDER_SCHEMA", "K1_LOCAL_SURFACE_REPORT_SCHEMA", "K1_LOCAL_SURFACE_REVIEW_SCHEMA", "K1_LOCAL_SURFACE_SCHEMA", @@ -246,6 +251,9 @@ __all__ = [ "LidarGroundBenchmarkV1", "LidarGroundError", "K1LocalSurfaceProfile", + "K1LocalSurfaceBoundViews", + "K1LocalSurfacePoseBinder", + "K1LocalSurfaceShadowCoordinator", "K1LocalSurfaceShadowEstimator", "K1LocalSurfaceShadowInput", "K1LocalSurfaceShadowResult", diff --git a/src/k1link/compute/lidar_local_surface_geometry.py b/src/k1link/compute/lidar_local_surface_geometry.py new file mode 100644 index 0000000..1ce166b --- /dev/null +++ b/src/k1link/compute/lidar_local_surface_geometry.py @@ -0,0 +1,403 @@ +from __future__ import annotations + +from collections.abc import Mapping +from dataclasses import dataclass +from typing import Final + +import numpy as np +import numpy.typing as npt + +from k1link.ground_segmentation import GroundSegmentationError as LidarGroundError + +POINT_UNCLASSIFIED: Final = 0 +POINT_SURFACE: Final = 1 +POINT_OCCUPIED: Final = 2 +POINT_BELOW_SURFACE: Final = 3 + + +@dataclass(frozen=True, slots=True) +class K1LocalSurfaceProfile: + """Dependency-light parameters shared by replay and live shadow.""" + + profile_id: str = "k1-vendor-map-dynamic-local-surface/v1" + local_radius_m: float = 10.0 + cell_size_m: float = 0.45 + surface_ttl_s: float = 1.25 + cell_lower_percentile: float = 20.0 + initial_lower_fraction: float = 0.55 + minimum_surface_cells: int = 18 + robust_iterations: int = 5 + robust_mad_scale: float = 2.8 + minimum_inlier_band_m: float = 0.10 + surface_band_m: float = 0.16 + obstacle_min_height_m: float = 0.20 + obstacle_max_height_m: float = 3.5 + maximum_pose_binding_ms: float = 100.0 + maximum_slope_deg: float = 40.0 + step_min_height_m: float = 0.07 + step_max_height_m: float = 0.32 + step_max_plane_residual_m: float = 0.45 + temporal_height_jump_m: float = 0.03 + temporal_slope_jump_deg: float = 0.5 + temporal_roughness_jump_m: float = 0.015 + + def __post_init__(self) -> None: + numeric = ( + self.local_radius_m, + self.cell_size_m, + self.surface_ttl_s, + self.cell_lower_percentile, + self.initial_lower_fraction, + self.robust_mad_scale, + self.minimum_inlier_band_m, + self.surface_band_m, + self.obstacle_min_height_m, + self.obstacle_max_height_m, + self.maximum_pose_binding_ms, + self.maximum_slope_deg, + self.step_min_height_m, + self.step_max_height_m, + self.step_max_plane_residual_m, + self.temporal_height_jump_m, + self.temporal_slope_jump_deg, + self.temporal_roughness_jump_m, + ) + if ( + not self.profile_id.strip() + or len(self.profile_id) > 160 + or not np.isfinite(numeric).all() + or not 1.0 <= self.local_radius_m <= 100.0 + or not 0.05 <= self.cell_size_m <= 5.0 + or not 0.05 <= self.surface_ttl_s <= 30.0 + or not 0.0 <= self.cell_lower_percentile <= 50.0 + or not 0.05 <= self.initial_lower_fraction <= 0.95 + or not 3 <= self.minimum_surface_cells <= 100_000 + or not 1 <= self.robust_iterations <= 20 + or not 1.0 <= self.robust_mad_scale <= 10.0 + or not 0.01 <= self.minimum_inlier_band_m <= 1.0 + or not 0.01 <= self.surface_band_m <= 1.0 + or not self.surface_band_m <= self.obstacle_min_height_m + or not self.obstacle_min_height_m < self.obstacle_max_height_m <= 20.0 + or not 1.0 <= self.maximum_pose_binding_ms <= 10_000.0 + or not 1.0 <= self.maximum_slope_deg < 90.0 + or not 0.02 <= self.step_min_height_m < self.step_max_height_m + or not self.step_max_height_m <= self.step_max_plane_residual_m <= 2.0 + or not 0.02 <= self.temporal_height_jump_m <= 2.0 + or not 0.1 <= self.temporal_slope_jump_deg <= 45.0 + or not 0.005 <= self.temporal_roughness_jump_m <= 1.0 + ): + raise LidarGroundError("K1 local-surface profile is invalid") + + def to_dict(self) -> dict[str, object]: + return { + "schema_version": "missioncore.k1-local-surface-profile/v1", + "profile_id": self.profile_id, + "input": { + "representation": "legacy-e10-vendor-map-with-pose", + "gravity_alignment": "vendor-map-z-assumed-diagnostic", + "physical_sensor_height_required": False, + "hardcoded_height_m": None, + }, + "rolling_surface": { + "local_radius_m": self.local_radius_m, + "cell_size_m": self.cell_size_m, + "surface_ttl_s": self.surface_ttl_s, + "cell_lower_percentile": self.cell_lower_percentile, + "initial_lower_fraction": self.initial_lower_fraction, + "minimum_surface_cells": self.minimum_surface_cells, + "robust_iterations": self.robust_iterations, + "robust_mad_scale": self.robust_mad_scale, + "minimum_inlier_band_m": self.minimum_inlier_band_m, + "maximum_slope_deg": self.maximum_slope_deg, + "step_min_height_m": self.step_min_height_m, + "step_max_height_m": self.step_max_height_m, + "step_max_plane_residual_m": self.step_max_plane_residual_m, + }, + "classification": { + "surface_band_m": self.surface_band_m, + "obstacle_min_height_m": self.obstacle_min_height_m, + "obstacle_max_height_m": self.obstacle_max_height_m, + "absence_of_points_means_free": False, + "unknown_is_traversable": False, + }, + "pose_binding": { + "basis": "recorded-nearest-host-monotonic-arrival", + "maximum_age_ms": self.maximum_pose_binding_ms, + }, + "temporal_qualification": { + "prediction_input": "previous-ttl-window-only", + "current_frame_excluded_from_prediction": True, + "height_jump_m": self.temporal_height_jump_m, + "slope_jump_deg": self.temporal_slope_jump_deg, + "roughness_jump_m": self.temporal_roughness_jump_m, + }, + "authority": { + "commands_enabled": False, + "navigation_or_safety_accepted": False, + }, + } + + +DEFAULT_K1_LOCAL_SURFACE_PROFILE: Final = K1LocalSurfaceProfile() + + +@dataclass(frozen=True, slots=True) +class PredictionEvidence: + prior_plane: npt.NDArray[np.float64] + cell_points: npt.NDArray[np.float64] + signed_residuals: npt.NDArray[np.float64] + + @property + def residual_p50_m(self) -> float: + return float(np.percentile(np.abs(self.signed_residuals), 50)) + + @property + def residual_p95_m(self) -> float: + return float(np.percentile(np.abs(self.signed_residuals), 95)) + + def inlier_fraction(self, surface_band_m: float) -> float: + return float(np.mean(np.abs(self.signed_residuals) <= surface_band_m)) + + +def update_cache( + cache: dict[tuple[int, int], tuple[float, float]], + cloud: npt.NDArray[np.float64], + session_seconds: float, + profile: K1LocalSurfaceProfile, +) -> None: + keys, points = cloud_cell_observations(cloud, profile) + for key, point in zip(keys, points, strict=True): + cache[(int(key[0]), int(key[1]))] = (float(point[2]), session_seconds) + + +def cloud_cell_observations( + cloud: npt.NDArray[np.float64], + profile: K1LocalSurfaceProfile, +) -> tuple[npt.NDArray[np.int64], npt.NDArray[np.float64]]: + if cloud.shape[0] == 0: + return np.empty((0, 2), dtype=np.int64), np.empty((0, 3), dtype=np.float64) + cells = np.floor(cloud[:, :2] / profile.cell_size_m).astype(np.int64) + order = np.lexsort((cells[:, 1], cells[:, 0])) + sorted_cells = cells[order] + sorted_z = cloud[order, 2] + changes: npt.NDArray[np.int64] = ( + np.flatnonzero(np.any(np.diff(sorted_cells, axis=0) != 0, axis=1)) + 1 + ).astype(np.int64, copy=False) + starts = np.concatenate((np.asarray([0]), changes)) + ends = np.concatenate((changes, np.asarray([cloud.shape[0]]))) + keys = np.empty((starts.shape[0], 2), dtype=np.int64) + points = np.empty((starts.shape[0], 3), dtype=np.float64) + half_cell = profile.cell_size_m * 0.5 + for index, (start, end) in enumerate(zip(starts, ends, strict=True)): + keys[index] = sorted_cells[start] + points[index] = ( + float(sorted_cells[start, 0]) * profile.cell_size_m + half_cell, + float(sorted_cells[start, 1]) * profile.cell_size_m + half_cell, + float(np.percentile(sorted_z[start:end], profile.cell_lower_percentile)), + ) + return keys, points + + +def expire_cache( + cache: dict[tuple[int, int], tuple[float, float]], + session_seconds: float, + position: npt.NDArray[np.float64], + profile: K1LocalSurfaceProfile, +) -> None: + maximum_radius_sq = (profile.local_radius_m + profile.cell_size_m) ** 2 + expired = [ + key + for key, (_, observed_seconds) in cache.items() + if session_seconds - observed_seconds > profile.surface_ttl_s + or ((key[0] + 0.5) * profile.cell_size_m - float(position[0])) ** 2 + + ((key[1] + 0.5) * profile.cell_size_m - float(position[1])) ** 2 + > maximum_radius_sq + ] + for key in expired: + del cache[key] + + +def local_cache_records( + cache: Mapping[tuple[int, int], tuple[float, float]], + position: npt.NDArray[np.float64], + profile: K1LocalSurfaceProfile, +) -> tuple[ + npt.NDArray[np.int64], + npt.NDArray[np.float64], + npt.NDArray[np.float64], +]: + values = [ + ( + (key[0] + 0.5) * profile.cell_size_m, + (key[1] + 0.5) * profile.cell_size_m, + z, + observed_seconds, + ) + for key, (z, observed_seconds) in sorted(cache.items()) + if ( + ((key[0] + 0.5) * profile.cell_size_m - float(position[0])) ** 2 + + ((key[1] + 0.5) * profile.cell_size_m - float(position[1])) ** 2 + <= profile.local_radius_m**2 + ) + ] + if not values: + return ( + np.empty((0, 2), dtype=np.int64), + np.empty((0, 3), dtype=np.float64), + np.empty(0, dtype=np.float64), + ) + array: npt.NDArray[np.float64] = np.asarray(values, dtype=np.float64) + keys = np.floor(array[:, :2] / profile.cell_size_m).astype(np.int64) + return keys, array[:, :3], array[:, 3] + + +def fit_surface( + cell_points: npt.NDArray[np.float64], + position: npt.NDArray[np.float64], + profile: K1LocalSurfaceProfile, +) -> tuple[ + npt.NDArray[np.float64], + npt.NDArray[np.bool_], + npt.NDArray[np.float64], +] | None: + centered_xy = cell_points[:, :2] - position[:2] + design = np.column_stack( + (centered_xy[:, 0], centered_xy[:, 1], np.ones(cell_points.shape[0])) + ) + cutoff = float(np.quantile(cell_points[:, 2], profile.initial_lower_fraction)) + inliers = cell_points[:, 2] <= cutoff + if int(np.count_nonzero(inliers)) < profile.minimum_surface_cells: + return None + coefficients: npt.NDArray[np.float64] = np.zeros(3, dtype=np.float64) + for _ in range(profile.robust_iterations): + try: + coefficients, _, rank, _ = np.linalg.lstsq( + design[inliers], cell_points[inliers, 2], rcond=None + ) + except np.linalg.LinAlgError: + return None + if rank < 3 or not np.isfinite(coefficients).all(): + return None + residuals = cell_points[:, 2] - design @ coefficients + center = float(np.median(residuals[inliers])) + mad = float(np.median(np.abs(residuals[inliers] - center))) + band = max( + profile.minimum_inlier_band_m, + profile.robust_mad_scale * 1.4826 * mad, + ) + updated = np.abs(residuals - center) <= band + if int(np.count_nonzero(updated)) < profile.minimum_surface_cells: + return None + if np.array_equal(updated, inliers): + break + inliers = updated + coefficients, _, rank, _ = np.linalg.lstsq( + design[inliers], cell_points[inliers, 2], rcond=None + ) + if rank < 3 or not np.isfinite(coefficients).all(): + return None + a, b, c = (float(value) for value in coefficients) + unnormalized: npt.NDArray[np.float64] = np.asarray( + [-a, -b, 1.0, a * float(position[0]) + b * float(position[1]) - c], + dtype=np.float64, + ) + norm = float(np.linalg.norm(unnormalized[:3])) + if norm <= 0 or not np.isfinite(norm): + return None + plane = unnormalized / norm + residuals = height_above_plane(cell_points, plane) + center = float(np.median(residuals[inliers])) + mad = float(np.median(np.abs(residuals[inliers] - center))) + band = max( + profile.minimum_inlier_band_m, + profile.robust_mad_scale * 1.4826 * mad, + ) + inliers = np.abs(residuals - center) <= band + if int(np.count_nonzero(inliers)) < profile.minimum_surface_cells: + return None + return plane.astype(" PredictionEvidence | None: + if ( + prior_cell_points.shape[0] < profile.minimum_surface_cells + or current_cell_points.shape[0] < profile.minimum_surface_cells + ): + return None + prior_fit = fit_surface(prior_cell_points, position, profile) + if prior_fit is None: + return None + prior_plane, _, _ = prior_fit + cutoff = float(np.quantile(current_cell_points[:, 2], profile.initial_lower_fraction)) + evaluation = current_cell_points[:, 2] <= cutoff + if int(np.count_nonzero(evaluation)) < profile.minimum_surface_cells: + return None + evaluation_points = current_cell_points[evaluation] + signed_residuals = height_above_plane(evaluation_points, prior_plane) + if signed_residuals.size == 0 or not np.isfinite(signed_residuals).all(): + return None + return PredictionEvidence( + prior_plane=prior_plane, + cell_points=evaluation_points, + signed_residuals=signed_residuals, + ) + + +def step_candidate_keys( + cell_keys: npt.NDArray[np.int64], + cell_points: npt.NDArray[np.float64], + plane: npt.NDArray[np.float64], + profile: K1LocalSurfaceProfile, +) -> set[tuple[int, int]]: + if cell_keys.shape[0] != cell_points.shape[0]: + raise LidarGroundError("K1 local-surface cell alignment is invalid") + residual = height_above_plane(cell_points, plane) + lookup = { + (int(key[0]), int(key[1])): float(value) + for key, value in zip(cell_keys, residual, strict=True) + if abs(float(value)) <= profile.step_max_plane_residual_m + } + candidates: set[tuple[int, int]] = set() + for key, value in lookup.items(): + for neighbor in ((key[0] + 1, key[1]), (key[0], key[1] + 1)): + neighbor_value = lookup.get(neighbor) + if neighbor_value is None: + continue + delta = abs(value - neighbor_value) + if profile.step_min_height_m <= delta <= profile.step_max_height_m: + candidates.add(key) + candidates.add(neighbor) + return candidates + + +def point_step_candidates( + cloud: npt.NDArray[np.float64], + local: npt.NDArray[np.bool_], + heights: npt.NDArray[np.float64], + candidate_keys: set[tuple[int, int]], + profile: K1LocalSurfaceProfile, +) -> npt.NDArray[np.uint8]: + result = np.zeros(cloud.shape[0], dtype=np.uint8) + if not candidate_keys: + return result + cells = np.floor(cloud[:, :2] / profile.cell_size_m).astype(np.int64) + for index in np.flatnonzero(local): + key = (int(cells[index, 0]), int(cells[index, 1])) + if ( + key in candidate_keys + and abs(float(heights[index])) <= profile.step_max_plane_residual_m + ): + result[index] = 1 + return result + + +def height_above_plane( + points: npt.NDArray[np.float64], + plane: npt.NDArray[np.float64], +) -> npt.NDArray[np.float64]: + return points @ plane[:3] + float(plane[3]) diff --git a/src/k1link/compute/lidar_local_surface_shadow.py b/src/k1link/compute/lidar_local_surface_shadow.py index 19058d0..428f7e5 100644 --- a/src/k1link/compute/lidar_local_surface_shadow.py +++ b/src/k1link/compute/lidar_local_surface_shadow.py @@ -13,26 +13,45 @@ import numpy.typing as npt from k1link.data_plane import DecodedPointCloudView, DecodedPoseView from k1link.ground_segmentation import GroundSegmentationError as LidarGroundError -from .lidar_local_surface import ( +from .lidar_local_surface_geometry import ( DEFAULT_K1_LOCAL_SURFACE_PROFILE, POINT_BELOW_SURFACE, POINT_OCCUPIED, POINT_SURFACE, K1LocalSurfaceProfile, - _cloud_cell_observations, - _expire_cache, - _fit_surface, - _height_above_plane, - _local_cache_records, - _point_step_candidates, - _prediction_metrics, - _step_candidate_keys, - _update_cache, +) +from .lidar_local_surface_geometry import ( + cloud_cell_observations as _cloud_cell_observations, +) +from .lidar_local_surface_geometry import ( + expire_cache as _expire_cache, +) +from .lidar_local_surface_geometry import ( + fit_surface as _fit_surface, +) +from .lidar_local_surface_geometry import ( + height_above_plane as _height_above_plane, +) +from .lidar_local_surface_geometry import ( + local_cache_records as _local_cache_records, +) +from .lidar_local_surface_geometry import ( + point_step_candidates as _point_step_candidates, +) +from .lidar_local_surface_geometry import ( + prediction_metrics as _prediction_metrics, +) +from .lidar_local_surface_geometry import ( + step_candidate_keys as _step_candidate_keys, +) +from .lidar_local_surface_geometry import ( + update_cache as _update_cache, ) from .live_perception import LatestWinsQueue K1_LOCAL_SURFACE_SHADOW_SCHEMA: Final = "missioncore.k1-local-surface-shadow-runtime/v1" K1_LOCAL_SURFACE_SHADOW_FRAME_SCHEMA: Final = "missioncore.k1-local-surface-shadow-frame/v1" +K1_LOCAL_SURFACE_BINDER_SCHEMA: Final = "missioncore.k1-local-surface-pose-binder/v1" ShadowFrameState = Literal[ "valid", @@ -42,6 +61,208 @@ ShadowFrameState = Literal[ ] +@dataclass(frozen=True, slots=True) +class K1LocalSurfaceBoundViews: + """One point frame paired to the nearest admitted host-arrival pose.""" + + point_cloud: DecodedPointCloudView + pose: DecodedPoseView + pose_binding_age_ms: float + + +class K1LocalSurfacePoseBinder: + """Bounded event-order-independent LiDAR↔pose binding for shadow work.""" + + def __init__( + self, + *, + maximum_pose_binding_ms: float, + point_capacity: int = 2, + pose_capacity: int = 16, + future_pose_wait_ms: float = 25.0, + retention_seconds: float = 3.0, + ) -> None: + if ( + not math.isfinite(maximum_pose_binding_ms) + or not 1 <= maximum_pose_binding_ms <= 10_000 + or not 1 <= point_capacity <= 8 + or not 2 <= pose_capacity <= 256 + or not math.isfinite(future_pose_wait_ms) + or not 0 <= future_pose_wait_ms <= maximum_pose_binding_ms + or not math.isfinite(retention_seconds) + or not 0.1 <= retention_seconds <= 30 + ): + raise LidarGroundError("K1 local-surface pose binder bounds are invalid") + self._maximum_delta_ns = round(maximum_pose_binding_ms * 1_000_000) + self._future_wait_ns = round(future_pose_wait_ms * 1_000_000) + self._retention_ns = round(retention_seconds * 1_000_000_000) + self._point_capacity = point_capacity + self._pose_capacity = pose_capacity + self._points: deque[DecodedPointCloudView] = deque() + self._poses: deque[DecodedPoseView] = deque() + self._lock = threading.Lock() + self._latest_time_ns = 0 + self._last_point_sequence = 0 + self._last_pose_sequence = 0 + self._point_published = 0 + self._pose_published = 0 + self._point_bound = 0 + self._point_missed = 0 + self._point_dropped_overflow = 0 + self._pose_dropped_overflow = 0 + self._maximum_point_depth = 0 + self._maximum_pose_depth = 0 + self._binding_age_ms: deque[float] = deque(maxlen=512) + + def publish_point_cloud( + self, + value: DecodedPointCloudView, + ) -> tuple[K1LocalSurfaceBoundViews, ...]: + if value.frame_id != "map": + raise LidarGroundError("K1 local-surface pose binder requires map-frame points") + with self._lock: + if value.context.sequence <= self._last_point_sequence: + raise LidarGroundError("K1 local-surface point sequence is not increasing") + self._last_point_sequence = value.context.sequence + self._point_published += 1 + if len(self._points) == self._point_capacity: + self._points.popleft() + self._point_dropped_overflow += 1 + self._points.append(value) + self._maximum_point_depth = max( + self._maximum_point_depth, + len(self._points), + ) + self._latest_time_ns = max(self._latest_time_ns, _view_time_ns(value)) + self._prune_poses_locked() + return self._drain_locked(force=False) + + def publish_pose( + self, + value: DecodedPoseView, + ) -> tuple[K1LocalSurfaceBoundViews, ...]: + if value.frame_id != "map" or value.child_frame_id != "sensor": + raise LidarGroundError("K1 local-surface pose binder requires map-from-sensor pose") + with self._lock: + if value.context.sequence <= self._last_pose_sequence: + raise LidarGroundError("K1 local-surface pose sequence is not increasing") + self._last_pose_sequence = value.context.sequence + self._pose_published += 1 + if len(self._poses) == self._pose_capacity: + self._poses.popleft() + self._pose_dropped_overflow += 1 + self._poses.append(value) + self._maximum_pose_depth = max( + self._maximum_pose_depth, + len(self._poses), + ) + self._latest_time_ns = max(self._latest_time_ns, _view_time_ns(value)) + self._prune_poses_locked() + return self._drain_locked(force=False) + + def flush(self) -> tuple[K1LocalSurfaceBoundViews, ...]: + with self._lock: + return self._drain_locked(force=True) + + def snapshot(self) -> dict[str, object]: + with self._lock: + ages: npt.NDArray[np.float64] = np.asarray( + self._binding_age_ms, + dtype=np.float64, + ) + return { + "schema_version": K1_LOCAL_SURFACE_BINDER_SCHEMA, + "clock_basis": "host-monotonic-arrival", + "maximum_pose_binding_ms": self._maximum_delta_ns / 1_000_000, + "future_pose_wait_ms": self._future_wait_ns / 1_000_000, + "points": { + "capacity": self._point_capacity, + "depth": len(self._points), + "maximum_depth": self._maximum_point_depth, + "published": self._point_published, + "bound": self._point_bound, + "missed": self._point_missed, + "dropped_overflow": self._point_dropped_overflow, + }, + "poses": { + "capacity": self._pose_capacity, + "depth": len(self._poses), + "maximum_depth": self._maximum_pose_depth, + "published": self._pose_published, + "dropped_overflow": self._pose_dropped_overflow, + }, + "binding_age_ms": { + "sample_count": int(ages.shape[0]), + "p50": float(np.percentile(ages, 50)) if ages.size else None, + "p95": float(np.percentile(ages, 95)) if ages.size else None, + "maximum": float(np.max(ages)) if ages.size else None, + }, + "authority": { + "commands_enabled": False, + "navigation_or_safety_accepted": False, + }, + } + + def _drain_locked( + self, + *, + force: bool, + ) -> tuple[K1LocalSurfaceBoundViews, ...]: + bound: list[K1LocalSurfaceBoundViews] = [] + while self._points: + point_cloud = self._points[0] + point_time_ns = _view_time_ns(point_cloud) + pose = min( + self._poses, + key=lambda candidate: abs(_view_time_ns(candidate) - point_time_ns), + default=None, + ) + if pose is None: + if force or self._latest_time_ns - point_time_ns >= self._maximum_delta_ns: + self._points.popleft() + self._point_missed += 1 + continue + break + pose_time_ns = _view_time_ns(pose) + delta_ns = abs(pose_time_ns - point_time_ns) + future_watermark_reached = self._latest_time_ns - point_time_ns >= self._future_wait_ns + recent_prior_pose = pose_time_ns <= point_time_ns and delta_ns <= self._future_wait_ns + if delta_ns <= self._maximum_delta_ns and ( + force + or pose_time_ns >= point_time_ns + or recent_prior_pose + or future_watermark_reached + ): + self._points.popleft() + age_ms = delta_ns / 1_000_000 + self._point_bound += 1 + self._binding_age_ms.append(age_ms) + bound.append( + K1LocalSurfaceBoundViews( + point_cloud=point_cloud, + pose=pose, + pose_binding_age_ms=age_ms, + ) + ) + continue + if force or self._latest_time_ns - point_time_ns >= self._maximum_delta_ns: + self._points.popleft() + self._point_missed += 1 + continue + break + return tuple(bound) + + def _prune_poses_locked(self) -> None: + cutoff = self._latest_time_ns - self._retention_ns + while self._poses and _view_time_ns(self._poses[0]) < cutoff: + self._poses.popleft() + + +def _view_time_ns(value: DecodedPointCloudView | DecodedPoseView) -> int: + received = value.context.received_monotonic_ns + return int(received if received is not None else value.context.captured_at_epoch_ns) + + @dataclass(frozen=True, slots=True) class K1LocalSurfaceShadowInput: """One immutable map-point/pose pair admitted to passive shadow work.""" @@ -111,7 +332,9 @@ class K1LocalSurfaceShadowInput: pose_binding_age_ms=pose_binding_age_ms, points_map=points, position_map=position, - published_monotonic_ns=time.monotonic_ns(), + published_monotonic_ns=( + point_cloud.context.processing_started_monotonic_ns + ), ) @@ -561,6 +784,8 @@ class K1LocalSurfaceShadowRuntime: self._processed = 0 self._failed = 0 self._state_counts: Counter[str] = Counter() + self._processing_ms: deque[float] = deque(maxlen=512) + self._result_age_ms: deque[float] = deque(maxlen=512) self._last_error: str | None = None self._inflight = False self._condition = threading.Condition() @@ -638,6 +863,8 @@ class K1LocalSurfaceShadowRuntime: "dropped_ring_overflow": self._result_dropped, "state_counts": dict(sorted(self._state_counts.items())), "failed": self._failed, + "processing_ms": _bounded_distribution(self._processing_ms), + "result_age_ms": _bounded_distribution(self._result_age_ms), "latest": latest.document() if latest is not None else None, }, "last_error": self._last_error, @@ -675,7 +902,128 @@ class K1LocalSurfaceShadowRuntime: self._results.append(result) self._processed += 1 self._state_counts[result.state] += 1 + self._processing_ms.append(result.processing_ms) + self._result_age_ms.append(result.result_age_ms) finally: with self._condition: self._inflight = False self._condition.notify_all() + + +class K1LocalSurfaceShadowCoordinator: + """Lazy session lifecycle around the bounded pose binder and estimator.""" + + def __init__( + self, + *, + profile: K1LocalSurfaceProfile = DEFAULT_K1_LOCAL_SURFACE_PROFILE, + point_capacity: int = 2, + pose_capacity: int = 16, + future_pose_wait_ms: float = 25.0, + retention_seconds: float = 3.0, + result_capacity: int = 8, + ) -> None: + self.profile = profile + self._queue_capacity = point_capacity + self._result_capacity = result_capacity + self._binder = K1LocalSurfacePoseBinder( + maximum_pose_binding_ms=profile.maximum_pose_binding_ms, + point_capacity=point_capacity, + pose_capacity=pose_capacity, + future_pose_wait_ms=future_pose_wait_ms, + retention_seconds=retention_seconds, + ) + self._lock = threading.Lock() + self._runtime: K1LocalSurfaceShadowRuntime | None = None + self._session_id: str | None = None + self._closed = False + + def begin_session(self, session_id: str) -> None: + with self._lock: + if self._closed: + raise RuntimeError("K1 local-surface shadow coordinator is closed") + if self._runtime is not None: + if self._session_id == session_id: + return + raise RuntimeError( + "K1 local-surface shadow coordinator session changed" + ) + self._runtime = K1LocalSurfaceShadowRuntime( + session_id, + profile=self.profile, + queue_capacity=self._queue_capacity, + result_capacity=self._result_capacity, + ) + self._session_id = session_id + + def publish_point_cloud(self, value: DecodedPointCloudView) -> int: + runtime = self._active_runtime() + bindings = self._binder.publish_point_cloud(value) + for binding in bindings: + runtime.publish_views(binding.point_cloud, binding.pose) + return len(bindings) + + def publish_pose(self, value: DecodedPoseView) -> int: + runtime = self._active_runtime() + bindings = self._binder.publish_pose(value) + for binding in bindings: + runtime.publish_views(binding.point_cloud, binding.pose) + return len(bindings) + + def close(self, *, timeout_seconds: float = 30.0) -> None: + with self._lock: + if self._closed: + return + self._closed = True + runtime = self._runtime + if runtime is None: + return + for binding in self._binder.flush(): + runtime.publish_views(binding.point_cloud, binding.pose) + runtime.close(timeout_seconds=timeout_seconds) + + def snapshot(self) -> dict[str, object]: + with self._lock: + runtime = self._runtime + session_id = self._session_id + closed = self._closed + runtime_snapshot = runtime.snapshot() if runtime is not None else None + return { + "schema_version": K1_LOCAL_SURFACE_SHADOW_SCHEMA, + "mode": "physical-live-shadow-diagnostic-only", + "session_id": session_id, + "binder": self._binder.snapshot(), + "runtime": runtime_snapshot, + "active": runtime is not None and not closed, + "closed": closed and ( + runtime_snapshot is None or bool(runtime_snapshot["closed"]) + ), + "ground_truth": False, + "authority": { + "commands_enabled": False, + "navigation_or_safety_accepted": False, + }, + } + + def _active_runtime(self) -> K1LocalSurfaceShadowRuntime: + with self._lock: + if self._closed: + raise RuntimeError("K1 local-surface shadow coordinator is closed") + runtime = self._runtime + if runtime is None: + raise RuntimeError( + "K1 local-surface shadow coordinator session is not active" + ) + return runtime + + +def _bounded_distribution( + values: deque[float], +) -> dict[str, float | int | None]: + array: npt.NDArray[np.float64] = np.asarray(values, dtype=np.float64) + return { + "sample_count": int(array.shape[0]), + "p50": float(np.percentile(array, 50)) if array.size else None, + "p95": float(np.percentile(array, 95)) if array.size else None, + "maximum": float(np.max(array)) if array.size else None, + } diff --git a/tests/test_e15_shadow_inference.py b/tests/test_e15_shadow_inference.py index 79590d0..0e86a0e 100644 --- a/tests/test_e15_shadow_inference.py +++ b/tests/test_e15_shadow_inference.py @@ -84,6 +84,138 @@ def test_e15_profile_rejects_command_authority(tmp_path: Path) -> None: module.read_live_profile(changed) +def test_e28_profile_pins_physical_k1_local_surface_gate() -> None: + module = _module() + profile_path = ( + Path(__file__).resolve().parents[1] + / "experiments" + / "perception" + / "worker" + / "e28_physical_k1_local_surface_profile.json" + ) + + profile, digest = module.read_live_profile(profile_path) + + assert len(digest) == 64 + assert profile["mode"] == "physical-shadow-gate" + assert profile["local_surface"] == { + "enabled": True, + "profile_id": "k1-vendor-map-dynamic-local-surface/v1", + "profile_sha256": ( + "7a59edc8404d0177a39175578743589bfb6b7822837170cded38ca2b6698cc26" + ), + "point_queue_capacity": 2, + "pose_buffer_capacity": 16, + "future_pose_wait_ms": 25.0, + "retention_seconds": 3.0, + "result_capacity": 8, + "acceptance": { + "minimum_bound_frames": 100, + "maximum_pose_miss_fraction": 0.05, + "maximum_point_drop_fraction": 0.01, + "maximum_runtime_drop_fraction": 0.01, + "maximum_p95_result_age_ms": 80.0, + }, + } + assert profile["authority"] == { + "commands_enabled": False, + "navigation_or_safety_accepted": False, + } + + +def test_e28_profile_rejects_unpinned_local_surface(tmp_path: Path) -> None: + module = _module() + source = ( + Path(__file__).resolve().parents[1] + / "experiments" + / "perception" + / "worker" + / "e28_physical_k1_local_surface_profile.json" + ) + value = json.loads(source.read_text()) + value["local_surface"]["profile_sha256"] = "0" * 64 + changed = tmp_path / "unpinned-local-surface.json" + changed.write_text(json.dumps(value)) + + with pytest.raises(RuntimeError, match="E28 physical local-surface"): + module.read_live_profile(changed) + + +def test_e28_local_surface_acceptance_requires_exact_bounded_accounting() -> None: + module = _module() + config = { + "profile_id": "k1-vendor-map-dynamic-local-surface/v1", + "point_queue_capacity": 2, + "pose_buffer_capacity": 16, + "acceptance": { + "minimum_bound_frames": 100, + "maximum_pose_miss_fraction": 0.05, + "maximum_point_drop_fraction": 0.01, + "maximum_runtime_drop_fraction": 0.01, + "maximum_p95_result_age_ms": 80.0, + }, + } + authority = { + "commands_enabled": False, + "navigation_or_safety_accepted": False, + } + snapshot = { + "closed": True, + "authority": authority, + "binder": { + "points": { + "capacity": 2, + "depth": 0, + "maximum_depth": 2, + "published": 102, + "bound": 100, + "missed": 1, + "dropped_overflow": 1, + }, + "poses": { + "capacity": 16, + "maximum_depth": 12, + }, + }, + "runtime": { + "closed": True, + "profile": { + "profile_id": "k1-vendor-map-dynamic-local-surface/v1", + }, + "queue": { + "capacity": 2, + "depth": 0, + "maximum_depth": 2, + "published": 100, + "consumed": 100, + "dropped_overflow": 0, + }, + "results": { + "published": 100, + "failed": 0, + "result_age_ms": {"p95": 40.0}, + }, + "occupancy_policy": { + "absence_of_points_means_free": False, + "unknown_is_traversable": False, + }, + "authority": authority, + }, + } + + checks = module._local_surface_acceptance_checks(snapshot, config) + + assert checks + assert all(checks.values()) + snapshot["runtime"]["results"]["result_age_ms"]["p95"] = 90.0 + assert ( + module._local_surface_acceptance_checks(snapshot, config)[ + "local_surface_maximum_p95_result_age_ms" + ] + is False + ) + + def test_persistent_worker_request_is_single_run_d_backed_and_token_bounded( tmp_path: Path, ) -> None: diff --git a/tests/test_e15_worker_package.py b/tests/test_e15_worker_package.py index db2b98d..c77f655 100644 --- a/tests/test_e15_worker_package.py +++ b/tests/test_e15_worker_package.py @@ -1,6 +1,8 @@ from __future__ import annotations import importlib.util +import os +import subprocess import sys from pathlib import Path @@ -29,10 +31,33 @@ def test_e15_worker_package_is_minimal_hash_addressed_projection(tmp_path: Path) assert package.name == f"e15-worker-package-{manifest['identity_sha256']}" assert manifest["identity"]["classification"] == ("minimal-live-worker-import-projection") - assert len(manifest["artifacts"]) == 12 + assert len(manifest["artifacts"]) == 15 assert (package / "k1link" / "compute" / "inline_temporal.py").is_file() + assert ( + package + / "k1link" + / "compute" + / "lidar_local_surface_shadow.py" + ).is_file() assert not (package / "k1link" / "device_plugins" / "xgrids_k1" / "mqtt").exists() assert ( "observation" not in (package / "k1link" / "device_plugins" / "xgrids_k1" / "__init__.py").read_text() ) + imported = subprocess.run( + [ + sys.executable, + "-c", + ( + "from k1link.compute.lidar_local_surface_shadow " + "import K1LocalSurfaceShadowCoordinator; " + "assert K1LocalSurfaceShadowCoordinator" + ), + ], + cwd=tmp_path, + env={**os.environ, "PYTHONPATH": str(package)}, + check=False, + capture_output=True, + text=True, + ) + assert imported.returncode == 0, imported.stderr diff --git a/tests/test_lidar_local_surface.py b/tests/test_lidar_local_surface.py index 62ab882..8a3dd9b 100644 --- a/tests/test_lidar_local_surface.py +++ b/tests/test_lidar_local_surface.py @@ -19,6 +19,12 @@ from k1link.compute import ( 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.web.lidar_api import build_lidar_router @@ -36,6 +42,10 @@ 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 _source_pack(root: Path) -> Path: frame_count = 8 available = np.asarray([True, True, True, True, True, True, True, False]) diff --git a/tests/test_live_perception_synchronizer.py b/tests/test_live_perception_synchronizer.py index 6c89297..34fb2b1 100644 --- a/tests/test_live_perception_synchronizer.py +++ b/tests/test_live_perception_synchronizer.py @@ -3,6 +3,12 @@ from __future__ import annotations import threading import time +import pytest + +from k1link.compute import ( + K1LocalSurfacePoseBinder, + K1LocalSurfaceShadowCoordinator, +) from k1link.compute.live_perception import LiveSensorSynchronizer from k1link.data_plane import ConsumerFrameContext, DecodedPointCloudView, DecodedPoseView @@ -66,10 +72,7 @@ def test_live_sensor_synchronizer_reports_each_fail_closed_boundary() -> None: assert synchronizer.bind_camera(1_000_000_000).state == "lidar-unavailable" synchronizer.publish_point_cloud(_points(1, 900_000_000)) - assert ( - synchronizer.bind_camera(1_000_000_000).state - == "lidar-camera-delta-exceeded" - ) + assert synchronizer.bind_camera(1_000_000_000).state == "lidar-camera-delta-exceeded" synchronizer.publish_point_cloud(_points(2, 1_000_000_000)) assert synchronizer.bind_camera(1_000_000_000).state == "pose-unavailable" @@ -116,3 +119,80 @@ def test_live_sensor_synchronizer_storage_never_exceeds_capacity() -> None: assert snapshot["pose_depth"] == 3 assert snapshot["evicted_points"] == 4 assert snapshot["evicted_poses"] == 4 + + +def test_local_surface_pose_binder_accepts_either_event_order() -> None: + points_first = K1LocalSurfacePoseBinder(maximum_pose_binding_ms=100) + assert points_first.publish_point_cloud(_points(1, 1_000_000_000)) == () + bound = points_first.publish_pose(_pose(1, 1_010_000_000)) + assert len(bound) == 1 + assert bound[0].point_cloud.context.sequence == 1 + assert bound[0].pose.context.sequence == 1 + assert bound[0].pose_binding_age_ms == 10.0 + + pose_first = K1LocalSurfacePoseBinder(maximum_pose_binding_ms=100) + assert pose_first.publish_pose(_pose(1, 2_000_000_000)) == () + bound = pose_first.publish_point_cloud(_points(1, 2_010_000_000)) + assert len(bound) == 1 + assert bound[0].pose_binding_age_ms == 10.0 + + snapshot = pose_first.snapshot() + assert snapshot["points"]["published"] == 1 + assert snapshot["points"]["bound"] == 1 + assert snapshot["points"]["missed"] == 0 + assert snapshot["authority"]["commands_enabled"] is False + + +def test_local_surface_pose_binder_is_bounded_and_accounts_for_misses() -> None: + binder = K1LocalSurfacePoseBinder( + maximum_pose_binding_ms=100, + point_capacity=2, + pose_capacity=3, + retention_seconds=10, + ) + for sequence in range(1, 6): + assert ( + binder.publish_point_cloud(_points(sequence, 1_000_000_000 + sequence * 1_000_000)) + == () + ) + assert binder.flush() == () + snapshot = binder.snapshot() + points = snapshot["points"] + assert points["capacity"] == 2 + assert points["maximum_depth"] == 2 + assert points["published"] == 5 + assert points["bound"] == 0 + assert points["dropped_overflow"] == 3 + assert points["missed"] == 2 + assert ( + points["bound"] + points["dropped_overflow"] + points["missed"] + points["depth"] + == points["published"] + ) + + +def test_local_surface_pose_binder_fails_closed_after_pose_deadline() -> None: + binder = K1LocalSurfacePoseBinder(maximum_pose_binding_ms=100) + assert binder.publish_point_cloud(_points(1, 1_000_000_000)) == () + assert binder.publish_pose(_pose(1, 1_200_000_000)) == () + snapshot = binder.snapshot() + assert snapshot["points"]["missed"] == 1 + assert snapshot["points"]["depth"] == 0 + + with pytest.raises(ValueError, match="sequence"): + binder.publish_pose(_pose(1, 1_210_000_000)) + + +def test_local_surface_shadow_coordinator_closes_exact_accounting() -> None: + coordinator = K1LocalSurfaceShadowCoordinator(result_capacity=2) + coordinator.begin_session("physical-k1-shadow-test") + assert coordinator.publish_pose(_pose(1, 1_000_000_000)) == 0 + assert coordinator.publish_point_cloud(_points(1, 1_010_000_000)) == 1 + coordinator.close() + + snapshot = coordinator.snapshot() + assert snapshot["closed"] is True + assert snapshot["binder"]["points"]["bound"] == 1 + assert snapshot["runtime"]["queue"]["published"] == 1 + assert snapshot["runtime"]["queue"]["consumed"] == 1 + assert snapshot["runtime"]["results"]["failed"] == 0 + assert snapshot["authority"]["navigation_or_safety_accepted"] is False