feat: qualify bounded K1 surface shadow
This commit is contained in:
DCCONSTRUCTIONS
parent
4c83e8a4e7
commit
e6d5411bdd
10
README.md
10
README.md
|
|
@ -158,6 +158,16 @@ in-band, above-plane and below-plane observations. On source frame `1254`,
|
|||
localizing the heavy tail without naming an object or granting safety
|
||||
authority.
|
||||
|
||||
The same estimator now runs behind
|
||||
`missioncore.k1-local-surface-shadow-runtime/v1`, a capacity-two
|
||||
latest-wins queue with a bounded diagnostic result ring. LAB E27 passed a
|
||||
`14.995 s` RAVNOVES00 slice at recorded 1× pace: `143/143` available frames
|
||||
were consumed, maximum queue depth was `1/2`, no frame was replaced, p95
|
||||
processing latency was `14.777 ms`, and state, point classes, step candidates
|
||||
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 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:
|
||||
|
|
|
|||
|
|
@ -4,7 +4,8 @@ Date: 2026-07-26
|
|||
Status: accepted architecture plan; L0/L1 implemented; L2 diagnostic A/B
|
||||
complete; full GOOSE and RELLIS qualification complete; L2.6d K1 replay
|
||||
local-surface temporal qualification, operator triage and prior-plane residual
|
||||
explainability implemented; bounded live shadow next
|
||||
explainability implemented; L2.6e recorded-source-paced bounded shadow
|
||||
qualified; physical K1 shadow next
|
||||
Scope: passively received real-time K1 point/pose evidence, immutable replay and
|
||||
future live shadow processing
|
||||
Explicitly out of scope: K1 firmware modification, a new onboard exporter, new
|
||||
|
|
@ -419,8 +420,11 @@ Dataset expansion is no longer the next gate.
|
|||
aggregate p95.
|
||||
- [ ] Complete the remaining qualification report with per-frame latency,
|
||||
point age, obstacle preservation and memory growth.
|
||||
- [ ] Replay the same profiles through a bounded latest-wins live-shadow queue;
|
||||
no K1 command, navigation or safety authority is added.
|
||||
- [x] Replay the same profile through a bounded latest-wins shadow queue at the
|
||||
recorded 1× source rate; no K1 command, navigation or safety authority is
|
||||
added.
|
||||
- [ ] Bind that runtime to physical live K1 point/pose evidence through the
|
||||
existing authenticated external-worker seam.
|
||||
|
||||
The implemented `missioncore.k1-local-surface/v1` derivative is reproducible
|
||||
through `experiments/perception/run_k1_local_surface.py` and is exposed
|
||||
|
|
@ -496,9 +500,27 @@ independent ground truth. Frame `1195` instead has only seven out-of-band cells
|
|||
together, preserving its separate interpretation as a surface-regime
|
||||
transition.
|
||||
|
||||
No fit threshold was changed after this review. The next gate is a bounded
|
||||
latest-wins live-shadow queue using the same profile and evidence contract,
|
||||
still without commands, free-space, navigation or safety authority.
|
||||
No fit threshold was changed after this review.
|
||||
|
||||
The L2.6e runtime contract
|
||||
`missioncore.k1-local-surface-shadow-runtime/v1` now copies and freezes one
|
||||
decoded map-point/pose pair, runs the accepted estimator behind a
|
||||
capacity-two latest-wins queue and retains only a bounded diagnostic result
|
||||
ring. It has no command method, never turns missing points into free space and
|
||||
publishes explicit false navigation/safety authority.
|
||||
|
||||
LAB E27 passed a `14.995 s` source-paced 1× RAVNOVES00 slice: all `143`
|
||||
available frames were consumed, maximum queue depth was `1/2`, no work was
|
||||
replaced and no processing failed. Processing latency was `12.261 ms` p50,
|
||||
`14.777 ms` p95 and `33.135 ms` maximum. Every processed result matched the
|
||||
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.
|
||||
|
||||
Exit: one immutable K1 session yields both a persistent reconstruction and a
|
||||
bounded local world state without hard-coded terrain height or scanner-side
|
||||
|
|
@ -521,7 +543,8 @@ independent gate without increasing unsafe false-free or false-dynamic output.
|
|||
|
||||
### L4 — live shadow integration
|
||||
|
||||
- [ ] Add a bounded LiDAR queue independent of camera cadence.
|
||||
- [x] Add a provider-neutral bounded LiDAR local-surface queue independent of
|
||||
camera cadence and qualify it at recorded 1× source pace.
|
||||
- [ ] 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.
|
||||
|
|
|
|||
|
|
@ -93,8 +93,9 @@ Not implemented:
|
|||
- no RELLIS ROS bag admission, continuous synchronized playback or production
|
||||
promotion;
|
||||
- no ray-cleared free-space or planner-authoritative rolling occupancy map.
|
||||
- no bounded live-shadow execution of the accepted K1 local-surface profile
|
||||
yet; the residual overlay remains replay-only and non-authoritative.
|
||||
- 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.
|
||||
|
||||
## Product surface boundary
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,97 @@
|
|||
# LAB E27 — bounded K1 local-surface shadow qualification
|
||||
|
||||
Date: 2026-07-26
|
||||
Status: **accepted for recorded-source-paced shadow diagnostic**
|
||||
Authority: diagnostic only; commands, navigation and safety acceptance disabled
|
||||
|
||||
## 1. Question
|
||||
|
||||
Can the accepted K1 rolling local-surface profile execute through a real
|
||||
bounded latest-wins worker loop at the recorded K1 source rate without queue
|
||||
growth, frame replacement or divergence from the immutable replay result?
|
||||
|
||||
This lab does not ask whether the derived surface is ground truth or
|
||||
planner-ready. It qualifies execution semantics only.
|
||||
|
||||
## 2. Fixed input
|
||||
|
||||
- session: `20260720T065719Z_viewer_live`;
|
||||
- source pack:
|
||||
`e10-lidar-pack-5da0396d32a27f9d1ca537cc2e8a371d386078d6f0dc71737b78620992af9625`;
|
||||
- source artifact SHA-256:
|
||||
`72aa73340b20fcfaa21b330ef5b93b975c14a70e2cd16a57752d9952ff05ad9a`;
|
||||
- replay reference:
|
||||
`k1-local-surface-628cd024775f02fea99765d1fb457efec2d5cc4d7371e56818dfe8e08c9b3b74`;
|
||||
- reference logical-content SHA-256:
|
||||
`b89a92887cedace9d3eab3e1490697fb6bc7fc16a2d1887f3fff7cb4547ceb14`;
|
||||
- selection: source frames `0–150`, `14.995 s`, 151 timeline entries and
|
||||
143 available LiDAR frames;
|
||||
- pace: recorded 1×;
|
||||
- work queue: latest-wins, capacity 2;
|
||||
- result ring: bounded to the 143-frame qualification selection.
|
||||
|
||||
The source pack and replay derivative were opened read-only. No scanner,
|
||||
firmware, MQTT command or persistent reconstruction was changed.
|
||||
|
||||
## 3. Implemented runtime boundary
|
||||
|
||||
`missioncore.k1-local-surface-shadow-runtime/v1` accepts only an already
|
||||
decoded map-frame point cloud and a compatible `T_map_from_sensor` pose. It:
|
||||
|
||||
1. copies and freezes the admitted point/pose pair;
|
||||
2. publishes work into a bounded latest-wins queue;
|
||||
3. runs the same robust rolling-cell and prior-only prediction profile used by
|
||||
replay;
|
||||
4. retains only a bounded diagnostic result ring;
|
||||
5. reports observed surface, observed occupied-above-surface, negative
|
||||
outliers, unverified step candidates, latency and freshness;
|
||||
6. explicitly keeps absence-of-points distinct from free space.
|
||||
|
||||
The runtime has no command method and every result carries
|
||||
`commands_enabled=false` and `navigation_or_safety_accepted=false`.
|
||||
|
||||
## 4. Result
|
||||
|
||||
Result:
|
||||
`k1-local-surface-shadow-04f14d8c580f74cbd5b0a452867563ebc6b3ef93d872129e4918680932253ab7`
|
||||
|
||||
| Metric | Result |
|
||||
| --- | ---: |
|
||||
| Published / consumed | 143 / 143 |
|
||||
| Latest-wins replacements | 0 |
|
||||
| Maximum queue depth | 1 / 2 |
|
||||
| Processing failures | 0 |
|
||||
| Processing p50 / p95 / max | 12.261 / 14.777 / 33.135 ms |
|
||||
| Result age p50 / p95 / max | 12.336 / 14.857 / 33.378 ms |
|
||||
| Replay state mismatches | 0 |
|
||||
| Point-class mismatches | 0 |
|
||||
| Step-candidate mismatches | 0 |
|
||||
| Maximum scalar delta | 0.0 |
|
||||
|
||||
All 143 results were valid for this selection. Queue accounting closed exactly:
|
||||
`consumed + dropped_overflow = published`, final depth was zero and the worker
|
||||
thread stopped cleanly.
|
||||
|
||||
## 5. Decision
|
||||
|
||||
The execution gate passes. The current CPU geometric profile is comfortably
|
||||
inside the observed roughly 10 Hz K1 publication interval on this 15-second
|
||||
slice, remains bounded and reproduces replay exactly when no work is replaced.
|
||||
|
||||
This result promotes the profile only from offline replay implementation to a
|
||||
recorded-source-paced shadow candidate. It does not promote:
|
||||
|
||||
- the surface estimate to ground truth;
|
||||
- observed occupancy to free-space evidence;
|
||||
- step candidates to semantic curbs;
|
||||
- the worker result to navigation or safety authority;
|
||||
- the replay transport to a physical-live K1 gate.
|
||||
|
||||
## 6. Next gate
|
||||
|
||||
Connect the runtime to the existing authenticated external worker stream using
|
||||
an explicit bounded LiDAR↔pose binder, then repeat at least 15 seconds against
|
||||
a physical K1 acquisition. Measure source sequence gaps, pose-binding misses,
|
||||
queue replacements, result age, memory slope and recovery across reconnect.
|
||||
React remains a read-only status/review surface; it does not execute the
|
||||
algorithm.
|
||||
|
|
@ -0,0 +1,365 @@
|
|||
#!/usr/bin/env python3
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import hashlib
|
||||
import json
|
||||
import math
|
||||
import os
|
||||
import time
|
||||
from datetime import UTC, datetime
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
from k1link.compute import (
|
||||
DEFAULT_K1_LOCAL_SURFACE_PROFILE,
|
||||
E10LidarFieldSource,
|
||||
K1LocalSurfaceShadowInput,
|
||||
K1LocalSurfaceShadowResult,
|
||||
K1LocalSurfaceShadowRuntime,
|
||||
K1LocalSurfaceV1,
|
||||
)
|
||||
from k1link.compute.lidar_local_surface import (
|
||||
FRAME_FIT_FAILED,
|
||||
FRAME_INSUFFICIENT_SURFACE,
|
||||
FRAME_POSE_STALE,
|
||||
FRAME_VALID,
|
||||
)
|
||||
|
||||
QUALIFICATION_SCHEMA = "missioncore.k1-local-surface-shadow-qualification/v1"
|
||||
|
||||
|
||||
def _arguments() -> argparse.Namespace:
|
||||
parser = argparse.ArgumentParser(
|
||||
description=(
|
||||
"Run the accepted K1 rolling-surface profile through a bounded "
|
||||
"latest-wins replay shadow and compare processed frames to the "
|
||||
"immutable replay derivative."
|
||||
)
|
||||
)
|
||||
parser.add_argument("source_pack", type=Path)
|
||||
parser.add_argument("reference_model", type=Path)
|
||||
parser.add_argument("output_root", type=Path)
|
||||
parser.add_argument("--start-frame", type=int, default=0)
|
||||
parser.add_argument("--duration-seconds", type=float, default=15.0)
|
||||
parser.add_argument("--pace-scale", type=float, default=1.0)
|
||||
parser.add_argument("--queue-capacity", type=int, default=2)
|
||||
return parser.parse_args()
|
||||
|
||||
|
||||
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:
|
||||
digest = hashlib.sha256()
|
||||
with path.open("rb") as stream:
|
||||
while chunk := stream.read(1024 * 1024):
|
||||
digest.update(chunk)
|
||||
return digest.hexdigest()
|
||||
|
||||
|
||||
def _distribution(values: list[float]) -> dict[str, float | int | None]:
|
||||
if not values:
|
||||
return {
|
||||
"sample_count": 0,
|
||||
"minimum": None,
|
||||
"mean": None,
|
||||
"p50": None,
|
||||
"p95": None,
|
||||
"maximum": None,
|
||||
}
|
||||
array = np.asarray(values, dtype=np.float64)
|
||||
if not np.isfinite(array).all():
|
||||
raise RuntimeError("shadow qualification latency is invalid")
|
||||
return {
|
||||
"sample_count": int(array.shape[0]),
|
||||
"minimum": float(np.min(array)),
|
||||
"mean": float(np.mean(array)),
|
||||
"p50": float(np.percentile(array, 50)),
|
||||
"p95": float(np.percentile(array, 95)),
|
||||
"maximum": float(np.max(array)),
|
||||
}
|
||||
|
||||
|
||||
def _expected_state(model: K1LocalSurfaceV1, frame_index: int) -> str:
|
||||
code = int(model.arrays["frame_failure_code"][frame_index])
|
||||
return {
|
||||
FRAME_VALID: "valid",
|
||||
FRAME_POSE_STALE: "pose-stale",
|
||||
FRAME_INSUFFICIENT_SURFACE: "insufficient-surface",
|
||||
FRAME_FIT_FAILED: "fit-failed",
|
||||
}[code]
|
||||
|
||||
|
||||
def _maximum_scalar_delta(
|
||||
result: K1LocalSurfaceShadowResult,
|
||||
model: K1LocalSurfaceV1,
|
||||
) -> float:
|
||||
frame_index = result.frame_index
|
||||
pairs = (
|
||||
(result.sensor_height_m, "sensor_height_m"),
|
||||
(result.slope_deg, "slope_deg"),
|
||||
(result.roughness_m, "roughness_m"),
|
||||
(result.confidence, "confidence"),
|
||||
(result.surface_max_age_ms, "surface_max_age_ms"),
|
||||
)
|
||||
deltas = [
|
||||
abs(float(value) - float(model.arrays[name][frame_index]))
|
||||
for value, name in pairs
|
||||
if value is not None
|
||||
]
|
||||
return max(deltas, default=0.0)
|
||||
|
||||
|
||||
def _qualification(
|
||||
source: E10LidarFieldSource,
|
||||
model: K1LocalSurfaceV1,
|
||||
*,
|
||||
start_frame: int,
|
||||
duration_seconds: float,
|
||||
pace_scale: float,
|
||||
queue_capacity: int,
|
||||
) -> dict[str, Any]:
|
||||
if (
|
||||
model.identity.get("source_pack_id") != source.pack_id
|
||||
or model.identity.get("source_pack_identity_sha256")
|
||||
!= source.manifest.get("identity_sha256")
|
||||
or model.identity.get("source_artifact_sha256")
|
||||
!= source.manifest.get("artifact", {}).get("sha256")
|
||||
or model.identity.get("profile") != DEFAULT_K1_LOCAL_SURFACE_PROFILE.to_dict()
|
||||
):
|
||||
raise RuntimeError("shadow qualification source/model binding is invalid")
|
||||
if (
|
||||
not 0 <= start_frame < source.frame_count
|
||||
or not math.isfinite(duration_seconds)
|
||||
or duration_seconds <= 0
|
||||
or not math.isfinite(pace_scale)
|
||||
or not 0 < pace_scale <= 10
|
||||
or not 1 <= queue_capacity <= 8
|
||||
):
|
||||
raise RuntimeError("shadow qualification selection is invalid")
|
||||
|
||||
arrays = source.arrays
|
||||
session_times = arrays["session_seconds"]
|
||||
start_seconds = float(session_times[start_frame])
|
||||
selected = [
|
||||
frame_index
|
||||
for frame_index in range(start_frame, source.frame_count)
|
||||
if float(session_times[frame_index]) - start_seconds <= duration_seconds
|
||||
]
|
||||
if len(selected) < 2:
|
||||
raise RuntimeError("shadow qualification selection is too short")
|
||||
expected_available = [
|
||||
frame_index for frame_index in selected if bool(arrays["sample_available"][frame_index])
|
||||
]
|
||||
runtime = K1LocalSurfaceShadowRuntime(
|
||||
f"{source.identity['session_id']}-qualification",
|
||||
queue_capacity=queue_capacity,
|
||||
result_capacity=min(256, max(1, len(expected_available))),
|
||||
)
|
||||
wall_started = time.perf_counter()
|
||||
offsets = arrays["cloud_offsets"]
|
||||
try:
|
||||
for frame_index in selected:
|
||||
release_at = (
|
||||
wall_started + (float(session_times[frame_index]) - start_seconds) * pace_scale
|
||||
)
|
||||
remaining = release_at - time.perf_counter()
|
||||
if remaining > 0:
|
||||
time.sleep(remaining)
|
||||
if not bool(arrays["sample_available"][frame_index]):
|
||||
continue
|
||||
start = int(offsets[frame_index])
|
||||
end = int(offsets[frame_index + 1])
|
||||
runtime.publish(
|
||||
K1LocalSurfaceShadowInput(
|
||||
frame_index=frame_index,
|
||||
source_frame_index=int(arrays["source_frame_indices"][frame_index]),
|
||||
session_seconds=float(session_times[frame_index]),
|
||||
pose_binding_age_ms=abs(float(arrays["pose_point_delta_ms"][frame_index])),
|
||||
points_map=np.asarray(
|
||||
arrays["cloud_points_map"][start:end],
|
||||
dtype=np.float64,
|
||||
),
|
||||
position_map=np.asarray(
|
||||
arrays["pose_positions_map"][frame_index],
|
||||
dtype=np.float64,
|
||||
),
|
||||
published_monotonic_ns=time.monotonic_ns(),
|
||||
)
|
||||
)
|
||||
runtime.close(timeout_seconds=30.0)
|
||||
results = runtime.results()
|
||||
runtime_snapshot = runtime.snapshot()
|
||||
finally:
|
||||
runtime.close()
|
||||
|
||||
state_mismatches = 0
|
||||
point_class_mismatches = 0
|
||||
step_candidate_mismatches = 0
|
||||
maximum_scalar_delta = 0.0
|
||||
for result in results:
|
||||
frame_index = result.frame_index
|
||||
state_mismatches += result.state != _expected_state(model, frame_index)
|
||||
if result.valid:
|
||||
start = int(offsets[frame_index])
|
||||
end = int(offsets[frame_index + 1])
|
||||
point_class_mismatches += int(
|
||||
np.count_nonzero(result.point_class != model.arrays["point_class"][start:end])
|
||||
)
|
||||
step_candidate_mismatches += int(
|
||||
np.count_nonzero(
|
||||
result.point_step_candidate != model.arrays["point_step_candidate"][start:end]
|
||||
)
|
||||
)
|
||||
maximum_scalar_delta = max(
|
||||
maximum_scalar_delta,
|
||||
_maximum_scalar_delta(result, model),
|
||||
)
|
||||
queue = runtime_snapshot["queue"]
|
||||
accepted = (
|
||||
int(queue["maximum_depth"]) <= queue_capacity
|
||||
and int(queue["dropped_overflow"]) == 0
|
||||
and int(queue["consumed"]) == len(expected_available)
|
||||
and len(results) == len(expected_available)
|
||||
and int(runtime_snapshot["results"]["failed"]) == 0
|
||||
and state_mismatches == 0
|
||||
and point_class_mismatches == 0
|
||||
and step_candidate_mismatches == 0
|
||||
and maximum_scalar_delta <= 1e-9
|
||||
)
|
||||
return {
|
||||
"schema_version": QUALIFICATION_SCHEMA,
|
||||
"identity": {
|
||||
"source_pack_id": source.pack_id,
|
||||
"source_pack_identity_sha256": source.manifest["identity_sha256"],
|
||||
"source_artifact_sha256": source.manifest["artifact"]["sha256"],
|
||||
"reference_model_id": model.model_id,
|
||||
"reference_logical_content_sha256": model.identity["logical_content_sha256"],
|
||||
"session_id": source.identity["session_id"],
|
||||
"profile": DEFAULT_K1_LOCAL_SURFACE_PROFILE.to_dict(),
|
||||
"selection": {
|
||||
"start_frame": start_frame,
|
||||
"end_frame": selected[-1],
|
||||
"source_duration_seconds": (float(session_times[selected[-1]]) - start_seconds),
|
||||
"selected_frames": len(selected),
|
||||
"available_frames": len(expected_available),
|
||||
},
|
||||
"runtime": {
|
||||
"queue_policy": "bounded-latest-wins",
|
||||
"queue_capacity": queue_capacity,
|
||||
"result_capacity": min(
|
||||
256,
|
||||
max(1, len(expected_available)),
|
||||
),
|
||||
"pace_scale": pace_scale,
|
||||
},
|
||||
"producer_sha256": _sha256(Path(__file__).resolve(strict=True)),
|
||||
},
|
||||
"state": "accepted" if accepted else "rejected",
|
||||
"accepted": accepted,
|
||||
"ground_truth": False,
|
||||
"metrics": {
|
||||
"wall_elapsed_seconds": time.perf_counter() - wall_started,
|
||||
"queue": queue,
|
||||
"result_states": runtime_snapshot["results"]["state_counts"],
|
||||
"processing_ms": _distribution([result.processing_ms for result in results]),
|
||||
"result_age_ms": _distribution([result.result_age_ms for result in results]),
|
||||
"replay_parity": {
|
||||
"compared_frames": len(results),
|
||||
"state_mismatches": state_mismatches,
|
||||
"point_class_mismatches": point_class_mismatches,
|
||||
"step_candidate_mismatches": step_candidate_mismatches,
|
||||
"maximum_scalar_delta": maximum_scalar_delta,
|
||||
},
|
||||
},
|
||||
"acceptance": {
|
||||
"queue_bounded": int(queue["maximum_depth"]) <= queue_capacity,
|
||||
"zero_latest_wins_replacements": int(queue["dropped_overflow"]) == 0,
|
||||
"zero_processing_failures": (int(runtime_snapshot["results"]["failed"]) == 0),
|
||||
"complete_processed_accounting": (
|
||||
int(queue["consumed"]) == len(expected_available)
|
||||
and len(results) == len(expected_available)
|
||||
),
|
||||
"replay_state_parity": state_mismatches == 0,
|
||||
"replay_point_class_parity": point_class_mismatches == 0,
|
||||
"replay_step_candidate_parity": step_candidate_mismatches == 0,
|
||||
"replay_scalar_parity": maximum_scalar_delta <= 1e-9,
|
||||
"navigation_or_safety_accepted": False,
|
||||
},
|
||||
"occupancy_policy": {
|
||||
"absence_of_points_means_free": False,
|
||||
"unknown_is_traversable": False,
|
||||
},
|
||||
"authority": {
|
||||
"commands_enabled": False,
|
||||
"navigation_or_safety_accepted": False,
|
||||
},
|
||||
}
|
||||
|
||||
|
||||
def main() -> int:
|
||||
arguments = _arguments()
|
||||
source = E10LidarFieldSource(arguments.source_pack)
|
||||
model = K1LocalSurfaceV1(arguments.reference_model)
|
||||
try:
|
||||
report = _qualification(
|
||||
source,
|
||||
model,
|
||||
start_frame=arguments.start_frame,
|
||||
duration_seconds=arguments.duration_seconds,
|
||||
pace_scale=arguments.pace_scale,
|
||||
queue_capacity=arguments.queue_capacity,
|
||||
)
|
||||
finally:
|
||||
model.close()
|
||||
source.close()
|
||||
report_sha256 = hashlib.sha256(_canonical_json(report)).hexdigest()
|
||||
result_id = f"k1-local-surface-shadow-{report_sha256}"
|
||||
report["result_id"] = result_id
|
||||
report["report_sha256"] = report_sha256
|
||||
report["created_at_utc"] = datetime.now(UTC).isoformat()
|
||||
output_root = arguments.output_root.expanduser().resolve()
|
||||
output_root.mkdir(mode=0o700, parents=True, exist_ok=True)
|
||||
output = output_root / result_id
|
||||
if output.exists():
|
||||
raise RuntimeError("shadow qualification result already exists")
|
||||
staging = output_root / f".{result_id}.{os.getpid()}.incomplete"
|
||||
staging.mkdir(mode=0o700, exist_ok=False)
|
||||
try:
|
||||
report_path = staging / "report.json"
|
||||
report_path.write_bytes(_canonical_json(report))
|
||||
os.replace(staging, output)
|
||||
except BaseException:
|
||||
if staging.exists():
|
||||
for path in staging.iterdir():
|
||||
path.unlink()
|
||||
staging.rmdir()
|
||||
raise
|
||||
print(
|
||||
json.dumps(
|
||||
{
|
||||
"result_id": result_id,
|
||||
"output": str(output),
|
||||
"state": report["state"],
|
||||
"metrics": report["metrics"],
|
||||
"authority": report["authority"],
|
||||
},
|
||||
ensure_ascii=False,
|
||||
indent=2,
|
||||
)
|
||||
)
|
||||
return 0 if report["accepted"] else 1
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
|
|
@ -118,6 +118,14 @@ from .lidar_local_surface import (
|
|||
build_k1_local_surface,
|
||||
k1_local_surface_catalog_item,
|
||||
)
|
||||
from .lidar_local_surface_shadow import (
|
||||
K1_LOCAL_SURFACE_SHADOW_FRAME_SCHEMA,
|
||||
K1_LOCAL_SURFACE_SHADOW_SCHEMA,
|
||||
K1LocalSurfaceShadowEstimator,
|
||||
K1LocalSurfaceShadowInput,
|
||||
K1LocalSurfaceShadowResult,
|
||||
K1LocalSurfaceShadowRuntime,
|
||||
)
|
||||
from .lidar_replay import (
|
||||
LIDAR_EQUIVALENCE_REPORT_SCHEMA,
|
||||
LIDAR_QUALITY_REPORT_SCHEMA,
|
||||
|
|
@ -213,6 +221,8 @@ __all__ = [
|
|||
"K1_LOCAL_SURFACE_REVIEW_SCHEMA",
|
||||
"K1_LOCAL_SURFACE_SCHEMA",
|
||||
"K1_LOCAL_SURFACE_TIMELINE_SCHEMA",
|
||||
"K1_LOCAL_SURFACE_SHADOW_FRAME_SCHEMA",
|
||||
"K1_LOCAL_SURFACE_SHADOW_SCHEMA",
|
||||
"LIDAR_FIELD_REVIEW_REPORT_SCHEMA",
|
||||
"LIDAR_FIELD_REVIEW_SCHEMA",
|
||||
"LIDAR_FIELD_REVIEW_WINDOW_SCHEMA",
|
||||
|
|
@ -236,6 +246,10 @@ __all__ = [
|
|||
"LidarGroundBenchmarkV1",
|
||||
"LidarGroundError",
|
||||
"K1LocalSurfaceProfile",
|
||||
"K1LocalSurfaceShadowEstimator",
|
||||
"K1LocalSurfaceShadowInput",
|
||||
"K1LocalSurfaceShadowResult",
|
||||
"K1LocalSurfaceShadowRuntime",
|
||||
"K1LocalSurfaceV1",
|
||||
"LidarReplayError",
|
||||
"LidarReplayPackV2",
|
||||
|
|
|
|||
|
|
@ -0,0 +1,681 @@
|
|||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
import threading
|
||||
import time
|
||||
from collections import Counter, deque
|
||||
from dataclasses import asdict, dataclass
|
||||
from typing import Any, Final, Literal
|
||||
|
||||
import numpy as np
|
||||
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 (
|
||||
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 .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"
|
||||
|
||||
ShadowFrameState = Literal[
|
||||
"valid",
|
||||
"pose-stale",
|
||||
"insufficient-surface",
|
||||
"fit-failed",
|
||||
]
|
||||
|
||||
|
||||
@dataclass(frozen=True, slots=True)
|
||||
class K1LocalSurfaceShadowInput:
|
||||
"""One immutable map-point/pose pair admitted to passive shadow work."""
|
||||
|
||||
frame_index: int
|
||||
source_frame_index: int
|
||||
session_seconds: float
|
||||
pose_binding_age_ms: float
|
||||
points_map: npt.NDArray[np.float64]
|
||||
position_map: npt.NDArray[np.float64]
|
||||
published_monotonic_ns: int
|
||||
|
||||
def __post_init__(self) -> None:
|
||||
points = np.array(self.points_map, dtype=np.float64, copy=True)
|
||||
position = np.array(self.position_map, dtype=np.float64, copy=True)
|
||||
if (
|
||||
self.frame_index < 0
|
||||
or self.source_frame_index < 0
|
||||
or not math.isfinite(self.session_seconds)
|
||||
or self.session_seconds < 0
|
||||
or not math.isfinite(self.pose_binding_age_ms)
|
||||
or self.pose_binding_age_ms < 0
|
||||
or self.published_monotonic_ns < 0
|
||||
or points.ndim != 2
|
||||
or points.shape[1:] != (3,)
|
||||
or position.shape != (3,)
|
||||
or not np.isfinite(points).all()
|
||||
or not np.isfinite(position).all()
|
||||
):
|
||||
raise LidarGroundError("K1 local-surface shadow input is invalid")
|
||||
points.flags.writeable = False
|
||||
position.flags.writeable = False
|
||||
object.__setattr__(self, "points_map", points)
|
||||
object.__setattr__(self, "position_map", position)
|
||||
|
||||
@classmethod
|
||||
def from_views(
|
||||
cls,
|
||||
point_cloud: DecodedPointCloudView,
|
||||
pose: DecodedPoseView,
|
||||
) -> K1LocalSurfaceShadowInput:
|
||||
"""Bind already-normalized views without interpreting vendor payloads."""
|
||||
|
||||
if (
|
||||
point_cloud.frame_id != "map"
|
||||
or pose.frame_id != "map"
|
||||
or pose.child_frame_id != "sensor"
|
||||
):
|
||||
raise LidarGroundError(
|
||||
"K1 local-surface shadow requires map points and map-from-sensor pose"
|
||||
)
|
||||
point_received_ns = point_cloud.context.received_monotonic_ns
|
||||
pose_received_ns = pose.context.received_monotonic_ns
|
||||
if point_received_ns is not None and pose_received_ns is not None:
|
||||
pose_binding_age_ms = abs(point_received_ns - pose_received_ns) / 1_000_000
|
||||
else:
|
||||
pose_binding_age_ms = (
|
||||
abs(point_cloud.context.captured_at_epoch_ns - pose.context.captured_at_epoch_ns)
|
||||
/ 1_000_000
|
||||
)
|
||||
points = np.asarray(point_cloud.positions_xyz, dtype=np.float64).reshape((-1, 3))
|
||||
position = np.asarray(pose.position_xyz, dtype=np.float64)
|
||||
return cls(
|
||||
frame_index=point_cloud.context.sequence,
|
||||
source_frame_index=point_cloud.context.sequence,
|
||||
session_seconds=point_cloud.context.captured_at_epoch_ns / 1_000_000_000,
|
||||
pose_binding_age_ms=pose_binding_age_ms,
|
||||
points_map=points,
|
||||
position_map=position,
|
||||
published_monotonic_ns=time.monotonic_ns(),
|
||||
)
|
||||
|
||||
|
||||
@dataclass(frozen=True, slots=True)
|
||||
class K1LocalSurfaceShadowResult:
|
||||
"""One bounded diagnostic result; it never describes free or safe space."""
|
||||
|
||||
frame_index: int
|
||||
source_frame_index: int
|
||||
session_seconds: float
|
||||
state: ShadowFrameState
|
||||
pose_binding_age_ms: float
|
||||
surface_cell_count: int
|
||||
surface_inlier_cell_count: int
|
||||
plane_coefficients_map: tuple[float, float, float, float] | None
|
||||
sensor_height_m: float | None
|
||||
slope_deg: float | None
|
||||
roughness_m: float | None
|
||||
confidence: float | None
|
||||
surface_max_age_ms: float | None
|
||||
prediction_available: bool
|
||||
prediction_cell_count: int
|
||||
prediction_residual_p50_m: float | None
|
||||
prediction_residual_p95_m: float | None
|
||||
prediction_inlier_fraction: float | None
|
||||
temporal_compared: bool
|
||||
temporal_jump: bool
|
||||
height_delta_m: float | None
|
||||
slope_delta_deg: float | None
|
||||
roughness_delta_m: float | None
|
||||
surface_point_count: int
|
||||
occupied_point_count: int
|
||||
below_surface_point_count: int
|
||||
step_candidate_point_count: int
|
||||
processing_ms: float
|
||||
result_age_ms: float
|
||||
point_class: npt.NDArray[np.uint8]
|
||||
point_height_m: npt.NDArray[np.float32]
|
||||
point_step_candidate: npt.NDArray[np.uint8]
|
||||
|
||||
@property
|
||||
def valid(self) -> bool:
|
||||
return self.state == "valid"
|
||||
|
||||
def document(self) -> dict[str, object]:
|
||||
return {
|
||||
"schema_version": K1_LOCAL_SURFACE_SHADOW_FRAME_SCHEMA,
|
||||
"frame_index": self.frame_index,
|
||||
"source_frame_index": self.source_frame_index,
|
||||
"session_seconds": self.session_seconds,
|
||||
"state": self.state,
|
||||
"valid": self.valid,
|
||||
"surface": {
|
||||
"pose_binding_age_ms": self.pose_binding_age_ms,
|
||||
"cell_count": self.surface_cell_count,
|
||||
"inlier_cell_count": self.surface_inlier_cell_count,
|
||||
"plane_coefficients_map": (
|
||||
list(self.plane_coefficients_map)
|
||||
if self.plane_coefficients_map is not None
|
||||
else None
|
||||
),
|
||||
"sensor_height_m": self.sensor_height_m,
|
||||
"slope_deg": self.slope_deg,
|
||||
"roughness_m": self.roughness_m,
|
||||
"confidence": self.confidence,
|
||||
"maximum_age_ms": self.surface_max_age_ms,
|
||||
},
|
||||
"prediction": {
|
||||
"available": self.prediction_available,
|
||||
"cell_count": self.prediction_cell_count,
|
||||
"residual_p50_m": self.prediction_residual_p50_m,
|
||||
"residual_p95_m": self.prediction_residual_p95_m,
|
||||
"inlier_fraction": self.prediction_inlier_fraction,
|
||||
"current_frame_excluded": True,
|
||||
},
|
||||
"temporal": {
|
||||
"compared": self.temporal_compared,
|
||||
"jump": self.temporal_jump,
|
||||
"height_delta_m": self.height_delta_m,
|
||||
"slope_delta_deg": self.slope_delta_deg,
|
||||
"roughness_delta_m": self.roughness_delta_m,
|
||||
},
|
||||
"counts": {
|
||||
"surface": self.surface_point_count,
|
||||
"occupied_observed": self.occupied_point_count,
|
||||
"below_surface": self.below_surface_point_count,
|
||||
"step_candidate": self.step_candidate_point_count,
|
||||
},
|
||||
"delivery": {
|
||||
"processing_ms": self.processing_ms,
|
||||
"result_age_ms": self.result_age_ms,
|
||||
},
|
||||
"ground_truth": False,
|
||||
"occupancy_policy": {
|
||||
"absence_of_points_means_free": False,
|
||||
"unknown_is_traversable": False,
|
||||
},
|
||||
"authority": {
|
||||
"commands_enabled": False,
|
||||
"navigation_or_safety_accepted": False,
|
||||
},
|
||||
}
|
||||
|
||||
|
||||
class K1LocalSurfaceShadowEstimator:
|
||||
"""Streaming-equivalent state for the accepted replay surface profile."""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
profile: K1LocalSurfaceProfile = DEFAULT_K1_LOCAL_SURFACE_PROFILE,
|
||||
) -> None:
|
||||
self.profile = profile
|
||||
self._cache: dict[tuple[int, int], tuple[float, float]] = {}
|
||||
self._previous_surface: tuple[float, float, float, float] | None = None
|
||||
self._last_frame_index = -1
|
||||
self._last_session_seconds = -math.inf
|
||||
|
||||
def process(
|
||||
self,
|
||||
value: K1LocalSurfaceShadowInput,
|
||||
) -> K1LocalSurfaceShadowResult:
|
||||
started_ns = time.monotonic_ns()
|
||||
if (
|
||||
value.frame_index <= self._last_frame_index
|
||||
or value.session_seconds < self._last_session_seconds
|
||||
):
|
||||
raise LidarGroundError("K1 local-surface shadow input order is not monotonic")
|
||||
self._last_frame_index = value.frame_index
|
||||
self._last_session_seconds = value.session_seconds
|
||||
if value.pose_binding_age_ms > self.profile.maximum_pose_binding_ms:
|
||||
return self._result(
|
||||
value,
|
||||
started_ns=started_ns,
|
||||
state="pose-stale",
|
||||
)
|
||||
|
||||
cloud = value.points_map
|
||||
position = value.position_map
|
||||
local = np.sum((cloud[:, :2] - position[:2]) ** 2, axis=1) <= (
|
||||
self.profile.local_radius_m**2
|
||||
)
|
||||
local_cloud = cloud[local]
|
||||
_expire_cache(
|
||||
self._cache,
|
||||
value.session_seconds,
|
||||
position,
|
||||
self.profile,
|
||||
)
|
||||
_, prior_cell_points, _ = _local_cache_records(
|
||||
self._cache,
|
||||
position,
|
||||
self.profile,
|
||||
)
|
||||
_, current_cell_points = _cloud_cell_observations(
|
||||
local_cloud,
|
||||
self.profile,
|
||||
)
|
||||
prediction = _prediction_metrics(
|
||||
prior_cell_points,
|
||||
current_cell_points,
|
||||
position,
|
||||
self.profile,
|
||||
)
|
||||
_update_cache(
|
||||
self._cache,
|
||||
local_cloud,
|
||||
value.session_seconds,
|
||||
self.profile,
|
||||
)
|
||||
cell_keys, cell_points, cell_times = _local_cache_records(
|
||||
self._cache,
|
||||
position,
|
||||
self.profile,
|
||||
)
|
||||
prediction_available = prediction is not None
|
||||
prediction_cell_count = prediction.cell_points.shape[0] if prediction is not None else 0
|
||||
prediction_residual_p50_m = prediction.residual_p50_m if prediction is not None else None
|
||||
prediction_residual_p95_m = prediction.residual_p95_m if prediction is not None else None
|
||||
prediction_inlier_fraction = (
|
||||
prediction.inlier_fraction(self.profile.surface_band_m)
|
||||
if prediction is not None
|
||||
else None
|
||||
)
|
||||
if cell_points.shape[0] < self.profile.minimum_surface_cells:
|
||||
return self._result(
|
||||
value,
|
||||
started_ns=started_ns,
|
||||
state="insufficient-surface",
|
||||
surface_cell_count=cell_points.shape[0],
|
||||
prediction_available=prediction_available,
|
||||
prediction_cell_count=prediction_cell_count,
|
||||
prediction_residual_p50_m=prediction_residual_p50_m,
|
||||
prediction_residual_p95_m=prediction_residual_p95_m,
|
||||
prediction_inlier_fraction=prediction_inlier_fraction,
|
||||
)
|
||||
fit = _fit_surface(cell_points, position, self.profile)
|
||||
if fit is None:
|
||||
return self._result(
|
||||
value,
|
||||
started_ns=started_ns,
|
||||
state="fit-failed",
|
||||
surface_cell_count=cell_points.shape[0],
|
||||
prediction_available=prediction_available,
|
||||
prediction_cell_count=prediction_cell_count,
|
||||
prediction_residual_p50_m=prediction_residual_p50_m,
|
||||
prediction_residual_p95_m=prediction_residual_p95_m,
|
||||
prediction_inlier_fraction=prediction_inlier_fraction,
|
||||
)
|
||||
plane, inliers, residuals = fit
|
||||
slope_deg = math.degrees(
|
||||
math.atan2(
|
||||
math.hypot(float(plane[0]), float(plane[1])),
|
||||
float(plane[2]),
|
||||
)
|
||||
)
|
||||
if not np.isfinite(slope_deg) or slope_deg > self.profile.maximum_slope_deg:
|
||||
return self._result(
|
||||
value,
|
||||
started_ns=started_ns,
|
||||
state="fit-failed",
|
||||
surface_cell_count=cell_points.shape[0],
|
||||
prediction_available=prediction_available,
|
||||
prediction_cell_count=prediction_cell_count,
|
||||
prediction_residual_p50_m=prediction_residual_p50_m,
|
||||
prediction_residual_p95_m=prediction_residual_p95_m,
|
||||
prediction_inlier_fraction=prediction_inlier_fraction,
|
||||
)
|
||||
|
||||
heights = _height_above_plane(cloud, plane)
|
||||
point_class = np.zeros(cloud.shape[0], dtype=np.uint8)
|
||||
point_class[local & (np.abs(heights) <= self.profile.surface_band_m)] = POINT_SURFACE
|
||||
point_class[
|
||||
local
|
||||
& (heights >= self.profile.obstacle_min_height_m)
|
||||
& (heights <= self.profile.obstacle_max_height_m)
|
||||
] = POINT_OCCUPIED
|
||||
point_class[local & (heights < -self.profile.surface_band_m)] = POINT_BELOW_SURFACE
|
||||
step_keys = _step_candidate_keys(
|
||||
cell_keys,
|
||||
cell_points,
|
||||
plane,
|
||||
self.profile,
|
||||
)
|
||||
point_step_candidate = _point_step_candidates(
|
||||
cloud,
|
||||
local,
|
||||
heights,
|
||||
step_keys,
|
||||
self.profile,
|
||||
)
|
||||
point_height_m = np.zeros(cloud.shape[0], dtype=np.float32)
|
||||
point_height_m[local] = heights[local].astype(np.float32)
|
||||
|
||||
sensor_height = float(_height_above_plane(position.reshape(1, 3), plane)[0])
|
||||
roughness = float(np.median(np.abs(residuals[inliers])))
|
||||
inlier_count = int(np.count_nonzero(inliers))
|
||||
coverage = min(
|
||||
1.0,
|
||||
inlier_count / (self.profile.minimum_surface_cells * 3),
|
||||
)
|
||||
roughness_confidence = math.exp(-roughness / max(self.profile.surface_band_m, 1e-6))
|
||||
pose_confidence = max(
|
||||
0.0,
|
||||
1.0 - value.pose_binding_age_ms / self.profile.maximum_pose_binding_ms,
|
||||
)
|
||||
confidence = float(
|
||||
np.clip(
|
||||
coverage * roughness_confidence * pose_confidence,
|
||||
0.0,
|
||||
1.0,
|
||||
)
|
||||
)
|
||||
temporal_compared = False
|
||||
temporal_jump = False
|
||||
height_delta_m: float | None = None
|
||||
slope_delta_deg: float | None = None
|
||||
roughness_delta_m: float | None = None
|
||||
if (
|
||||
self._previous_surface is not None
|
||||
and value.session_seconds - self._previous_surface[0] <= self.profile.surface_ttl_s
|
||||
):
|
||||
temporal_compared = True
|
||||
height_delta_m = abs(sensor_height - self._previous_surface[1])
|
||||
slope_delta_deg = abs(slope_deg - self._previous_surface[2])
|
||||
roughness_delta_m = abs(roughness - self._previous_surface[3])
|
||||
temporal_jump = (
|
||||
height_delta_m > self.profile.temporal_height_jump_m
|
||||
or slope_delta_deg > self.profile.temporal_slope_jump_deg
|
||||
or roughness_delta_m > self.profile.temporal_roughness_jump_m
|
||||
)
|
||||
self._previous_surface = (
|
||||
value.session_seconds,
|
||||
sensor_height,
|
||||
slope_deg,
|
||||
roughness,
|
||||
)
|
||||
point_class.flags.writeable = False
|
||||
point_height_m.flags.writeable = False
|
||||
point_step_candidate.flags.writeable = False
|
||||
return self._result(
|
||||
value,
|
||||
started_ns=started_ns,
|
||||
state="valid",
|
||||
surface_cell_count=cell_points.shape[0],
|
||||
surface_inlier_cell_count=inlier_count,
|
||||
plane_coefficients_map=(
|
||||
float(plane[0]),
|
||||
float(plane[1]),
|
||||
float(plane[2]),
|
||||
float(plane[3]),
|
||||
),
|
||||
sensor_height_m=sensor_height,
|
||||
slope_deg=slope_deg,
|
||||
roughness_m=roughness,
|
||||
confidence=confidence,
|
||||
surface_max_age_ms=max(
|
||||
0.0,
|
||||
(value.session_seconds - float(np.min(cell_times[inliers]))) * 1_000.0,
|
||||
),
|
||||
temporal_compared=temporal_compared,
|
||||
temporal_jump=temporal_jump,
|
||||
height_delta_m=height_delta_m,
|
||||
slope_delta_deg=slope_delta_deg,
|
||||
roughness_delta_m=roughness_delta_m,
|
||||
surface_point_count=int(np.count_nonzero(point_class == POINT_SURFACE)),
|
||||
occupied_point_count=int(np.count_nonzero(point_class == POINT_OCCUPIED)),
|
||||
below_surface_point_count=int(np.count_nonzero(point_class == POINT_BELOW_SURFACE)),
|
||||
step_candidate_point_count=int(np.count_nonzero(point_step_candidate)),
|
||||
point_class=point_class,
|
||||
point_height_m=point_height_m,
|
||||
point_step_candidate=point_step_candidate,
|
||||
prediction_available=prediction_available,
|
||||
prediction_cell_count=prediction_cell_count,
|
||||
prediction_residual_p50_m=prediction_residual_p50_m,
|
||||
prediction_residual_p95_m=prediction_residual_p95_m,
|
||||
prediction_inlier_fraction=prediction_inlier_fraction,
|
||||
)
|
||||
|
||||
def _result(
|
||||
self,
|
||||
value: K1LocalSurfaceShadowInput,
|
||||
*,
|
||||
started_ns: int,
|
||||
state: ShadowFrameState,
|
||||
surface_cell_count: int = 0,
|
||||
surface_inlier_cell_count: int = 0,
|
||||
plane_coefficients_map: tuple[float, float, float, float] | None = None,
|
||||
sensor_height_m: float | None = None,
|
||||
slope_deg: float | None = None,
|
||||
roughness_m: float | None = None,
|
||||
confidence: float | None = None,
|
||||
surface_max_age_ms: float | None = None,
|
||||
prediction_available: bool = False,
|
||||
prediction_cell_count: int = 0,
|
||||
prediction_residual_p50_m: float | None = None,
|
||||
prediction_residual_p95_m: float | None = None,
|
||||
prediction_inlier_fraction: float | None = None,
|
||||
temporal_compared: bool = False,
|
||||
temporal_jump: bool = False,
|
||||
height_delta_m: float | None = None,
|
||||
slope_delta_deg: float | None = None,
|
||||
roughness_delta_m: float | None = None,
|
||||
surface_point_count: int = 0,
|
||||
occupied_point_count: int = 0,
|
||||
below_surface_point_count: int = 0,
|
||||
step_candidate_point_count: int = 0,
|
||||
point_class: npt.NDArray[np.uint8] | None = None,
|
||||
point_height_m: npt.NDArray[np.float32] | None = None,
|
||||
point_step_candidate: npt.NDArray[np.uint8] | None = None,
|
||||
) -> K1LocalSurfaceShadowResult:
|
||||
finished_ns = time.monotonic_ns()
|
||||
if point_class is None:
|
||||
point_class = np.zeros(value.points_map.shape[0], dtype=np.uint8)
|
||||
point_class.flags.writeable = False
|
||||
if point_height_m is None:
|
||||
point_height_m = np.zeros(value.points_map.shape[0], dtype=np.float32)
|
||||
point_height_m.flags.writeable = False
|
||||
if point_step_candidate is None:
|
||||
point_step_candidate = np.zeros(
|
||||
value.points_map.shape[0],
|
||||
dtype=np.uint8,
|
||||
)
|
||||
point_step_candidate.flags.writeable = False
|
||||
return K1LocalSurfaceShadowResult(
|
||||
frame_index=value.frame_index,
|
||||
source_frame_index=value.source_frame_index,
|
||||
session_seconds=value.session_seconds,
|
||||
state=state,
|
||||
pose_binding_age_ms=value.pose_binding_age_ms,
|
||||
surface_cell_count=surface_cell_count,
|
||||
surface_inlier_cell_count=surface_inlier_cell_count,
|
||||
plane_coefficients_map=plane_coefficients_map,
|
||||
sensor_height_m=sensor_height_m,
|
||||
slope_deg=slope_deg,
|
||||
roughness_m=roughness_m,
|
||||
confidence=confidence,
|
||||
surface_max_age_ms=surface_max_age_ms,
|
||||
prediction_available=prediction_available,
|
||||
prediction_cell_count=prediction_cell_count,
|
||||
prediction_residual_p50_m=prediction_residual_p50_m,
|
||||
prediction_residual_p95_m=prediction_residual_p95_m,
|
||||
prediction_inlier_fraction=prediction_inlier_fraction,
|
||||
temporal_compared=temporal_compared,
|
||||
temporal_jump=temporal_jump,
|
||||
height_delta_m=height_delta_m,
|
||||
slope_delta_deg=slope_delta_deg,
|
||||
roughness_delta_m=roughness_delta_m,
|
||||
surface_point_count=surface_point_count,
|
||||
occupied_point_count=occupied_point_count,
|
||||
below_surface_point_count=below_surface_point_count,
|
||||
step_candidate_point_count=step_candidate_point_count,
|
||||
processing_ms=(finished_ns - started_ns) / 1_000_000,
|
||||
result_age_ms=max(
|
||||
0.0,
|
||||
(finished_ns - value.published_monotonic_ns) / 1_000_000,
|
||||
),
|
||||
point_class=point_class,
|
||||
point_height_m=point_height_m,
|
||||
point_step_candidate=point_step_candidate,
|
||||
)
|
||||
|
||||
|
||||
class K1LocalSurfaceShadowRuntime:
|
||||
"""One bounded latest-wins worker with a bounded diagnostic result ring."""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
session_id: str,
|
||||
*,
|
||||
profile: K1LocalSurfaceProfile = DEFAULT_K1_LOCAL_SURFACE_PROFILE,
|
||||
queue_capacity: int = 2,
|
||||
result_capacity: int = 8,
|
||||
) -> None:
|
||||
if (
|
||||
not session_id
|
||||
or len(session_id) > 160
|
||||
or not 1 <= queue_capacity <= 8
|
||||
or not 1 <= result_capacity <= 256
|
||||
):
|
||||
raise LidarGroundError("K1 local-surface shadow runtime bounds are invalid")
|
||||
self.session_id = session_id
|
||||
self.profile = profile
|
||||
self._queue = LatestWinsQueue[K1LocalSurfaceShadowInput](queue_capacity)
|
||||
self._result_capacity = result_capacity
|
||||
self._results: deque[K1LocalSurfaceShadowResult] = deque(maxlen=result_capacity)
|
||||
self._result_dropped = 0
|
||||
self._processed = 0
|
||||
self._failed = 0
|
||||
self._state_counts: Counter[str] = Counter()
|
||||
self._last_error: str | None = None
|
||||
self._inflight = False
|
||||
self._condition = threading.Condition()
|
||||
self._closed = False
|
||||
self._estimator = K1LocalSurfaceShadowEstimator(profile)
|
||||
self._thread = threading.Thread(
|
||||
target=self._run,
|
||||
name=f"k1-local-surface-shadow-{session_id}",
|
||||
daemon=True,
|
||||
)
|
||||
self._thread.start()
|
||||
|
||||
def publish(self, value: K1LocalSurfaceShadowInput) -> None:
|
||||
with self._condition:
|
||||
if self._closed:
|
||||
raise RuntimeError("K1 local-surface shadow runtime is closed")
|
||||
self._queue.publish(value)
|
||||
|
||||
def publish_views(
|
||||
self,
|
||||
point_cloud: DecodedPointCloudView,
|
||||
pose: DecodedPoseView,
|
||||
) -> None:
|
||||
self.publish(K1LocalSurfaceShadowInput.from_views(point_cloud, pose))
|
||||
|
||||
def wait_until_idle(self, timeout_seconds: float) -> bool:
|
||||
if timeout_seconds < 0:
|
||||
raise ValueError("K1 local-surface shadow wait timeout is invalid")
|
||||
deadline = time.monotonic() + timeout_seconds
|
||||
while True:
|
||||
queue_snapshot = self._queue.snapshot()
|
||||
with self._condition:
|
||||
accounted = (
|
||||
queue_snapshot.consumed + queue_snapshot.dropped_overflow
|
||||
== queue_snapshot.published
|
||||
)
|
||||
if accounted and queue_snapshot.depth == 0 and not self._inflight:
|
||||
return True
|
||||
remaining = deadline - time.monotonic()
|
||||
if remaining <= 0:
|
||||
return False
|
||||
self._condition.wait(timeout=remaining)
|
||||
|
||||
def close(self, *, timeout_seconds: float = 30.0) -> None:
|
||||
if timeout_seconds <= 0:
|
||||
raise ValueError("K1 local-surface shadow close timeout is invalid")
|
||||
with self._condition:
|
||||
already_closed = self._closed
|
||||
self._closed = True
|
||||
if not already_closed:
|
||||
self._queue.close()
|
||||
self._thread.join(timeout=timeout_seconds)
|
||||
if self._thread.is_alive():
|
||||
raise RuntimeError("K1 local-surface shadow worker did not stop")
|
||||
|
||||
def results(self) -> tuple[K1LocalSurfaceShadowResult, ...]:
|
||||
with self._condition:
|
||||
return tuple(self._results)
|
||||
|
||||
def snapshot(self) -> dict[str, Any]:
|
||||
queue_snapshot = self._queue.snapshot()
|
||||
with self._condition:
|
||||
latest = self._results[-1] if self._results else None
|
||||
return {
|
||||
"schema_version": K1_LOCAL_SURFACE_SHADOW_SCHEMA,
|
||||
"mode": "live-shadow-diagnostic-only",
|
||||
"session_id": self.session_id,
|
||||
"profile": self.profile.to_dict(),
|
||||
"queue_policy": "bounded-latest-wins",
|
||||
"queue": asdict(queue_snapshot),
|
||||
"results": {
|
||||
"capacity": self._result_capacity,
|
||||
"depth": len(self._results),
|
||||
"published": self._processed,
|
||||
"dropped_ring_overflow": self._result_dropped,
|
||||
"state_counts": dict(sorted(self._state_counts.items())),
|
||||
"failed": self._failed,
|
||||
"latest": latest.document() if latest is not None else None,
|
||||
},
|
||||
"last_error": self._last_error,
|
||||
"closed": self._closed and not self._thread.is_alive(),
|
||||
"ground_truth": False,
|
||||
"occupancy_policy": {
|
||||
"absence_of_points_means_free": False,
|
||||
"unknown_is_traversable": False,
|
||||
},
|
||||
"authority": {
|
||||
"commands_enabled": False,
|
||||
"navigation_or_safety_accepted": False,
|
||||
},
|
||||
}
|
||||
|
||||
def _run(self) -> None:
|
||||
while True:
|
||||
value = self._queue.take_next()
|
||||
if value is None:
|
||||
with self._condition:
|
||||
self._condition.notify_all()
|
||||
return
|
||||
with self._condition:
|
||||
self._inflight = True
|
||||
try:
|
||||
result = self._estimator.process(value)
|
||||
except (LidarGroundError, ValueError, np.linalg.LinAlgError) as exc:
|
||||
with self._condition:
|
||||
self._failed += 1
|
||||
self._last_error = type(exc).__name__
|
||||
else:
|
||||
with self._condition:
|
||||
if len(self._results) == self._result_capacity:
|
||||
self._result_dropped += 1
|
||||
self._results.append(result)
|
||||
self._processed += 1
|
||||
self._state_counts[result.state] += 1
|
||||
finally:
|
||||
with self._condition:
|
||||
self._inflight = False
|
||||
self._condition.notify_all()
|
||||
|
|
@ -2,9 +2,11 @@ from __future__ import annotations
|
|||
|
||||
import hashlib
|
||||
import json
|
||||
import time
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
from fastapi import APIRouter
|
||||
from fastapi.routing import APIRoute
|
||||
|
||||
|
|
@ -12,6 +14,8 @@ from k1link.compute import (
|
|||
E10_LIDAR_PACK_SCHEMA,
|
||||
E10LidarFieldSource,
|
||||
K1LocalSurfaceProfile,
|
||||
K1LocalSurfaceShadowInput,
|
||||
K1LocalSurfaceShadowRuntime,
|
||||
K1LocalSurfaceV1,
|
||||
build_k1_local_surface,
|
||||
)
|
||||
|
|
@ -51,9 +55,7 @@ def _source_pack(root: Path) -> Path:
|
|||
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_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)
|
||||
|
|
@ -188,8 +190,9 @@ def test_k1_local_surface_is_dynamic_source_bound_and_read_only(
|
|||
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 (
|
||||
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
|
||||
|
|
@ -239,3 +242,165 @@ def test_k1_local_surface_is_dynamic_source_bound_and_read_only(
|
|||
assert review["review_profile_id"] == "missioncore-local-surface-attention/v1"
|
||||
assert review["access"] == "read-only"
|
||||
assert str(tmp_path) not in repr(review)
|
||||
|
||||
|
||||
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["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()
|
||||
|
|
|
|||
Loading…
Reference in New Issue