SILVER/NODEDC_MISSION_CORE
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

feat(perception): qualify lossless lidar observations

Browse Source
This commit is contained in:
DCCONSTRUCTIONS
2026-07-30 11:37:57 +03:00
parent e56fa0c074
commit 7d1a70d8e0
13 changed files with 1874 additions and 119 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/k1link/compute/__init__.py
+22
View File
@@ -40,6 +40,18 @@ from .e33_worker_shadow import (
read_e33_worker_shadow_result,
run_e33_worker_shadow,
)
from .e51_motion_semantic_qualification import (
E51_FRAME_SCHEMA,
E51_PROFILE_SCHEMA,
E51_REPORT_SCHEMA,
E51_RESULT_SCHEMA,
E51_SIGNAL_SCHEMA,
E51MotionSemanticError,
E51MotionSemanticResult,
build_e51_motion_semantic_qualification,
derive_motion_semantic_signal,
read_e51_motion_semantic_qualification,
)
from .evaluation_pack import (
ANNOTATION_CONTRACT_SCHEMA,
EVALUATION_PACK_SCHEMA,
@@ -397,6 +409,16 @@ __all__ = [
"E33WorkerShadowResult",
"read_e33_worker_shadow_result",
"run_e33_worker_shadow",
"E51_FRAME_SCHEMA",
"E51_PROFILE_SCHEMA",
"E51_REPORT_SCHEMA",
"E51_RESULT_SCHEMA",
"E51_SIGNAL_SCHEMA",
"E51MotionSemanticError",
"E51MotionSemanticResult",
"build_e51_motion_semantic_qualification",
"derive_motion_semantic_signal",
"read_e51_motion_semantic_qualification",
"build_lidar_ground_annotation_template",
"build_lidar_ground_benchmark",
"build_k1_local_surface",
src/k1link/compute/e51_motion_semantic_qualification.py
+940
View File
@@ -0,0 +1,940 @@
"""Immutable E51 qualification of motion, proximity and semantic evidence."""
from __future__ import annotations
import hashlib
import json
import math
import os
import re
import resource
import shutil
import sys
import time
from dataclasses import dataclass
from datetime import UTC, datetime
from itertools import zip_longest
from pathlib import Path
from typing import Any, Final, TextIO, cast
import numpy as np
from .e32_track_geometry_replay import read_e32_track_geometry_replay
from .e34_temporal_occupied_replay import (
E34TemporalOccupiedReplay,
read_e34_temporal_occupied_replay,
)
from .lidar_field_review import E10LidarFieldSource
E51_PROFILE_SCHEMA: Final = "missioncore.e51-motion-semantic-profile/v1"
E51_RESULT_SCHEMA: Final = "missioncore.e51-motion-semantic-result/v1"
E51_FRAME_SCHEMA: Final = "missioncore.e51-motion-semantic-frame/v1"
E51_SIGNAL_SCHEMA: Final = "missioncore.e51-motion-semantic-signal/v1"
E51_REPORT_SCHEMA: Final = "missioncore.e51-motion-semantic-report/v1"
E51_FRAMES_NAME: Final = "motion-semantic-frames.jsonl"
E51_REPORT_NAME: Final = "run-report.json"
E51_MANIFEST_NAME: Final = "manifest.json"
_RESULT_ID = re.compile(r"^e51-motion-semantic-[a-f0-9]{64}$")
_SHA256 = re.compile(r"^[a-f0-9]{64}$")
class E51MotionSemanticError(RuntimeError):
"""An E51 profile, source, replay or immutable result is invalid."""
@dataclass(frozen=True, slots=True)
class E51MotionSemanticResult:
"""One validated immutable E51 diagnostic result."""
result_root: Path
result_id: str
manifest: dict[str, Any]
report: dict[str, Any]
@property
def accepted(self) -> bool:
return bool(_object(self.report.get("acceptance"), "E51 acceptance")["accepted"])
@dataclass(frozen=True, slots=True)
class _Profile:
raw: dict[str, Any]
expected_e32_result_id: str
expected_e34_result_id: str
expected_source_pack_id: str
minimum_motion_observations: int
minimum_motion_span_seconds: float
minimum_motion_displacement_m: float
minimum_motion_speed_mps: float
maximum_motion_speed_mps: float
proximity_threshold_m: float
maximum_signals_per_frame: int
maximum_latency_p95_ms: float
maximum_rss_growth_mib: float
maximum_lidar_camera_age_p95_ms: float
maximum_pose_age_p95_ms: float
def build_e51_motion_semantic_qualification(
*,
e32_result_root: Path,
e34_result_root: Path,
e10_source_root: Path,
profile_path: Path,
output_root: Path,
) -> E51MotionSemanticResult:
"""Build or verify the bounded E51 diagnostic derivative."""
profile = _read_profile(profile_path)
e32 = read_e32_track_geometry_replay(e32_result_root)
e34 = read_e34_temporal_occupied_replay(e34_result_root)
source = E10LidarFieldSource(e10_source_root)
try:
_validate_bindings(profile=profile, e32=e32, e34=e34, source=source)
e32_artifacts = _verified_artifacts(
e32.result_root,
e32.manifest.get("artifacts"),
key="role",
)
e34_artifacts = _verified_artifacts(
e34.result_root,
e34.manifest.get("artifacts"),
key="kind",
)
source_artifact = _object(
source.manifest.get("artifact"),
"E51 E10 source artifact",
)
upstream_before = {
"e32": _artifact_identity(e32_artifacts),
"e34": _artifact_identity(e34_artifacts),
"e10": {
"lidar-pack": {
"byte_length": source_artifact["byte_length"],
"sha256": source_artifact["sha256"],
}
},
}
identity = {
"schema_version": E51_RESULT_SCHEMA,
"profile": profile.raw,
"profile_sha256": _sha256(profile_path.resolve(strict=True)),
"source_session_id": source.identity["session_id"],
"frame_count": e32.manifest["identity"]["frame_count"],
"e32_result_id": e32.result_id,
"e32_identity_sha256": e32.manifest["identity_sha256"],
"e34_result_id": e34.result_id,
"e34_identity_sha256": e34.manifest["identity_sha256"],
"source_pack_id": source.pack_id,
"source_pack_identity_sha256": source.manifest["identity_sha256"],
"upstream_artifacts": upstream_before,
"producer_sha256": _sha256(Path(__file__).resolve(strict=True)),
"policy": {
"dynamic_class_available": False,
"collision_state_available": False,
"free_space_available": False,
"absence_of_points_means_free": False,
"persistent_reconstruction_mutated": False,
},
"authority": _authority(),
}
identity_sha256 = hashlib.sha256(_canonical_json(identity)).hexdigest()
result_id = f"e51-motion-semantic-{identity_sha256}"
destination = output_root.expanduser().absolute()
destination.mkdir(mode=0o700, parents=True, exist_ok=True)
result_root = destination / result_id
if result_root.exists():
return read_e51_motion_semantic_qualification(result_root)
staging = destination / f".{result_id}.{os.getpid()}.incomplete"
staging.mkdir(mode=0o700, exist_ok=False)
try:
report = _run_qualification(
staging=staging,
result_id=result_id,
e32_frames=e32_artifacts["track-geometry-frames"],
e34=e34,
e34_frames=e34_artifacts["temporal-occupied-frames"],
source=source,
profile=profile,
)
upstream_after = {
"e32": _artifact_identity(
_verified_artifacts(
e32.result_root,
e32.manifest.get("artifacts"),
key="role",
)
),
"e34": _artifact_identity(
_verified_artifacts(
e34.result_root,
e34.manifest.get("artifacts"),
key="kind",
)
),
"e10": {
"lidar-pack": {
"byte_length": source_artifact["byte_length"],
"sha256": _sha256(source.root / str(source_artifact["path"])),
}
},
}
report["acceptance"]["upstream_unchanged"] = (
upstream_after == upstream_before
)
checks = cast(dict[str, bool], report["acceptance"]["checks"])
checks["upstream_unchanged"] = upstream_after == upstream_before
report["acceptance"]["accepted"] = all(checks.values())
_write_json(staging / E51_REPORT_NAME, report)
artifacts = [
_artifact(staging / E51_FRAMES_NAME, "motion-semantic-frames"),
_artifact(staging / E51_REPORT_NAME, "run-report"),
]
manifest = {
"schema_version": E51_RESULT_SCHEMA,
"result_id": result_id,
"identity_sha256": identity_sha256,
"identity": identity,
"created_at_utc": datetime.now(UTC)
.isoformat(timespec="milliseconds")
.replace("+00:00", "Z"),
"classification": "private-diagnostic-derivative",
"ground_truth": False,
"artifacts": artifacts,
}
_write_json(staging / E51_MANIFEST_NAME, manifest)
os.replace(staging, result_root)
except BaseException:
shutil.rmtree(staging, ignore_errors=True)
raise
return read_e51_motion_semantic_qualification(result_root)
finally:
source.close()
def read_e51_motion_semantic_qualification(
result_root: Path,
) -> E51MotionSemanticResult:
"""Read and verify one immutable E51 result."""
root = result_root.expanduser().absolute().resolve(strict=True)
if not root.is_dir() or _RESULT_ID.fullmatch(root.name) is None:
raise E51MotionSemanticError("E51 result id is invalid")
manifest = _read_json(root / E51_MANIFEST_NAME)
identity = _object(manifest.get("identity"), "E51 identity")
identity_sha256 = manifest.get("identity_sha256")
if (
manifest.get("schema_version") != E51_RESULT_SCHEMA
or identity.get("schema_version") != E51_RESULT_SCHEMA
or not isinstance(identity_sha256, str)
or _SHA256.fullmatch(identity_sha256) is None
or hashlib.sha256(_canonical_json(identity)).hexdigest() != identity_sha256
or root.name != f"e51-motion-semantic-{identity_sha256}"
or manifest.get("result_id") != root.name
):
raise E51MotionSemanticError("E51 result identity is invalid")
artifacts = _verified_artifacts(root, manifest.get("artifacts"), key="kind")
required = {"motion-semantic-frames", "run-report"}
if set(artifacts) != required:
raise E51MotionSemanticError("E51 artifact set is invalid")
report = _read_json(artifacts["run-report"])
acceptance = _object(report.get("acceptance"), "E51 acceptance")
checks = _object(acceptance.get("checks"), "E51 acceptance checks")
if (
report.get("schema_version") != E51_REPORT_SCHEMA
or report.get("result_id") != root.name
or acceptance.get("accepted") is not all(value is True for value in checks.values())
or report.get("authority") != _authority()
):
raise E51MotionSemanticError("E51 report is invalid")
return E51MotionSemanticResult(
result_root=root,
result_id=root.name,
manifest=manifest,
report=report,
)
def derive_motion_semantic_signal(
component: dict[str, Any],
geometry: dict[str, Any] | None,
*,
map_frame_jump_candidate: bool,
profile: dict[str, float | int],
) -> dict[str, Any] | None:
"""Derive one conservative diagnostic signal from existing evidence."""
state = component.get("state")
if state not in {"current", "held"}:
raise E51MotionSemanticError("E51 component freshness is invalid")
history = _list(component.get("history_tail"), "E51 component history")
motion = _motion_metrics(
history,
map_frame_jump_candidate=map_frame_jump_candidate,
minimum_observations=int(profile["minimum_motion_observations"]),
minimum_span_seconds=float(profile["minimum_motion_span_seconds"]),
minimum_displacement_m=float(profile["minimum_motion_displacement_m"]),
minimum_speed_mps=float(profile["minimum_motion_speed_mps"]),
maximum_speed_mps=float(profile["maximum_motion_speed_mps"]),
)
evidence_state = (
str(geometry.get("evidence_state"))
if geometry is not None
else "held-temporal-evidence"
)
reason_codes = (
[
str(value)
for value in _list(
geometry.get("reason_codes"),
"E51 geometry reason codes",
)
]
if geometry is not None
else []
)
conflict = evidence_state == "conflict" or any(
"conflict" in value or "collision" in value for value in reason_codes
)
semantic = geometry.get("semantic") if geometry is not None else None
if semantic is None:
provenance = _object(
component.get("semantic_provenance"),
"E51 semantic provenance",
)
labels = _list(provenance.get("labels"), "E51 semantic labels")
track_ids = _list(provenance.get("track_ids"), "E51 semantic track ids")
if labels or track_ids or provenance.get("owner") is not None:
semantic = {
"owner": provenance.get("owner"),
"labels": labels,
"track_ids": track_ids,
"source": "e34-held-semantic-provenance",
}
range_m = geometry.get("range_m") if geometry is not None else None
range_value = (
float(range_m)
if isinstance(range_m, int | float) and math.isfinite(float(range_m))
else None
)
proximity_candidate = (
state == "current"
and range_value is not None
and range_value <= float(profile["proximity_threshold_m"])
)
if (
not motion["candidate"]
and not proximity_candidate
and semantic is None
and not conflict
):
return None
return {
"schema_version": E51_SIGNAL_SCHEMA,
"temporal_id": component["temporal_id"],
"source_owner_key": component["source_owner_key"],
"owner_kind": component["owner_kind"],
"freshness": {
"state": state,
"age_seconds": component["last_observed_age_seconds"],
"current_hit_backed": state == "current",
},
"semantic": {
"available": semantic is not None,
"value": semantic,
"confidence": {
"available": False,
"value": None,
"reason": "upstream-contract-has-no-numeric-confidence",
},
},
"evidence": {
"state": evidence_state,
"conflict": conflict,
"reason_codes": reason_codes,
},
"motion": motion,
"proximity": {
"candidate": proximity_candidate,
"range_m": range_value,
"threshold_m": float(profile["proximity_threshold_m"]),
"classification": "diagnostic-near-occupied-candidate",
},
"collision": {
"state": "unavailable",
"reason": "vehicle-body-and-lidar-mount-geometry-not-bound",
},
"authority": _authority(),
}
def _run_qualification(
*,
staging: Path,
result_id: str,
e32_frames: Path,
e34: E34TemporalOccupiedReplay,
e34_frames: Path,
source: E10LidarFieldSource,
profile: _Profile,
) -> dict[str, Any]:
started = time.perf_counter()
rss_start = _process_peak_rss_mib()
frame_latencies_ms: list[float] = []
signal_counts = {
"total": 0,
"motion_candidates": 0,
"proximity_candidates": 0,
"semantic_available": 0,
"conflicts": 0,
"current": 0,
"held": 0,
}
frame_count = 0
accepted_current_point_rows = 0
map_frame_jump_candidates = 0
maximum_signals_observed = 0
frames_path = staging / E51_FRAMES_NAME
with (
e32_frames.open("r", encoding="utf-8") as e32_stream,
e34_frames.open("r", encoding="utf-8") as e34_stream,
frames_path.open("x", encoding="utf-8") as output,
):
for e32_line, e34_line in zip_longest(e32_stream, e34_stream):
frame_started = time.perf_counter()
if e32_line is None or e34_line is None:
raise E51MotionSemanticError("E51 upstream frame counts differ")
e32_frame = _parse_json_line(e32_line, "E51 E32 frame")
e34_frame = _parse_json_line(e34_line, "E51 E34 frame")
_validate_frame_pair(e32_frame, e34_frame, frame_count)
geometries = {
str(geometry["owner_key"]): geometry
for geometry in _object_list(
e32_frame.get("geometries"),
"E51 E32 geometries",
)
}
jump = _object(
e34_frame.get("map_frame_jump"),
"E51 map-frame jump",
)
jump_candidate = jump.get("candidate") is True
map_frame_jump_candidates += int(jump_candidate)
signals: list[dict[str, Any]] = []
for component in [
*_object_list(e34_frame.get("current"), "E51 current components"),
*_object_list(e34_frame.get("held"), "E51 held components"),
]:
owner_key = str(component.get("source_owner_key"))
signal = derive_motion_semantic_signal(
component,
geometries.get(owner_key),
map_frame_jump_candidate=jump_candidate,
profile={
"minimum_motion_observations": profile.minimum_motion_observations,
"minimum_motion_span_seconds": profile.minimum_motion_span_seconds,
"minimum_motion_displacement_m": profile.minimum_motion_displacement_m,
"minimum_motion_speed_mps": profile.minimum_motion_speed_mps,
"maximum_motion_speed_mps": profile.maximum_motion_speed_mps,
"proximity_threshold_m": profile.proximity_threshold_m,
},
)
if signal is not None:
signals.append(signal)
maximum_signals_observed = max(maximum_signals_observed, len(signals))
if len(signals) > profile.maximum_signals_per_frame:
raise E51MotionSemanticError(
"E51 signal count exceeds the bounded profile"
)
input_summary = _object(e34_frame.get("input"), "E51 E34 input")
current_point_rows = _nonnegative_int(
input_summary.get("accepted_current_point_rows"),
"E51 accepted current point rows",
)
accepted_current_point_rows += current_point_rows
for signal in signals:
_count_signal(signal_counts, signal)
record = {
"schema_version": E51_FRAME_SCHEMA,
"frame_index": frame_count,
"source_frame_index": e34_frame["source_frame_index"],
"session_seconds": e34_frame["session_seconds"],
"source_available": e34_frame["source_available"],
"layer_state": e34_frame["layer_state"],
"accepted_current_point_rows": current_point_rows,
"signal_count": len(signals),
"signals": signals,
"policy": {
"dynamic_class_available": False,
"collision_state_available": False,
"free_space_available": False,
"absence_of_points_means_free": False,
"persistent_reconstruction_mutated": False,
},
"authority": _authority(),
}
_write_json_line(output, record)
frame_count += 1
frame_latencies_ms.append(
(time.perf_counter() - frame_started) * 1_000.0
)
rss_end = _process_peak_rss_mib()
lidar_age = _finite_abs(source.arrays["lidar_camera_delta_ms"])
pose_age = _finite_abs(source.arrays["pose_point_delta_ms"])
latency = _distribution(np.asarray(frame_latencies_ms, dtype=np.float64))
lidar_age_report = _distribution(lidar_age)
pose_age_report = _distribution(pose_age)
e34_occupancy = _object(
_object(e34.report.get("metrics"), "E51 E34 metrics").get("occupancy"),
"E51 E34 occupancy metrics",
)
expected_point_rows = _nonnegative_int(
e34_occupancy.get("e34_consumed_current_point_rows"),
"E51 E34 consumed point rows",
)
rss_growth = max(0.0, rss_end - rss_start)
checks = {
"complete_frame_accounting": frame_count
== _nonnegative_int(
e34.manifest["identity"].get("frame_count"),
"E51 E34 frame count",
),
"map_frame_jump_candidates_zero": map_frame_jump_candidates == 0,
"current_obstacle_rows_preserved": (
accepted_current_point_rows == expected_point_rows
),
"latency_p95_within_gate": _required_float(latency["p95"])
<= profile.maximum_latency_p95_ms,
"rss_growth_within_gate": rss_growth <= profile.maximum_rss_growth_mib,
"lidar_camera_age_p95_within_gate": _required_float(
lidar_age_report["p95"]
)
<= profile.maximum_lidar_camera_age_p95_ms,
"pose_age_p95_within_gate": _required_float(pose_age_report["p95"])
<= profile.maximum_pose_age_p95_ms,
"bounded_signal_state": maximum_signals_observed
<= profile.maximum_signals_per_frame,
"free_space_not_published": True,
"dynamic_class_not_invented": True,
"collision_state_not_invented": True,
"upstream_unchanged": False,
}
return {
"schema_version": E51_REPORT_SCHEMA,
"result_id": result_id,
"status": "diagnostic-only",
"ground_truth": False,
"metrics": {
"frames": {
"processed": frame_count,
"map_frame_jump_candidates": map_frame_jump_candidates,
},
"signals": {
**signal_counts,
"maximum_per_frame": maximum_signals_observed,
},
"obstacle_preservation": {
"e34_consumed_current_point_rows": expected_point_rows,
"e51_observed_current_point_rows": accepted_current_point_rows,
"exact": accepted_current_point_rows == expected_point_rows,
"persistent_reconstruction_mutated": False,
},
"runtime": {
"elapsed_ms": (time.perf_counter() - started) * 1_000.0,
"frame_processing_ms": latency,
"process_peak_rss_start_mib": rss_start,
"process_peak_rss_end_mib": rss_end,
"process_peak_rss_growth_mib": rss_growth,
"rss_measurement": "process-peak-rss",
},
"point_age_ms": {
"lidar_to_camera": lidar_age_report,
"pose_to_lidar": pose_age_report,
"basis": "accepted-e10-nearest-host-arrival-best-effort",
},
},
"semantic_contract": {
"camera_owns_semantics": True,
"numeric_confidence_available": False,
"freshness_explicit": True,
"conflict_explicit": True,
},
"motion_contract": {
"classification": "diagnostic-motion-candidate",
"dynamic_class_available": False,
"map_frame_jump_rejected": True,
},
"proximity_contract": {
"classification": "diagnostic-near-occupied-candidate",
"collision_state_available": False,
"reason": "vehicle-body-and-lidar-mount-geometry-not-bound",
},
"acceptance": {
"accepted": False,
"upstream_unchanged": False,
"checks": checks,
},
"authority": _authority(),
}
def _motion_metrics(
history: list[object],
*,
map_frame_jump_candidate: bool,
minimum_observations: int,
minimum_span_seconds: float,
minimum_displacement_m: float,
minimum_speed_mps: float,
maximum_speed_mps: float,
) -> dict[str, Any]:
points = [_object(value, "E51 history observation") for value in history]
if len(points) < 2:
span_seconds = 0.0
displacement_m = 0.0
speed_mps = 0.0
else:
first = points[0]
last = points[-1]
first_xyz = _xyz(first.get("centroid_map_xyz_m"))
last_xyz = _xyz(last.get("centroid_map_xyz_m"))
span_seconds = float(last["session_seconds"]) - float(first["session_seconds"])
displacement_m = math.dist(first_xyz, last_xyz)
speed_mps = displacement_m / span_seconds if span_seconds > 0.0 else 0.0
candidate = (
not map_frame_jump_candidate
and len(points) >= minimum_observations
and span_seconds >= minimum_span_seconds
and displacement_m >= minimum_displacement_m
and minimum_speed_mps <= speed_mps <= maximum_speed_mps
)
return {
"candidate": candidate,
"classification": "diagnostic-motion-candidate",
"observation_count": len(points),
"span_seconds": span_seconds,
"displacement_m": displacement_m,
"speed_mps": speed_mps,
"map_frame_jump_rejected": map_frame_jump_candidate,
"dynamic_class_available": False,
}
def _validate_bindings(
*,
profile: _Profile,
e32: Any,
e34: E34TemporalOccupiedReplay,
source: E10LidarFieldSource,
) -> None:
e34_identity = _object(e34.manifest.get("identity"), "E51 E34 identity")
if (
e32.result_id != profile.expected_e32_result_id
or e34.result_id != profile.expected_e34_result_id
or source.pack_id != profile.expected_source_pack_id
or e34_identity.get("e32_result_id") != e32.result_id
or e34_identity.get("source_session_id") != source.identity.get("session_id")
or not e34.accepted
):
raise E51MotionSemanticError("E51 upstream binding is invalid")
def _validate_frame_pair(
e32_frame: dict[str, Any],
e34_frame: dict[str, Any],
expected_index: int,
) -> None:
e32_seconds = e32_frame.get("session_seconds")
e34_seconds = e34_frame.get("session_seconds")
if (
e32_frame.get("frame_index") != expected_index
or e34_frame.get("frame_index") != expected_index
or not isinstance(e32_seconds, int | float)
or not isinstance(e34_seconds, int | float)
or abs(float(e32_seconds) - float(e34_seconds)) > 1e-9
or e32_frame.get("source_frame_index")
!= e34_frame.get("source_frame_index")
):
raise E51MotionSemanticError("E51 upstream frame alignment is invalid")
def _read_profile(path: Path) -> _Profile:
raw = _read_json(path.expanduser().resolve(strict=True))
motion = _object(raw.get("motion"), "E51 motion profile")
proximity = _object(raw.get("proximity"), "E51 proximity profile")
acceptance = _object(raw.get("acceptance"), "E51 acceptance profile")
expected = _object(raw.get("expected"), "E51 expected sources")
profile = _Profile(
raw=raw,
expected_e32_result_id=_required_string(expected.get("e32_result_id")),
expected_e34_result_id=_required_string(expected.get("e34_result_id")),
expected_source_pack_id=_required_string(expected.get("source_pack_id")),
minimum_motion_observations=_positive_int(
motion.get("minimum_observations")
),
minimum_motion_span_seconds=_positive_float(
motion.get("minimum_span_seconds")
),
minimum_motion_displacement_m=_positive_float(
motion.get("minimum_displacement_m")
),
minimum_motion_speed_mps=_positive_float(
motion.get("minimum_speed_mps")
),
maximum_motion_speed_mps=_positive_float(
motion.get("maximum_speed_mps")
),
proximity_threshold_m=_positive_float(
proximity.get("threshold_m")
),
maximum_signals_per_frame=_positive_int(
acceptance.get("maximum_signals_per_frame")
),
maximum_latency_p95_ms=_positive_float(
acceptance.get("maximum_latency_p95_ms")
),
maximum_rss_growth_mib=_positive_float(
acceptance.get("maximum_rss_growth_mib")
),
maximum_lidar_camera_age_p95_ms=_positive_float(
acceptance.get("maximum_lidar_camera_age_p95_ms")
),
maximum_pose_age_p95_ms=_positive_float(
acceptance.get("maximum_pose_age_p95_ms")
),
)
if (
raw.get("schema_version") != E51_PROFILE_SCHEMA
or raw.get("profile_id")
!= "e51-motion-proximity-semantic-qualification/v1"
or profile.maximum_motion_speed_mps <= profile.minimum_motion_speed_mps
):
raise E51MotionSemanticError("E51 profile is invalid")
return profile
def _count_signal(counts: dict[str, int], signal: dict[str, Any]) -> None:
counts["total"] += 1
freshness = _object(signal["freshness"], "E51 signal freshness")
counts[str(freshness["state"])] += 1
if _object(signal["motion"], "E51 signal motion")["candidate"] is True:
counts["motion_candidates"] += 1
if _object(signal["proximity"], "E51 signal proximity")["candidate"] is True:
counts["proximity_candidates"] += 1
if _object(signal["semantic"], "E51 signal semantic")["available"] is True:
counts["semantic_available"] += 1
if _object(signal["evidence"], "E51 signal evidence")["conflict"] is True:
counts["conflicts"] += 1
def _finite_abs(value: Any) -> np.ndarray[Any, np.dtype[np.float64]]:
array = np.abs(np.asarray(value, dtype=np.float64))
return array[np.isfinite(array)]
def _distribution(values: np.ndarray[Any, np.dtype[np.float64]]) -> dict[str, Any]:
finite = values[np.isfinite(values)]
if finite.size == 0:
return {
"sample_count": 0,
"minimum": None,
"mean": None,
"p50": None,
"p95": None,
"maximum": None,
}
return {
"sample_count": int(finite.size),
"minimum": float(np.min(finite)),
"mean": float(np.mean(finite)),
"p50": float(np.percentile(finite, 50)),
"p95": float(np.percentile(finite, 95)),
"maximum": float(np.max(finite)),
}
def _process_peak_rss_mib() -> float:
value = float(resource.getrusage(resource.RUSAGE_SELF).ru_maxrss)
divisor = 1024.0 * 1024.0 if sys.platform == "darwin" else 1024.0
return value / divisor
def _verified_artifacts(
root: Path,
raw: object,
*,
key: str,
) -> dict[str, Path]:
result: dict[str, Path] = {}
for value in _list(raw, "E51 artifacts"):
artifact = _object(value, "E51 artifact")
name = artifact.get(key)
relative = artifact.get("path")
sha256 = artifact.get("sha256")
byte_length = artifact.get("byte_length")
if (
not isinstance(name, str)
or not name
or name in result
or not isinstance(relative, str)
or Path(relative).name != relative
or not isinstance(sha256, str)
or _SHA256.fullmatch(sha256) is None
or not isinstance(byte_length, int)
or isinstance(byte_length, bool)
or byte_length < 0
):
raise E51MotionSemanticError("E51 artifact descriptor is invalid")
path = (root / relative).resolve(strict=True)
if (
path.parent != root
or not path.is_file()
or path.stat().st_size != byte_length
or _sha256(path) != sha256
):
raise E51MotionSemanticError("E51 artifact integrity failed")
result[name] = path
return result
def _artifact_identity(artifacts: dict[str, Path]) -> dict[str, dict[str, Any]]:
return {
name: {
"byte_length": path.stat().st_size,
"sha256": _sha256(path),
}
for name, path in sorted(artifacts.items())
}
def _artifact(path: Path, kind: str) -> dict[str, Any]:
return {
"kind": kind,
"path": path.name,
"media_type": (
"application/x-ndjson"
if path.suffix == ".jsonl"
else "application/json"
),
"byte_length": path.stat().st_size,
"sha256": _sha256(path),
}
def _parse_json_line(line: str, label: str) -> dict[str, Any]:
try:
value = json.loads(line)
except json.JSONDecodeError as exc:
raise E51MotionSemanticError(f"{label} is invalid JSON") from exc
return _object(value, label)
def _read_json(path: Path) -> dict[str, Any]:
try:
value = json.loads(path.read_text(encoding="utf-8"))
except (OSError, json.JSONDecodeError) as exc:
raise E51MotionSemanticError(f"E51 JSON is invalid: {path}") from exc
return _object(value, f"E51 JSON {path.name}")
def _write_json(path: Path, value: object) -> None:
path.write_bytes(_canonical_json(value) + b"\n")
def _write_json_line(stream: TextIO, value: object) -> None:
stream.write(_canonical_json(value).decode("utf-8"))
stream.write("\n")
def _canonical_json(value: object) -> bytes:
return json.dumps(
value,
ensure_ascii=False,
sort_keys=True,
separators=(",", ":"),
allow_nan=False,
).encode("utf-8")
def _sha256(path: Path) -> str:
digest = hashlib.sha256()
with path.open("rb") as stream:
for chunk in iter(lambda: stream.read(1024 * 1024), b""):
digest.update(chunk)
return digest.hexdigest()
def _authority() -> dict[str, bool]:
return {
"commands_enabled": False,
"navigation_or_safety_accepted": False,
}
def _object(value: object, label: str) -> dict[str, Any]:
if not isinstance(value, dict):
raise E51MotionSemanticError(f"{label} must be an object")
return cast(dict[str, Any], value)
def _list(value: object, label: str) -> list[object]:
if not isinstance(value, list):
raise E51MotionSemanticError(f"{label} must be a list")
return value
def _object_list(value: object, label: str) -> list[dict[str, Any]]:
return [_object(item, label) for item in _list(value, label)]
def _xyz(value: object) -> tuple[float, float, float]:
values = _list(value, "E51 centroid")
if (
len(values) != 3
or not all(
isinstance(item, int | float) and math.isfinite(float(item))
for item in values
)
):
raise E51MotionSemanticError("E51 centroid is invalid")
numeric = cast(list[int | float], values)
return (float(numeric[0]), float(numeric[1]), float(numeric[2]))
def _required_string(value: object) -> str:
if not isinstance(value, str) or not value:
raise E51MotionSemanticError("E51 required string is invalid")
return value
def _positive_int(value: object) -> int:
if not isinstance(value, int) or isinstance(value, bool) or value <= 0:
raise E51MotionSemanticError("E51 positive integer is invalid")
return value
def _nonnegative_int(value: object, label: str) -> int:
if not isinstance(value, int) or isinstance(value, bool) or value < 0:
raise E51MotionSemanticError(f"{label} is invalid")
return value
def _positive_float(value: object) -> float:
if (
not isinstance(value, int | float)
or isinstance(value, bool)
or not math.isfinite(float(value))
or float(value) <= 0.0
):
raise E51MotionSemanticError("E51 positive number is invalid")
return float(value)
def _required_float(value: object) -> float:
if not isinstance(value, int | float) or not math.isfinite(float(value)):
raise E51MotionSemanticError("E51 required number is invalid")
return float(value)
src/k1link/compute/lidar_local_surface.py
+229 -18
View File
@@ -24,6 +24,7 @@ from .lidar_field_review import (
RAVNOVES00_CENTRAL_WINDOWS,
E10LidarFieldSource,
)
from .lidar_replay import LIDAR_REPLAY_PACK_SCHEMA, LidarReplayPackV2
K1_LOCAL_SURFACE_SCHEMA: Final = "missioncore.k1-local-surface/v1"
K1_LOCAL_SURFACE_REPORT_SCHEMA: Final = "missioncore.k1-local-surface-report/v1"
@@ -53,6 +54,25 @@ _LOCAL_SURFACE_ID = re.compile(r"^k1-local-surface-[a-f0-9]{64}$")
_E10_PACK_ID = re.compile(r"^e10-lidar-pack-[a-f0-9]{64}$")
_SHA256 = re.compile(r"^[a-f0-9]{64}$")
@dataclass(frozen=True, slots=True)
class _LocalSurfaceSourceView:
pack_id: str
identity_sha256: str
artifact_sha256: str
session_id: str
representation: str
schema_version: str
intensity_available: bool
field_retention: dict[str, object] | None
arrays: Mapping[str, npt.NDArray[Any]]
frame_count: int
point_count: int
LocalSurfaceSource = (
E10LidarFieldSource | LidarReplayPackV2 | _LocalSurfaceSourceView
)
@dataclass(frozen=True, slots=True)
class K1LocalSurfaceProfile:
@@ -511,9 +531,11 @@ class K1LocalSurfaceV1:
def frame_detail(
self,
source: E10LidarFieldSource,
source: LocalSurfaceSource,
frame_index: int,
) -> dict[str, object]:
source_view = _local_surface_source_view(source)
source = source_view
_validate_source_binding(self, source)
if not 0 <= frame_index < source.frame_count:
raise IndexError(frame_index)
@@ -550,7 +572,7 @@ class K1LocalSurfaceV1:
"schema_version": K1_LOCAL_SURFACE_FRAME_SCHEMA,
"model_id": self.model_id,
"source_pack_id": source.pack_id,
"session_id": source.identity["session_id"],
"session_id": source.session_id,
"frame_index": frame_index,
"frame_count": source.frame_count,
"source_frame_index": int(source.arrays["source_frame_indices"][frame_index]),
@@ -716,7 +738,9 @@ class K1LocalSurfaceV1:
"ground_truth": False,
}
def timeline_detail(self, source: E10LidarFieldSource) -> dict[str, object]:
def timeline_detail(self, source: LocalSurfaceSource) -> dict[str, object]:
source_view = _local_surface_source_view(source)
source = source_view
_validate_source_binding(self, source)
frame_count = source.frame_count
if self.has_temporal_qualification:
@@ -745,7 +769,7 @@ class K1LocalSurfaceV1:
"schema_version": K1_LOCAL_SURFACE_TIMELINE_SCHEMA,
"model_id": self.model_id,
"source_pack_id": source.pack_id,
"session_id": source.identity["session_id"],
"session_id": source.session_id,
"frame_count": frame_count,
"source_frame_index": source.arrays["source_frame_indices"]
.astype(np.int64)
@@ -772,7 +796,9 @@ class K1LocalSurfaceV1:
"authority": self.report["authority"],
}
def review_detail(self, source: E10LidarFieldSource) -> dict[str, object]:
def review_detail(self, source: LocalSurfaceSource) -> dict[str, object]:
source_view = _local_surface_source_view(source)
source = source_view
_validate_source_binding(self, source)
criteria = self._review_criteria()
reason_counts = {
@@ -788,7 +814,7 @@ class K1LocalSurfaceV1:
"review_profile_id": K1_LOCAL_SURFACE_REVIEW_PROFILE_ID,
"model_id": self.model_id,
"source_pack_id": source.pack_id,
"session_id": source.identity["session_id"],
"session_id": source.session_id,
"available": False,
"criteria": criteria,
"summary": {
@@ -918,7 +944,7 @@ class K1LocalSurfaceV1:
"review_profile_id": K1_LOCAL_SURFACE_REVIEW_PROFILE_ID,
"model_id": self.model_id,
"source_pack_id": source.pack_id,
"session_id": source.identity["session_id"],
"session_id": source.session_id,
"available": True,
"criteria": criteria,
"summary": {
@@ -985,7 +1011,7 @@ class K1LocalSurfaceV1:
def build_k1_local_surface(
source: E10LidarFieldSource,
source: LocalSurfaceSource,
output_root: Path,
*,
profile: K1LocalSurfaceProfile = DEFAULT_K1_LOCAL_SURFACE_PROFILE,
@@ -995,6 +1021,8 @@ def build_k1_local_surface(
if not display_name.strip() or len(display_name) > 200:
raise LidarGroundError("K1 local-surface display name is invalid")
source_view = _local_surface_source_view(source)
source = source_view
started = time.perf_counter()
frame_count = source.frame_count
point_count = source.point_count
@@ -1174,9 +1202,11 @@ def build_k1_local_surface(
identity = {
"schema_version": K1_LOCAL_SURFACE_SCHEMA,
"source_pack_id": source.pack_id,
"source_pack_identity_sha256": source.manifest["identity_sha256"],
"source_artifact_sha256": source.manifest["artifact"]["sha256"],
"session_id": source.identity["session_id"],
"source_pack_identity_sha256": source.identity_sha256,
"source_artifact_sha256": source.artifact_sha256,
"source_schema_version": source.schema_version,
"source_representation": source.representation,
"session_id": source.session_id,
"display_name": display_name,
"frame_count": frame_count,
"valid_frame_count": int(np.count_nonzero(valid)),
@@ -1204,12 +1234,15 @@ def build_k1_local_surface(
"schema_version": K1_LOCAL_SURFACE_REPORT_SCHEMA,
"model_id": model_id,
"display_name": display_name,
"session_id": source.identity["session_id"],
"session_id": source.session_id,
"source_pack_id": source.pack_id,
"status": "diagnostic-only",
"ground_truth": False,
"source": {
"representation": "legacy-e10-vendor-map-with-pose",
"representation": source.representation,
"schema_version": source.schema_version,
"intensity_available": source.intensity_available,
"field_retention": source.field_retention,
"immutable": True,
"passive_processing_only": True,
"firmware_or_device_commands_used": False,
@@ -1725,7 +1758,7 @@ def _height_above_plane(
def _anchors(
source: E10LidarFieldSource,
source: _LocalSurfaceSourceView,
valid: npt.NDArray[np.bool_],
) -> list[dict[str, object]]:
source_indices = source.arrays["source_frame_indices"]
@@ -1737,6 +1770,18 @@ def _anchors(
for window in RAVNOVES00_CENTRAL_WINDOWS:
if candidates.size == 0:
frame_index = 0
elif source.representation == "lossless-lidar-replay-v2":
midpoint_seconds = (window.start_seconds + window.end_seconds) / 2.0
frame_index = int(
candidates[
np.argmin(
np.abs(
source.arrays["session_seconds"][candidates]
- midpoint_seconds
)
)
]
)
else:
frame_index = int(
candidates[
@@ -1763,20 +1808,185 @@ def _anchors(
def _validate_source_binding(
model: K1LocalSurfaceV1,
source: E10LidarFieldSource,
source: _LocalSurfaceSourceView,
) -> None:
if (
model.identity.get("source_pack_id") != source.pack_id
or model.identity.get("source_pack_identity_sha256")
!= source.manifest.get("identity_sha256")
!= source.identity_sha256
or model.identity.get("source_artifact_sha256")
!= source.manifest.get("artifact", {}).get("sha256")
!= source.artifact_sha256
or model.identity.get("frame_count") != source.frame_count
or model.identity.get("point_count") != source.point_count
):
raise LidarGroundError("K1 local-surface source binding is invalid")
def _local_surface_source_view(
source: LocalSurfaceSource | _LocalSurfaceSourceView,
) -> _LocalSurfaceSourceView:
if isinstance(source, _LocalSurfaceSourceView):
return source
if isinstance(source, E10LidarFieldSource):
artifact = _object(source.manifest.get("artifact"), "E10 LiDAR artifact")
identity_sha256 = source.manifest.get("identity_sha256")
artifact_sha256 = artifact.get("sha256")
session_id = source.identity.get("session_id")
if (
not isinstance(identity_sha256, str)
or _SHA256.fullmatch(identity_sha256) is None
or not isinstance(artifact_sha256, str)
or _SHA256.fullmatch(artifact_sha256) is None
or not isinstance(session_id, str)
or not session_id
):
raise LidarGroundError("E10 local-surface source binding is invalid")
arrays = {
name: np.asarray(source.arrays[name])
for name in (
"source_frame_indices",
"session_seconds",
"sample_available",
"cloud_offsets",
"cloud_points_map",
"pose_positions_map",
"pose_quaternions_map_from_lidar",
"pose_point_delta_ms",
)
}
return _LocalSurfaceSourceView(
pack_id=source.pack_id,
identity_sha256=identity_sha256,
artifact_sha256=artifact_sha256,
session_id=session_id,
representation="legacy-e10-vendor-map-with-pose",
schema_version=str(source.identity["schema_version"]),
intensity_available=False,
field_retention=None,
arrays=arrays,
frame_count=source.frame_count,
point_count=source.point_count,
)
if not isinstance(source, LidarReplayPackV2):
raise LidarGroundError("K1 local-surface source type is unsupported")
identity_sha256 = source.manifest.get("identity_sha256")
session_id = source.identity.get("session_id")
artifact_sha256 = _lidar_replay_arrays_sha256(source)
if (
source.identity.get("schema_version") != LIDAR_REPLAY_PACK_SCHEMA
or not isinstance(identity_sha256, str)
or _SHA256.fullmatch(identity_sha256) is None
or not isinstance(session_id, str)
or not session_id
):
raise LidarGroundError("LiDAR replay v2 local-surface binding is invalid")
point_times = np.asarray(
source.arrays["point_received_monotonic_ns"],
dtype="<i8",
)
pose_times = np.asarray(
source.arrays["pose_received_monotonic_ns"],
dtype="<i8",
)
if point_times.size < 1 or np.any(np.diff(point_times) <= 0):
raise LidarGroundError("LiDAR replay v2 point time is not strictly increasing")
pose_indices, pose_delta_ms = _nearest_pose_indices(point_times, pose_times)
if pose_times.size:
positions = np.asarray(
source.arrays["pose_positions_map"][pose_indices],
dtype="<f8",
)
quaternions = np.asarray(
source.arrays["pose_quaternions_map_from_lidar"][pose_indices],
dtype="<f8",
)
else:
positions = np.zeros((source.point_frame_count, 3), dtype="<f8")
quaternions = np.tile(
np.asarray([0.0, 0.0, 0.0, 1.0], dtype="<f8"),
(source.point_frame_count, 1),
)
origin_ns = int(
min(
int(point_times[0]),
int(pose_times[0]) if pose_times.size else int(point_times[0]),
)
)
session_seconds = (
point_times.astype(np.float64) - float(origin_ns)
) / 1_000_000_000.0
arrays = {
"source_frame_indices": np.asarray(
source.arrays["point_capture_sequence"],
dtype="<i8",
),
"session_seconds": np.asarray(session_seconds, dtype="<f8"),
"sample_available": np.ones(source.point_frame_count, dtype="?"),
"cloud_offsets": np.asarray(source.arrays["point_offsets"], dtype="<i8"),
"cloud_points_map": np.asarray(
source.arrays["point_xyz_map"],
dtype="<f8",
),
"pose_positions_map": positions,
"pose_quaternions_map_from_lidar": quaternions,
"pose_point_delta_ms": pose_delta_ms,
}
field_retention_value = source.identity.get("field_retention")
field_retention = (
cast(dict[str, object], field_retention_value)
if isinstance(field_retention_value, dict)
else None
)
return _LocalSurfaceSourceView(
pack_id=source.pack_id,
identity_sha256=identity_sha256,
artifact_sha256=artifact_sha256,
session_id=session_id,
representation="lossless-lidar-replay-v2",
schema_version=LIDAR_REPLAY_PACK_SCHEMA,
intensity_available=True,
field_retention=field_retention,
arrays=arrays,
frame_count=source.point_frame_count,
point_count=source.point_count,
)
def _lidar_replay_arrays_sha256(source: LidarReplayPackV2) -> str:
artifacts = _list(source.manifest.get("artifacts"), "LiDAR replay artifacts")
for value in artifacts:
artifact = _object(value, "LiDAR replay artifact")
if artifact.get("kind") == "lidar-arrays":
sha256 = artifact.get("sha256")
if isinstance(sha256, str) and _SHA256.fullmatch(sha256) is not None:
return sha256
raise LidarGroundError("LiDAR replay arrays artifact is missing")
def _nearest_pose_indices(
point_times: npt.NDArray[np.int64],
pose_times: npt.NDArray[np.int64],
) -> tuple[npt.NDArray[np.int64], npt.NDArray[np.float64]]:
if pose_times.size == 0:
return (
np.zeros(point_times.shape[0], dtype="<i8"),
np.full(point_times.shape[0], np.inf, dtype="<f8"),
)
if np.any(np.diff(pose_times) < 0):
raise LidarGroundError("LiDAR replay v2 pose time is not monotonic")
right = np.searchsorted(pose_times, point_times, side="left")
right = np.clip(right, 0, pose_times.shape[0] - 1)
left = np.maximum(right - 1, 0)
right_delta = np.abs(pose_times[right] - point_times)
left_delta = np.abs(pose_times[left] - point_times)
indices = np.where(left_delta <= right_delta, left, right).astype("<i8")
delta_ms = (
np.abs(pose_times[indices] - point_times).astype(np.float64)
/ 1_000_000.0
)
return indices, np.asarray(delta_ms, dtype="<f8")
def _valid_distribution(
values: npt.NDArray[np.float64],
mask: npt.NDArray[np.bool_],
@@ -1810,7 +2020,8 @@ def _logical_sha256(arrays: Mapping[str, npt.NDArray[Any]]) -> str:
digest.update(name.encode())
digest.update(array.dtype.str.encode())
digest.update(_canonical_json(list(array.shape)))
digest.update(memoryview(array).cast("B"))
if array.nbytes:
digest.update(memoryview(array).cast("B"))
return digest.hexdigest()
src/k1link/compute/lidar_replay.py
+144 -93
View File
@@ -21,12 +21,9 @@ from k1link.data_plane import (
DecodedPoseView,
)
from k1link.device_plugins.xgrids_k1.protocol.streams import (
LioPointCloudFrame,
LioPoseFrame,
decode_lio_pcl,
decode_lio_pose,
)
from k1link.device_plugins.xgrids_k1.viewer.messages import StreamMessage
from k1link.device_plugins.xgrids_k1.viewer.replay import iter_replay_messages
from .lidar_contract import (
@@ -86,6 +83,27 @@ class LidarReplayError(ValueError):
"""A replay pack or its source evidence violates the v2 contract."""
class _MaterializedNpz:
"""One-time decompression wrapper for bounded repeated array access."""
def __init__(self, path: Path) -> None:
archive = np.load(path, allow_pickle=False)
try:
self.files = list(archive.files)
self._arrays = {
name: np.asarray(archive[name])
for name in self.files
}
finally:
archive.close()
def __getitem__(self, name: str) -> npt.NDArray[Any]:
return self._arrays[name]
def close(self) -> None:
self._arrays.clear()
@dataclass(frozen=True, slots=True)
class LidarReplayPointFrame:
capture_sequence: int
@@ -180,7 +198,7 @@ class LidarReplayPackV2:
self.arrays_path = artifacts["lidar-arrays"]
self.quality_path = artifacts["lidar-quality"]
self.equivalence_path = artifacts["live-replay-equivalence"]
self.arrays = np.load(self.arrays_path, allow_pickle=False)
self.arrays = _MaterializedNpz(self.arrays_path)
if set(self.arrays.files) != set(_ARRAY_DTYPES):
self.close()
raise LidarReplayError("LiDAR replay array set is incompatible")
@@ -413,6 +431,7 @@ def build_lidar_replay_pack_v2(
"artifacts": artifacts,
}
_write_json(staging / LIDAR_MANIFEST_NAME, manifest)
del arrays
os.replace(staging, output)
try:
validation = LidarReplayPackV2(output)
@@ -486,127 +505,159 @@ def lidar_pack_detail(pack: LidarReplayPackV2) -> dict[str, object]:
def _capture_arrays(path: Path) -> dict[str, npt.NDArray[Any]]:
point_messages: list[tuple[StreamMessage, LioPointCloudFrame]] = []
pose_messages: list[tuple[StreamMessage, LioPoseFrame]] = []
point_capture_sequence: list[int] = []
point_payload_bytes: list[int] = []
point_received_at_epoch_ns: list[int] = []
point_received_monotonic_ns: list[int] = []
point_header_seq: list[int] = []
point_header_stamp: list[int] = []
point_scaler: list[int] = []
point_counts: list[int] = []
pose_capture_sequence: list[int] = []
pose_payload_bytes: list[int] = []
pose_received_at_epoch_ns: list[int] = []
pose_received_monotonic_ns: list[int] = []
pose_header_seq: list[int] = []
pose_header_stamp: list[int] = []
pose_header_scaler: list[int] = []
pose_stamp: list[int] = []
pose_positions_map: list[tuple[float, float, float]] = []
pose_quaternions_map_from_lidar: list[tuple[float, float, float, float]] = []
pose_distance: list[float] = []
pose_accuracy: list[float] = []
for message in iter_replay_messages(path):
if message.source != "k1mqtt" or message.received_monotonic_ns is None:
raise LidarReplayError("LiDAR v2 requires native capture with exact host time")
if message.topic.endswith(_POINT_TOPIC_SUFFIX):
point_messages.append((message, decode_lio_pcl(message.payload)))
point_frame = decode_lio_pcl(message.payload)
point_capture_sequence.append(message.sequence)
point_payload_bytes.append(len(message.payload))
point_received_at_epoch_ns.append(message.received_at_epoch_ns)
point_received_monotonic_ns.append(message.received_monotonic_ns)
point_header_seq.append(point_frame.header.seq)
point_header_stamp.append(point_frame.header.stamp)
point_scaler.append(point_frame.header.scaler)
point_counts.append(len(point_frame.points))
elif message.topic.endswith(_POSE_TOPIC_SUFFIX):
pose_messages.append((message, decode_lio_pose(message.payload)))
if not point_messages:
pose_frame = decode_lio_pose(message.payload)
pose_capture_sequence.append(message.sequence)
pose_payload_bytes.append(len(message.payload))
pose_received_at_epoch_ns.append(message.received_at_epoch_ns)
pose_received_monotonic_ns.append(message.received_monotonic_ns)
pose_header_seq.append(pose_frame.header.seq)
pose_header_stamp.append(pose_frame.header.stamp)
pose_header_scaler.append(pose_frame.header.scaler)
pose_stamp.append(pose_frame.pose_stamp)
pose_positions_map.append(pose_frame.position_xyz)
pose_quaternions_map_from_lidar.append(pose_frame.orientation_xyzw)
pose_distance.append(pose_frame.distance)
pose_accuracy.append(pose_frame.pose_accuracy)
if not point_capture_sequence:
raise LidarReplayError("LiDAR replay source contains no lio_pcl frames")
if any(count <= 0 for count in point_counts):
raise LidarReplayError("LiDAR replay contains an empty point frame")
point_offsets = [0]
point_raw: list[npt.NDArray[np.int64]] = []
point_xyz: list[npt.NDArray[np.float64]] = []
point_rgbi: list[npt.NDArray[np.uint32]] = []
point_intensity: list[npt.NDArray[np.uint8]] = []
for _, frame in point_messages:
point_offsets = np.empty(len(point_counts) + 1, dtype="<i8")
point_offsets[0] = 0
np.cumsum(np.asarray(point_counts, dtype="<i8"), out=point_offsets[1:])
point_count = int(point_offsets[-1])
point_raw = np.empty((point_count, 3), dtype="<i8")
point_xyz = np.empty((point_count, 3), dtype="<f8")
point_rgbi = np.empty(point_count, dtype="<u4")
point_intensity = np.empty(point_count, dtype="u1")
point_index = 0
for message in iter_replay_messages(path):
if not message.topic.endswith(_POINT_TOPIC_SUFFIX):
continue
if point_index >= len(point_counts):
raise LidarReplayError("LiDAR source changed between bounded passes")
point_frame = decode_lio_pcl(message.payload)
if (
message.source != "k1mqtt"
or message.received_monotonic_ns is None
or message.sequence != point_capture_sequence[point_index]
or message.received_at_epoch_ns
!= point_received_at_epoch_ns[point_index]
or message.received_monotonic_ns
!= point_received_monotonic_ns[point_index]
or len(message.payload) != point_payload_bytes[point_index]
or point_frame.header.seq != point_header_seq[point_index]
or point_frame.header.stamp != point_header_stamp[point_index]
or point_frame.header.scaler != point_scaler[point_index]
or len(point_frame.points) != point_counts[point_index]
):
raise LidarReplayError("LiDAR source changed between bounded passes")
start = int(point_offsets[point_index])
end = int(point_offsets[point_index + 1])
raw = np.asarray(
[(point.x_raw, point.y_raw, point.z_raw) for point in frame.points],
[
(point.x_raw, point.y_raw, point.z_raw)
for point in point_frame.points
],
dtype="<i8",
).reshape((-1, 3))
rgbi = np.asarray([point.rgbi for point in frame.points], dtype="<u4")
intensity = (rgbi & np.uint32(0xFF)).astype(np.uint8)
xyz = raw.astype(np.float64) / float(frame.header.scaler)
point_raw.append(raw)
point_xyz.append(xyz)
point_rgbi.append(rgbi)
point_intensity.append(intensity)
point_offsets.append(point_offsets[-1] + raw.shape[0])
rgbi = np.asarray(
[point.rgbi for point in point_frame.points],
dtype="<u4",
)
point_raw[start:end] = raw
point_xyz[start:end] = raw.astype(np.float64) / float(
point_frame.header.scaler
)
point_rgbi[start:end] = rgbi
point_intensity[start:end] = (rgbi & np.uint32(0xFF)).astype(np.uint8)
point_index += 1
if point_index != len(point_counts):
raise LidarReplayError("LiDAR source changed between bounded passes")
arrays: dict[str, npt.NDArray[Any]] = {
"point_capture_sequence": _message_int_array(point_messages, "sequence"),
"point_payload_bytes": np.asarray(
[len(message.payload) for message, _ in point_messages],
"point_capture_sequence": np.asarray(point_capture_sequence, dtype="<i8"),
"point_payload_bytes": np.asarray(point_payload_bytes, dtype="<i8"),
"point_received_at_epoch_ns": np.asarray(
point_received_at_epoch_ns,
dtype="<i8",
),
"point_received_at_epoch_ns": _message_int_array(
point_messages,
"received_at_epoch_ns",
),
"point_received_monotonic_ns": np.asarray(
[message.received_monotonic_ns for message, _ in point_messages],
point_received_monotonic_ns,
dtype="<i8",
),
"point_header_seq": np.asarray(
[frame.header.seq for _, frame in point_messages],
dtype="<u8",
),
"point_header_stamp": np.asarray(
[frame.header.stamp for _, frame in point_messages],
"point_header_seq": np.asarray(point_header_seq, dtype="<u8"),
"point_header_stamp": np.asarray(point_header_stamp, dtype="<i8"),
"point_scaler": np.asarray(point_scaler, dtype="<i8"),
"point_offsets": point_offsets,
"point_raw_xyz": point_raw,
"point_xyz_map": point_xyz,
"point_rgbi": point_rgbi,
"point_intensity": point_intensity,
"pose_capture_sequence": np.asarray(pose_capture_sequence, dtype="<i8"),
"pose_payload_bytes": np.asarray(pose_payload_bytes, dtype="<i8"),
"pose_received_at_epoch_ns": np.asarray(
pose_received_at_epoch_ns,
dtype="<i8",
),
"point_scaler": np.asarray(
[frame.header.scaler for _, frame in point_messages],
dtype="<i8",
),
"point_offsets": np.asarray(point_offsets, dtype="<i8"),
"point_raw_xyz": np.concatenate(point_raw),
"point_xyz_map": np.concatenate(point_xyz),
"point_rgbi": np.concatenate(point_rgbi),
"point_intensity": np.concatenate(point_intensity),
"pose_capture_sequence": _message_int_array(pose_messages, "sequence"),
"pose_payload_bytes": np.asarray(
[len(message.payload) for message, _ in pose_messages],
dtype="<i8",
),
"pose_received_at_epoch_ns": _message_int_array(
pose_messages,
"received_at_epoch_ns",
),
"pose_received_monotonic_ns": np.asarray(
[message.received_monotonic_ns for message, _ in pose_messages],
dtype="<i8",
),
"pose_header_seq": np.asarray(
[frame.header.seq for _, frame in pose_messages],
dtype="<u8",
),
"pose_header_stamp": np.asarray(
[frame.header.stamp for _, frame in pose_messages],
dtype="<i8",
),
"pose_header_scaler": np.asarray(
[frame.header.scaler for _, frame in pose_messages],
dtype="<i8",
),
"pose_stamp": np.asarray(
[frame.pose_stamp for _, frame in pose_messages],
pose_received_monotonic_ns,
dtype="<i8",
),
"pose_header_seq": np.asarray(pose_header_seq, dtype="<u8"),
"pose_header_stamp": np.asarray(pose_header_stamp, dtype="<i8"),
"pose_header_scaler": np.asarray(pose_header_scaler, dtype="<i8"),
"pose_stamp": np.asarray(pose_stamp, dtype="<i8"),
"pose_positions_map": np.asarray(
[frame.position_xyz for _, frame in pose_messages],
pose_positions_map,
dtype="<f8",
).reshape((-1, 3)),
"pose_quaternions_map_from_lidar": np.asarray(
[frame.orientation_xyzw for _, frame in pose_messages],
pose_quaternions_map_from_lidar,
dtype="<f8",
).reshape((-1, 4)),
"pose_distance": np.asarray(
[frame.distance for _, frame in pose_messages],
dtype="<f8",
),
"pose_accuracy": np.asarray(
[frame.pose_accuracy for _, frame in pose_messages],
dtype="<f8",
),
"pose_distance": np.asarray(pose_distance, dtype="<f8"),
"pose_accuracy": np.asarray(pose_accuracy, dtype="<f8"),
}
return arrays
def _message_int_array(
messages: list[tuple[StreamMessage, Any]],
attribute: str,
) -> npt.NDArray[np.int64]:
return np.asarray(
[getattr(message, attribute) for message, _ in messages],
dtype="<i8",
)
def _validate_arrays(arrays: Any, identity: dict[str, Any]) -> None:
for name, dtype in _ARRAY_DTYPES.items():
if arrays[name].dtype != dtype: