feat(perception): add mixed-route vegetation review
This commit is contained in:
DCCONSTRUCTIONS
@@ -0,0 +1,418 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Publish LiDAR/pose evidence aligned to an immutable mixed-route review pack."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import hashlib
|
||||
import json
|
||||
import os
|
||||
import shutil
|
||||
import tempfile
|
||||
from datetime import UTC, datetime
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
from fuse_e6_tracking_lidar import CameraAnchor, _lidar_samples
|
||||
|
||||
from k1link.compute.jobs import validate_camera_compute_job
|
||||
from k1link.device_plugins.xgrids_k1.analyze.calibrated_overlay import (
|
||||
_load_calibration_snapshot,
|
||||
)
|
||||
from k1link.device_plugins.xgrids_k1.analyze.calibrated_projection import (
|
||||
Kb4ProjectionProfile,
|
||||
)
|
||||
from k1link.device_plugins.xgrids_k1.mqtt.capture import read_capture_clock_origin
|
||||
from k1link.device_plugins.xgrids_k1.protocol.streams import decode_lio_pcl
|
||||
from k1link.device_plugins.xgrids_k1.viewer.replay import iter_replay_messages
|
||||
|
||||
SCHEMA = "missioncore.mixed-route-lidar-pack/v1"
|
||||
REVIEW_SCHEMA = "missioncore.mixed-route-review-pack/v1"
|
||||
MAXIMUM_LIDAR_CAMERA_DELTA_MS = 100.0
|
||||
MAXIMUM_POSE_POINT_DELTA_MS = 100.0
|
||||
CAUSAL_HISTORY_SECONDS = 1.0
|
||||
|
||||
|
||||
class MixedRouteLidarPackError(RuntimeError):
|
||||
"""The recorded route cannot satisfy the selected LiDAR evidence contract."""
|
||||
|
||||
|
||||
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 _arguments() -> argparse.Namespace:
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument("--job", type=Path, required=True)
|
||||
parser.add_argument("--session", type=Path, required=True)
|
||||
parser.add_argument("--review-pack", type=Path, required=True)
|
||||
parser.add_argument("--calibration", type=Path, required=True)
|
||||
parser.add_argument("--output-root", type=Path, required=True)
|
||||
return parser.parse_args()
|
||||
|
||||
|
||||
def _read_review_pack(root: Path) -> tuple[dict[str, Any], list[dict[str, Any]]]:
|
||||
resolved = root.resolve(strict=True)
|
||||
manifest_path = resolved / "manifest.json"
|
||||
try:
|
||||
manifest = json.loads(manifest_path.read_text(encoding="utf-8"))
|
||||
except (OSError, json.JSONDecodeError) as exc:
|
||||
raise MixedRouteLidarPackError("mixed-route review manifest is invalid") from exc
|
||||
identity = manifest.get("identity") if isinstance(manifest, dict) else None
|
||||
timeline = manifest.get("timeline") if isinstance(manifest, dict) else None
|
||||
frames = manifest.get("frames") if isinstance(manifest, dict) else None
|
||||
if (
|
||||
manifest.get("schema_version") != REVIEW_SCHEMA
|
||||
or not isinstance(identity, dict)
|
||||
or identity.get("schema_version") != REVIEW_SCHEMA
|
||||
or identity.get("ground_truth") is not False
|
||||
or not isinstance(timeline, dict)
|
||||
or not isinstance(frames, list)
|
||||
or manifest.get("frame_count") != len(frames)
|
||||
or not frames
|
||||
):
|
||||
raise MixedRouteLidarPackError("mixed-route review contract changed")
|
||||
timeline_path = resolved / str(timeline.get("path"))
|
||||
if (
|
||||
not timeline_path.is_file()
|
||||
or timeline.get("sha256") != _sha256(timeline_path)
|
||||
or timeline.get("byte_length") != timeline_path.stat().st_size
|
||||
):
|
||||
raise MixedRouteLidarPackError("mixed-route review timeline changed")
|
||||
rows: list[dict[str, Any]] = []
|
||||
previous_seconds = -1.0
|
||||
with timeline_path.open(encoding="utf-8") as stream:
|
||||
for expected, line in enumerate(stream):
|
||||
try:
|
||||
row = json.loads(line)
|
||||
except json.JSONDecodeError as exc:
|
||||
raise MixedRouteLidarPackError("mixed-route timeline JSON is invalid") from exc
|
||||
seconds = row.get("session_seconds") if isinstance(row, dict) else None
|
||||
if (
|
||||
not isinstance(row, dict)
|
||||
or row.get("frame_index") != expected
|
||||
or row.get("sequence") != expected + 1
|
||||
or row.get("source_sequence") != row.get("source_frame_index") + 1
|
||||
or not isinstance(seconds, (int, float))
|
||||
or isinstance(seconds, bool)
|
||||
or float(seconds) <= previous_seconds
|
||||
):
|
||||
raise MixedRouteLidarPackError("mixed-route timeline row changed")
|
||||
rows.append(row)
|
||||
previous_seconds = float(seconds)
|
||||
if len(rows) != len(frames):
|
||||
raise MixedRouteLidarPackError("mixed-route timeline is incomplete")
|
||||
for frame in frames:
|
||||
path = resolved / str(frame.get("path"))
|
||||
if (
|
||||
not path.is_file()
|
||||
or frame.get("byte_length") != path.stat().st_size
|
||||
or frame.get("sha256") != _sha256(path)
|
||||
):
|
||||
raise MixedRouteLidarPackError("mixed-route source frame changed")
|
||||
return manifest, rows
|
||||
|
||||
|
||||
def _causal_history_clouds(
|
||||
raw_path: Path,
|
||||
*,
|
||||
origin_monotonic_ns: int,
|
||||
sample_seconds: list[float],
|
||||
) -> list[np.ndarray]:
|
||||
grouped: list[list[np.ndarray]] = [[] for _ in sample_seconds]
|
||||
last = sample_seconds[-1]
|
||||
for message in iter_replay_messages(raw_path):
|
||||
monotonic_ns = message.received_monotonic_ns
|
||||
if not isinstance(monotonic_ns, int) or monotonic_ns < origin_monotonic_ns:
|
||||
raise MixedRouteLidarPackError("MQTT replay message has no compatible clock")
|
||||
seconds = (monotonic_ns - origin_monotonic_ns) / 1e9
|
||||
if seconds > last:
|
||||
break
|
||||
if not message.topic.endswith("/lio_pcl"):
|
||||
continue
|
||||
matching = [
|
||||
index
|
||||
for index, sample_time in enumerate(sample_seconds)
|
||||
if sample_time - CAUSAL_HISTORY_SECONDS <= seconds <= sample_time
|
||||
]
|
||||
if not matching:
|
||||
continue
|
||||
frame = decode_lio_pcl(message.payload)
|
||||
cloud = np.asarray(
|
||||
[point.scaled_xyz(frame.header.scaler) for point in frame.points],
|
||||
dtype=np.float32,
|
||||
).reshape((-1, 3))
|
||||
if cloud.shape[0] == 0 or not np.isfinite(cloud).all():
|
||||
raise MixedRouteLidarPackError("causal LiDAR history is empty or non-finite")
|
||||
for index in matching:
|
||||
grouped[index].append(cloud)
|
||||
result: list[np.ndarray] = []
|
||||
for clouds in grouped:
|
||||
if not clouds:
|
||||
raise MixedRouteLidarPackError("selected frame has no causal LiDAR history")
|
||||
result.append(np.concatenate(clouds))
|
||||
return result
|
||||
|
||||
|
||||
def prepare(
|
||||
*,
|
||||
job_root: Path,
|
||||
session_root: Path,
|
||||
review_pack_root: Path,
|
||||
calibration_root: Path,
|
||||
output_root: Path,
|
||||
) -> Path:
|
||||
job = validate_camera_compute_job(job_root)
|
||||
session = session_root.resolve(strict=True)
|
||||
if not session.is_dir() or session.name != job.session_id:
|
||||
raise MixedRouteLidarPackError("camera job and observation session differ")
|
||||
review, timeline = _read_review_pack(review_pack_root)
|
||||
review_identity = review["identity"]
|
||||
if (
|
||||
review_identity.get("job_id") != job.job_id
|
||||
or review_identity.get("input_sha256") != job.input_sha256
|
||||
or review_identity.get("session_id") != job.session_id
|
||||
or review_identity.get("source_id") != job.source_id
|
||||
or review_identity.get("codec_epoch") != job.codec_epoch
|
||||
):
|
||||
raise MixedRouteLidarPackError("review pack and camera job differ")
|
||||
|
||||
calibration, calibration_sha256 = _load_calibration_snapshot(
|
||||
calibration_root.resolve(strict=True)
|
||||
)
|
||||
projection = Kb4ProjectionProfile.from_factory_calibration(calibration, job.source_id)
|
||||
capture_root = session / "captures" / "mqtt_live"
|
||||
origin_path = capture_root / "mqtt.timeline.origin.json"
|
||||
origin = read_capture_clock_origin(origin_path)
|
||||
anchors = [
|
||||
CameraAnchor(
|
||||
frame_index=int(row["frame_index"]),
|
||||
source_frame_index=int(row["source_frame_index"]),
|
||||
host_session_seconds=(
|
||||
int(row["host_monotonic_ns"]) - origin.started_monotonic_ns
|
||||
)
|
||||
/ 1e9,
|
||||
video_session_seconds=float(row["session_seconds"]),
|
||||
)
|
||||
for row in timeline
|
||||
]
|
||||
if any(
|
||||
anchor.host_session_seconds != anchor.video_session_seconds
|
||||
for anchor in anchors
|
||||
):
|
||||
raise MixedRouteLidarPackError("review timeline does not use host arrival time")
|
||||
samples = list(
|
||||
_lidar_samples(
|
||||
capture_root / "mqtt.raw.k1mqtt",
|
||||
anchors,
|
||||
origin_monotonic_ns=origin.started_monotonic_ns,
|
||||
maximum_lidar_camera_delta_s=MAXIMUM_LIDAR_CAMERA_DELTA_MS / 1000.0,
|
||||
maximum_pose_point_delta_s=MAXIMUM_POSE_POINT_DELTA_MS / 1000.0,
|
||||
)
|
||||
)
|
||||
if len(samples) != len(anchors):
|
||||
raise MixedRouteLidarPackError("LiDAR sampler did not account for every anchor")
|
||||
|
||||
count = len(anchors)
|
||||
available = np.zeros((count,), dtype=np.bool_)
|
||||
offsets = [0]
|
||||
clouds: list[np.ndarray] = []
|
||||
positions = np.full((count, 3), np.nan, dtype=np.float64)
|
||||
quaternions = np.full((count, 4), np.nan, dtype=np.float64)
|
||||
lidar_delta = np.full((count,), np.nan, dtype=np.float64)
|
||||
pose_delta = np.full((count,), np.nan, dtype=np.float64)
|
||||
sample_seconds: list[float] = []
|
||||
for index, (anchor, sample) in enumerate(zip(anchors, samples, strict=True)):
|
||||
if sample is None:
|
||||
offsets.append(offsets[-1])
|
||||
sample_seconds.append(float("nan"))
|
||||
continue
|
||||
cloud = np.asarray(
|
||||
[
|
||||
point.scaled_xyz(sample.point_frame.header.scaler)
|
||||
for point in sample.point_frame.points
|
||||
],
|
||||
dtype=np.float32,
|
||||
).reshape((-1, 3))
|
||||
if cloud.shape[0] == 0 or not np.isfinite(cloud).all():
|
||||
raise MixedRouteLidarPackError("selected LiDAR sample is empty or non-finite")
|
||||
available[index] = True
|
||||
clouds.append(cloud)
|
||||
offsets.append(offsets[-1] + cloud.shape[0])
|
||||
positions[index] = sample.pose_frame.position_xyz
|
||||
quaternions[index] = sample.pose_frame.orientation_xyzw
|
||||
lidar_delta[index] = (
|
||||
sample.point_session_seconds - anchor.host_session_seconds
|
||||
) * 1000.0
|
||||
pose_delta[index] = (
|
||||
sample.pose_session_seconds - sample.point_session_seconds
|
||||
) * 1000.0
|
||||
sample_seconds.append(sample.point_session_seconds)
|
||||
|
||||
if not available.all() or not np.isfinite(np.asarray(sample_seconds)).all():
|
||||
raise MixedRouteLidarPackError(
|
||||
"every mixed-route review island must have a temporally admissible LiDAR sample"
|
||||
)
|
||||
history_clouds = _causal_history_clouds(
|
||||
capture_root / "mqtt.raw.k1mqtt",
|
||||
origin_monotonic_ns=origin.started_monotonic_ns,
|
||||
sample_seconds=sample_seconds,
|
||||
)
|
||||
history_offsets = [0]
|
||||
for cloud in history_clouds:
|
||||
history_offsets.append(history_offsets[-1] + cloud.shape[0])
|
||||
|
||||
identity = {
|
||||
"schema_version": SCHEMA,
|
||||
"job_id": job.job_id,
|
||||
"input_sha256": job.input_sha256,
|
||||
"session_id": job.session_id,
|
||||
"source_id": job.source_id,
|
||||
"camera_slot": "camera_1",
|
||||
"calibration_sha256": calibration_sha256,
|
||||
"review_pack_id": review["pack_id"],
|
||||
"review_pack_identity_sha256": review["identity_sha256"],
|
||||
"selected_source_frame_indices": [
|
||||
int(row["source_frame_index"]) for row in timeline
|
||||
],
|
||||
"frame_count": count,
|
||||
"available_lidar_frames": int(available.sum()),
|
||||
"point_count": int(offsets[-1]),
|
||||
"causal_history_seconds": CAUSAL_HISTORY_SECONDS,
|
||||
"causal_history_point_count": int(history_offsets[-1]),
|
||||
"temporal_policy": {
|
||||
"binding": "nearest-host-arrival-best-effort",
|
||||
"maximum_lidar_camera_delta_ms": MAXIMUM_LIDAR_CAMERA_DELTA_MS,
|
||||
"maximum_pose_point_delta_ms": MAXIMUM_POSE_POINT_DELTA_MS,
|
||||
"clock_source": "recorded-host-monotonic-arrival",
|
||||
},
|
||||
"projection": {
|
||||
"model": "kb4",
|
||||
"width": projection.width,
|
||||
"height": projection.height,
|
||||
"source_coordinates": "k1-map",
|
||||
"target_camera": job.source_id,
|
||||
},
|
||||
"ground_truth": False,
|
||||
"authority": {
|
||||
"navigation_or_safety_accepted": False,
|
||||
"actuation_allowed": False,
|
||||
},
|
||||
"producer_sha256": _sha256(Path(__file__).resolve(strict=True)),
|
||||
}
|
||||
identity_sha256 = hashlib.sha256(_canonical_json(identity)).hexdigest()
|
||||
pack_id = f"mixed-route-lidar-pack-{identity_sha256}"
|
||||
parent = output_root.resolve()
|
||||
parent.mkdir(mode=0o700, parents=True, exist_ok=True)
|
||||
final = parent / pack_id
|
||||
if final.exists():
|
||||
return final
|
||||
staging = Path(tempfile.mkdtemp(prefix=f".{pack_id}.", dir=parent))
|
||||
published = False
|
||||
try:
|
||||
arrays_path = staging / "lidar-pack.npz"
|
||||
np.savez_compressed(
|
||||
arrays_path,
|
||||
frame_indices=np.arange(count, dtype=np.int64),
|
||||
source_frame_indices=np.asarray(
|
||||
[row["source_frame_index"] for row in timeline], dtype=np.int64
|
||||
),
|
||||
session_seconds=np.asarray(
|
||||
[anchor.video_session_seconds for anchor in anchors], dtype=np.float64
|
||||
),
|
||||
host_session_seconds=np.asarray(
|
||||
[anchor.host_session_seconds for anchor in anchors], dtype=np.float64
|
||||
),
|
||||
lidar_session_seconds=np.asarray(sample_seconds, dtype=np.float64),
|
||||
sample_available=available,
|
||||
cloud_offsets=np.asarray(offsets, dtype=np.int64),
|
||||
cloud_points_map=(
|
||||
np.concatenate(clouds) if clouds else np.empty((0, 3), dtype=np.float32)
|
||||
),
|
||||
pose_positions_map=positions,
|
||||
pose_quaternions_map_from_lidar=quaternions,
|
||||
lidar_camera_delta_ms=lidar_delta,
|
||||
pose_point_delta_ms=pose_delta,
|
||||
causal_history_seconds=np.asarray(
|
||||
[CAUSAL_HISTORY_SECONDS], dtype=np.float64
|
||||
),
|
||||
causal_history_offsets=np.asarray(history_offsets, dtype=np.int64),
|
||||
causal_history_points_map=np.concatenate(history_clouds),
|
||||
intrinsic_fx_fy_cx_cy=np.asarray(
|
||||
projection.intrinsic_fx_fy_cx_cy, dtype=np.float64
|
||||
),
|
||||
distortion_kb4=np.asarray(projection.distortion_kb4, dtype=np.float64),
|
||||
t_camera_from_lidar=np.asarray(projection.t_camera_from_lidar, dtype=np.float64),
|
||||
)
|
||||
manifest = {
|
||||
"schema_version": SCHEMA,
|
||||
"pack_id": pack_id,
|
||||
"identity_sha256": identity_sha256,
|
||||
"identity": identity,
|
||||
"created_at_utc": datetime.now(UTC)
|
||||
.isoformat(timespec="milliseconds")
|
||||
.replace("+00:00", "Z"),
|
||||
"classification": "private-recorded-sensor-review-input",
|
||||
"ground_truth": False,
|
||||
"artifact": {
|
||||
"path": arrays_path.name,
|
||||
"media_type": "application/x-npz",
|
||||
"byte_length": arrays_path.stat().st_size,
|
||||
"sha256": _sha256(arrays_path),
|
||||
},
|
||||
}
|
||||
(staging / "manifest.json").write_text(
|
||||
json.dumps(manifest, ensure_ascii=False, sort_keys=True, indent=2) + "\n",
|
||||
encoding="utf-8",
|
||||
)
|
||||
os.replace(staging, final)
|
||||
published = True
|
||||
finally:
|
||||
if not published:
|
||||
shutil.rmtree(staging, ignore_errors=True)
|
||||
return final
|
||||
|
||||
|
||||
def main() -> int:
|
||||
args = _arguments()
|
||||
output = prepare(
|
||||
job_root=args.job,
|
||||
session_root=args.session,
|
||||
review_pack_root=args.review_pack,
|
||||
calibration_root=args.calibration,
|
||||
output_root=args.output_root,
|
||||
)
|
||||
manifest = json.loads((output / "manifest.json").read_text(encoding="utf-8"))
|
||||
print(
|
||||
json.dumps(
|
||||
{
|
||||
"pack_id": manifest["pack_id"],
|
||||
"output": str(output),
|
||||
"frames": manifest["identity"]["frame_count"],
|
||||
"lidar_frames": manifest["identity"]["available_lidar_frames"],
|
||||
"points": manifest["identity"]["point_count"],
|
||||
"artifact_sha256": manifest["artifact"]["sha256"],
|
||||
},
|
||||
sort_keys=True,
|
||||
)
|
||||
)
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
+340
@@ -0,0 +1,340 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Run DDRNet on an immutable mixed-route camera review pack."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import hashlib
|
||||
import json
|
||||
import math
|
||||
import platform
|
||||
import statistics
|
||||
import time
|
||||
from pathlib import Path, PurePosixPath
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
import torch
|
||||
from PIL import Image
|
||||
from run_goose_vegetation_benchmark import (
|
||||
CLASS_COUNT,
|
||||
expand_mask,
|
||||
infer,
|
||||
load_mapping,
|
||||
load_model,
|
||||
percentile,
|
||||
preprocess,
|
||||
read_json,
|
||||
save_image,
|
||||
sha256,
|
||||
stable_digest,
|
||||
validate_contracts,
|
||||
)
|
||||
|
||||
SCHEMA = "missioncore.mixed-route-ddrnet-islands/v1"
|
||||
PACK_SCHEMA = "missioncore.mixed-route-review-pack/v1"
|
||||
AUTHORITY = {
|
||||
"ground_truth": False,
|
||||
"candidate_accepted": False,
|
||||
"navigation_or_safety_accepted": False,
|
||||
"camera_semantics_can_clear_rigid_geometry": False,
|
||||
"actuation_allowed": False,
|
||||
}
|
||||
|
||||
|
||||
class MixedRouteDdrnetError(RuntimeError):
|
||||
"""The route pack or DDRNet evidence changed or is incomplete."""
|
||||
|
||||
|
||||
def arguments() -> argparse.Namespace:
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument("--pack", type=Path, required=True)
|
||||
parser.add_argument("--config", type=Path, required=True)
|
||||
parser.add_argument("--policy", type=Path, required=True)
|
||||
parser.add_argument("--provider-map", type=Path, required=True)
|
||||
parser.add_argument("--checkpoint", type=Path, required=True)
|
||||
parser.add_argument("--dataset-root", type=Path, required=True)
|
||||
parser.add_argument("--output", type=Path, required=True)
|
||||
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("utf-8")
|
||||
|
||||
|
||||
def object_value(value: object, label: str) -> dict[str, Any]:
|
||||
if not isinstance(value, dict) or not all(isinstance(key, str) for key in value):
|
||||
raise MixedRouteDdrnetError(f"{label} must be an object")
|
||||
return value
|
||||
|
||||
|
||||
def load_pack(root: Path) -> tuple[dict[str, Any], list[dict[str, Any]]]:
|
||||
pack = root.resolve(strict=True)
|
||||
if not pack.is_dir() or pack.is_symlink():
|
||||
raise MixedRouteDdrnetError("mixed-route review pack is unavailable")
|
||||
manifest_path = pack / "manifest.json"
|
||||
manifest = object_value(
|
||||
json.loads(manifest_path.read_text(encoding="utf-8")),
|
||||
"mixed-route manifest",
|
||||
)
|
||||
identity = object_value(manifest.get("identity"), "mixed-route identity")
|
||||
identity_sha256 = manifest.get("identity_sha256")
|
||||
frames = manifest.get("frames")
|
||||
frame_count = manifest.get("frame_count")
|
||||
if (
|
||||
manifest.get("schema_version") != PACK_SCHEMA
|
||||
or identity.get("schema_version") != PACK_SCHEMA
|
||||
or not isinstance(identity_sha256, str)
|
||||
or hashlib.sha256(canonical_json(identity)).hexdigest() != identity_sha256
|
||||
or manifest.get("pack_id") != f"mixed-route-review-pack-{identity_sha256}"
|
||||
or identity.get("ground_truth") is not False
|
||||
or object_value(identity.get("authority"), "mixed-route authority").get(
|
||||
"navigation_or_safety_accepted"
|
||||
)
|
||||
is not False
|
||||
or not isinstance(frame_count, int)
|
||||
or isinstance(frame_count, bool)
|
||||
or not 1 <= frame_count <= 64
|
||||
or not isinstance(frames, list)
|
||||
or len(frames) != frame_count
|
||||
):
|
||||
raise MixedRouteDdrnetError("mixed-route review pack identity changed")
|
||||
timeline_descriptor = object_value(manifest.get("timeline"), "mixed-route timeline")
|
||||
timeline_path = pack / "timeline.jsonl"
|
||||
if (
|
||||
timeline_descriptor.get("path") != timeline_path.name
|
||||
or timeline_path.stat().st_size != timeline_descriptor.get("byte_length")
|
||||
or sha256(timeline_path) != timeline_descriptor.get("sha256")
|
||||
):
|
||||
raise MixedRouteDdrnetError("mixed-route timeline proof changed")
|
||||
rows: list[dict[str, Any]] = []
|
||||
with timeline_path.open(encoding="utf-8") as stream:
|
||||
for expected, line in enumerate(stream):
|
||||
row = object_value(json.loads(line), "mixed-route timeline row")
|
||||
seconds = row.get("session_seconds")
|
||||
if (
|
||||
row.get("frame_index") != expected
|
||||
or row.get("sequence") != expected + 1
|
||||
or not isinstance(row.get("source_sequence"), int)
|
||||
or row.get("source_frame_index") != row["source_sequence"] - 1
|
||||
or not isinstance(seconds, (int, float))
|
||||
or isinstance(seconds, bool)
|
||||
or (rows and float(seconds) <= float(rows[-1]["session_seconds"]))
|
||||
):
|
||||
raise MixedRouteDdrnetError("mixed-route timeline order changed")
|
||||
rows.append(row)
|
||||
if len(rows) != frame_count:
|
||||
raise MixedRouteDdrnetError("mixed-route timeline is incomplete")
|
||||
for expected, (descriptor_raw, row) in enumerate(zip(frames, rows)): # noqa: B905
|
||||
descriptor = object_value(descriptor_raw, "mixed-route frame descriptor")
|
||||
relative = descriptor.get("path")
|
||||
if relative != f"frames/frame-{expected + 1:06d}.png":
|
||||
raise MixedRouteDdrnetError("mixed-route frame path changed")
|
||||
pure = PurePosixPath(relative)
|
||||
path = pack.joinpath(*pure.parts)
|
||||
if (
|
||||
path.is_symlink()
|
||||
or not path.is_file()
|
||||
or not path.resolve().is_relative_to(pack)
|
||||
or path.stat().st_size != descriptor.get("byte_length")
|
||||
or sha256(path) != descriptor.get("sha256")
|
||||
or not isinstance(descriptor.get("source_segment_sha256"), str)
|
||||
or row.get("source_sequence")
|
||||
!= identity["selected_sequences"][expected]
|
||||
):
|
||||
raise MixedRouteDdrnetError("mixed-route frame proof changed")
|
||||
return manifest, rows
|
||||
|
||||
|
||||
def overlay(source: Image.Image, semantic: np.ndarray, palette: np.ndarray) -> Image.Image:
|
||||
if semantic.shape != (600, 800):
|
||||
raise MixedRouteDdrnetError("expanded semantic mask shape changed")
|
||||
base = source.convert("RGBA")
|
||||
colors = Image.fromarray(palette[semantic], mode="RGBA")
|
||||
return Image.alpha_composite(base, colors)
|
||||
|
||||
|
||||
def run() -> int:
|
||||
args = arguments()
|
||||
if not torch.cuda.is_available():
|
||||
raise MixedRouteDdrnetError("CUDA is required for DDRNet islands")
|
||||
if args.output.exists():
|
||||
raise MixedRouteDdrnetError("DDRNet islands output already exists")
|
||||
manifest, timeline = load_pack(args.pack)
|
||||
config = read_json(args.config, "benchmark config")
|
||||
policy = read_json(args.policy, "mission policy")
|
||||
provider_map = read_json(args.provider_map, "provider map")
|
||||
candidate = validate_contracts(config, policy, provider_map, "ddrnet")
|
||||
checkpoint = args.checkpoint.resolve(strict=True)
|
||||
if (
|
||||
checkpoint.is_symlink()
|
||||
or checkpoint.stat().st_size != candidate["checkpoint_size_bytes"]
|
||||
or sha256(checkpoint) != candidate["checkpoint_sha256"]
|
||||
):
|
||||
raise MixedRouteDdrnetError("DDRNet checkpoint identity changed")
|
||||
dataset_root = args.dataset_root.resolve(strict=True)
|
||||
mapping_path = dataset_root / config["dataset"]["mapping_relative_path"]
|
||||
names, palette = load_mapping(mapping_path, config["dataset"]["mapping_sha256"])
|
||||
|
||||
args.output.mkdir(mode=0o700, parents=True, exist_ok=False)
|
||||
mask_root = args.output / "semantic-masks"
|
||||
overlay_root = args.output / "overlay-frames"
|
||||
mask_root.mkdir(mode=0o700)
|
||||
overlay_root.mkdir(mode=0o700)
|
||||
torch.cuda.empty_cache()
|
||||
model, model_name, architecture_failures = load_model("ddrnet", checkpoint)
|
||||
first_path = args.pack / manifest["frames"][0]["path"]
|
||||
with Image.open(first_path) as opened:
|
||||
warm_source = opened.convert("RGB")
|
||||
warm_tensor, _ = preprocess(warm_source)
|
||||
warmup_ms = [infer(model, warm_tensor)[1] for _ in range(3)]
|
||||
torch.cuda.reset_peak_memory_stats()
|
||||
|
||||
latencies_ms: list[float] = []
|
||||
aggregate = np.zeros(CLASS_COUNT, dtype=np.int64)
|
||||
frame_results: list[dict[str, Any]] = []
|
||||
started = time.perf_counter()
|
||||
for index, (descriptor, timeline_row) in enumerate(
|
||||
zip(manifest["frames"], timeline) # noqa: B905 - Worker image uses Python 3.9.
|
||||
):
|
||||
source_path = args.pack / descriptor["path"]
|
||||
with Image.open(source_path) as opened:
|
||||
source = opened.convert("RGB")
|
||||
if source.size != (800, 600):
|
||||
raise MixedRouteDdrnetError("mixed-route source resolution changed")
|
||||
tensor, crop_box = preprocess(source)
|
||||
prediction, latency_ms = infer(model, tensor)
|
||||
expanded = expand_mask(prediction, source.size, crop_box)
|
||||
latencies_ms.append(latency_ms)
|
||||
aggregate += np.bincount(expanded.reshape(-1), minlength=CLASS_COUNT)
|
||||
mask_path = mask_root / f"frame-{index + 1:06d}.png"
|
||||
overlay_path = overlay_root / f"frame-{index + 1:06d}.png"
|
||||
mask_sha256 = save_image(mask_path, expanded, "L")
|
||||
overlay_sha256 = save_image(overlay_path, overlay(source, expanded, palette))
|
||||
present = np.flatnonzero(np.bincount(expanded.reshape(-1), minlength=CLASS_COUNT))
|
||||
frame_results.append(
|
||||
{
|
||||
"frame_index": index,
|
||||
"source_sequence": timeline_row["source_sequence"],
|
||||
"source_frame_index": timeline_row["source_frame_index"],
|
||||
"session_seconds": timeline_row["session_seconds"],
|
||||
"latency_ms": round(latency_ms, 6),
|
||||
"present_classes": [
|
||||
{"class_id": int(class_id), "label": names[int(class_id)]}
|
||||
for class_id in present
|
||||
],
|
||||
"mask": {
|
||||
"path": mask_path.relative_to(args.output).as_posix(),
|
||||
"byte_length": mask_path.stat().st_size,
|
||||
"sha256": mask_sha256,
|
||||
},
|
||||
"overlay": {
|
||||
"path": overlay_path.relative_to(args.output).as_posix(),
|
||||
"byte_length": overlay_path.stat().st_size,
|
||||
"sha256": overlay_sha256,
|
||||
},
|
||||
}
|
||||
)
|
||||
wall_seconds = time.perf_counter() - started
|
||||
if len(frame_results) != manifest["frame_count"]:
|
||||
raise MixedRouteDdrnetError("DDRNet island accounting changed")
|
||||
timing = {
|
||||
"prewarm_inference_count": len(warmup_ms),
|
||||
"prewarm_latency_ms_first": round(warmup_ms[0], 6),
|
||||
"prewarm_latency_ms_last": round(warmup_ms[-1], 6),
|
||||
"inference_wall_seconds": round(wall_seconds, 6),
|
||||
"latency_ms_mean": round(statistics.fmean(latencies_ms), 6),
|
||||
"latency_ms_p50": round(percentile(latencies_ms, 0.5), 6),
|
||||
"latency_ms_p95": round(percentile(latencies_ms, 0.95), 6),
|
||||
"throughput_fps_from_mean_inference": round(
|
||||
1000.0 / statistics.fmean(latencies_ms), 6
|
||||
),
|
||||
}
|
||||
if any(not math.isfinite(float(value)) for value in timing.values()):
|
||||
raise MixedRouteDdrnetError("DDRNet timing is non-finite")
|
||||
result: dict[str, Any] = {
|
||||
"schema_version": SCHEMA,
|
||||
"status": "review-islands-ready-not-accepted",
|
||||
"worker_id": "worker-006",
|
||||
"source": {
|
||||
"pack_id": manifest["pack_id"],
|
||||
"pack_identity_sha256": manifest["identity_sha256"],
|
||||
"job_id": manifest["identity"]["job_id"],
|
||||
"input_sha256": manifest["identity"]["input_sha256"],
|
||||
"session_id": manifest["identity"]["session_id"],
|
||||
"source_id": manifest["identity"]["source_id"],
|
||||
"frame_count": manifest["frame_count"],
|
||||
"ground_truth_available": False,
|
||||
},
|
||||
"candidate": {
|
||||
"candidate_key": "ddrnet",
|
||||
"candidate_id": candidate["candidate_id"],
|
||||
"loaded_model_name": model_name,
|
||||
"architecture_probe_failures": architecture_failures,
|
||||
"checkpoint_size_bytes": checkpoint.stat().st_size,
|
||||
"checkpoint_sha256": sha256(checkpoint),
|
||||
},
|
||||
"taxonomy": {
|
||||
"schema_version": "missioncore.lab-v1-vegetation-taxonomy/v1",
|
||||
"classes": [
|
||||
{
|
||||
"class_id": class_id,
|
||||
"label": names[class_id],
|
||||
"color_rgb": palette[class_id, :3].astype(int).tolist(),
|
||||
"disposition": "undefined" if class_id == 0 else "prediction",
|
||||
}
|
||||
for class_id in range(CLASS_COUNT)
|
||||
],
|
||||
},
|
||||
"aggregate_prediction_pixels": aggregate.tolist(),
|
||||
"frames": frame_results,
|
||||
"timing": timing,
|
||||
"resource": {
|
||||
"hostname": platform.node(),
|
||||
"gpu_name": torch.cuda.get_device_name(0),
|
||||
"peak_allocated_vram_bytes": int(torch.cuda.max_memory_allocated()),
|
||||
"peak_reserved_vram_bytes": int(torch.cuda.max_memory_reserved()),
|
||||
"torch_version": torch.__version__,
|
||||
"cuda_runtime_version": torch.version.cuda,
|
||||
"python_version": platform.python_version(),
|
||||
},
|
||||
"provenance": {
|
||||
"pack_manifest_sha256": sha256(args.pack / "manifest.json"),
|
||||
"config_sha256": sha256(args.config),
|
||||
"policy_sha256": sha256(args.policy),
|
||||
"provider_map_sha256": sha256(args.provider_map),
|
||||
"runner_sha256": sha256(Path(__file__)),
|
||||
},
|
||||
"limitations": [
|
||||
"Selected independently decodable islands are not a complete route timeline.",
|
||||
"RAVNOVES004TREE has no route truth; class colors are model predictions.",
|
||||
"DDRNet evidence cannot clear rigid geometry, person or vehicle vetoes.",
|
||||
],
|
||||
"authority": AUTHORITY,
|
||||
}
|
||||
result["result_id"] = f"mixed-route-ddrnet-islands-{stable_digest(result)}"
|
||||
(args.output / "result.json").write_text(
|
||||
json.dumps(result, ensure_ascii=False, sort_keys=True, indent=2) + "\n",
|
||||
encoding="utf-8",
|
||||
)
|
||||
print(
|
||||
json.dumps(
|
||||
{
|
||||
"result_id": result["result_id"],
|
||||
"frames": len(frame_results),
|
||||
"latency_p95_ms": timing["latency_ms_p95"],
|
||||
},
|
||||
sort_keys=True,
|
||||
)
|
||||
)
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(run())
|
||||
@@ -0,0 +1,277 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Seal fail-closed TRAVEL/TGS evidence for mixed-route review islands."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import csv
|
||||
import json
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
from build_tgs_fail_closed_evidence import (
|
||||
TgsEvidenceError,
|
||||
_load_float32,
|
||||
classify_exact_input,
|
||||
costmap_grid,
|
||||
rasterize_costmap,
|
||||
sha256_file,
|
||||
write_deterministic_npz,
|
||||
)
|
||||
|
||||
CONFIG_SCHEMA = "missioncore.mixed-route-tgs-review-profile/v1"
|
||||
INPUT_SCHEMA = "missioncore.mixed-route-tgs-input/v1"
|
||||
RESULT_SCHEMA = "missioncore.mixed-route-tgs-result/v1"
|
||||
FRAME_COUNT = 10
|
||||
|
||||
|
||||
def _timing(path: Path) -> dict[str, object]:
|
||||
rows: list[dict[str, object]] = []
|
||||
with path.open(encoding="utf-8", newline="") as stream:
|
||||
for raw in csv.DictReader(stream, delimiter="\t"):
|
||||
try:
|
||||
row = {
|
||||
"profile_id": str(raw["profile"]),
|
||||
"slot": int(raw["slot"]),
|
||||
"wall_seconds": float(raw["wall_seconds"]),
|
||||
"max_rss_kib": int(raw["max_rss_kib"]),
|
||||
}
|
||||
except (KeyError, TypeError, ValueError) as exc:
|
||||
raise TgsEvidenceError("TGS timing row is invalid") from exc
|
||||
if (
|
||||
row["profile_id"] not in {"current_increment", "causal_rolling_1s"}
|
||||
or not 0 <= row["slot"] < FRAME_COUNT
|
||||
or not 0 <= row["wall_seconds"] < 60
|
||||
or not 0 < row["max_rss_kib"] < 16 * 1024 * 1024
|
||||
):
|
||||
raise TgsEvidenceError("TGS timing value is invalid")
|
||||
rows.append(row)
|
||||
if len(rows) != FRAME_COUNT * 2:
|
||||
raise TgsEvidenceError("TGS timing is incomplete")
|
||||
seconds = np.asarray([row["wall_seconds"] for row in rows], dtype=np.float64)
|
||||
return {
|
||||
"runs": rows,
|
||||
"wall_seconds_mean": round(float(seconds.mean()), 6),
|
||||
"wall_seconds_p95": round(float(np.percentile(seconds, 95)), 6),
|
||||
"max_rss_kib": max(int(row["max_rss_kib"]) for row in rows),
|
||||
}
|
||||
|
||||
|
||||
def build(run_root: Path, config_path: Path, output_root: Path) -> dict[str, object]:
|
||||
if output_root.exists():
|
||||
raise TgsEvidenceError("mixed-route TGS evidence already exists")
|
||||
config = json.loads(config_path.read_text(encoding="utf-8"))
|
||||
source = config.get("source") if isinstance(config, dict) else None
|
||||
invariants = config.get("invariants") if isinstance(config, dict) else None
|
||||
if (
|
||||
config.get("schema_version") != CONFIG_SCHEMA
|
||||
or not isinstance(source, dict)
|
||||
or not isinstance(invariants, dict)
|
||||
or invariants.get("aos_allowed") is not False
|
||||
or invariants.get("missing_support_means_free") is not False
|
||||
or invariants.get("future_frames_used") is not False
|
||||
or invariants.get("navigation_or_actuation_allowed") is not False
|
||||
or config.get("state_codes")
|
||||
!= {
|
||||
"UNOBSERVED": 0,
|
||||
"GROUND_SUPPORT": 1,
|
||||
"NONGROUND_OCCUPIED": 2,
|
||||
"UNKNOWN_REJECTED": 3,
|
||||
}
|
||||
):
|
||||
raise TgsEvidenceError("mixed-route TGS profile changed")
|
||||
input_manifest_path = run_root / "inputs" / "input-manifest.json"
|
||||
input_manifest = json.loads(input_manifest_path.read_text(encoding="utf-8"))
|
||||
if (
|
||||
input_manifest.get("schema_version") != INPUT_SCHEMA
|
||||
or input_manifest.get("source_pack_id") != source.get("source_pack_id")
|
||||
or input_manifest.get("source_pack_sha256")
|
||||
!= source.get("source_pack_sha256")
|
||||
or input_manifest.get("config_sha256") != sha256_file(config_path)
|
||||
or input_manifest.get("coordinate_frame") != "map-gravity-local"
|
||||
or input_manifest.get("future_frames_used") is not False
|
||||
or input_manifest.get("frame_count") != FRAME_COUNT
|
||||
or len(input_manifest.get("records", [])) != FRAME_COUNT * 2
|
||||
):
|
||||
raise TgsEvidenceError("mixed-route TGS input manifest changed")
|
||||
records = {
|
||||
(str(row["profile_id"]), int(row["slot"])): row
|
||||
for row in input_manifest["records"]
|
||||
}
|
||||
if len(records) != FRAME_COUNT * 2:
|
||||
raise TgsEvidenceError("mixed-route TGS input records are not unique")
|
||||
|
||||
cell_size = float(config["costmap"]["cell_size_m"])
|
||||
radius = float(config["costmap"]["radius_m"])
|
||||
grid = costmap_grid(radius, cell_size)
|
||||
arrays: dict[str, np.ndarray] = {
|
||||
"costmap_cell_indices_xy": grid[:, :2].astype(np.int32),
|
||||
"costmap_cell_centers_xy_m": grid[:, 2:].astype(np.float32),
|
||||
"source_frame_indices": np.asarray(
|
||||
[
|
||||
records[("current_increment", slot)]["source_frame_index"]
|
||||
for slot in range(FRAME_COUNT)
|
||||
],
|
||||
dtype=np.int64,
|
||||
),
|
||||
"session_seconds": np.asarray(
|
||||
[
|
||||
records[("current_increment", slot)]["session_seconds"]
|
||||
for slot in range(FRAME_COUNT)
|
||||
],
|
||||
dtype=np.float64,
|
||||
),
|
||||
}
|
||||
summaries: list[dict[str, object]] = []
|
||||
for profile_id in ("current_increment", "causal_rolling_1s"):
|
||||
all_points: list[np.ndarray] = []
|
||||
all_states: list[np.ndarray] = []
|
||||
offsets = [0]
|
||||
grid_states: list[np.ndarray] = []
|
||||
ground_counts: list[np.ndarray] = []
|
||||
nonground_counts: list[np.ndarray] = []
|
||||
rejected_counts: list[np.ndarray] = []
|
||||
z_bounds_rows: list[np.ndarray] = []
|
||||
for slot in range(FRAME_COUNT):
|
||||
record = records[(profile_id, slot)]
|
||||
native_path = run_root / "inputs" / str(record["relative_path"])
|
||||
if (
|
||||
not native_path.is_file()
|
||||
or native_path.stat().st_size != record["bytes"]
|
||||
or sha256_file(native_path) != record["sha256"]
|
||||
):
|
||||
raise TgsEvidenceError("sealed mixed-route TGS input changed")
|
||||
output = run_root / "outputs" / profile_id
|
||||
points, states = classify_exact_input(
|
||||
_load_float32(native_path, 4),
|
||||
_load_float32(output / f"{slot}_ground.bin", 4),
|
||||
_load_float32(output / f"{slot}_nonground.bin", 4),
|
||||
min_range_m=float(config["tgs"]["min_range_m"]),
|
||||
max_range_m=float(config["tgs"]["max_range_m"]),
|
||||
)
|
||||
grid_state, ground, nonground, rejected, z_bounds = rasterize_costmap(
|
||||
points,
|
||||
states,
|
||||
grid,
|
||||
cell_size_m=cell_size,
|
||||
)
|
||||
all_points.append(points.astype(np.float32, copy=False))
|
||||
all_states.append(states)
|
||||
offsets.append(offsets[-1] + points.shape[0])
|
||||
grid_states.append(grid_state)
|
||||
ground_counts.append(ground)
|
||||
nonground_counts.append(nonground)
|
||||
rejected_counts.append(rejected)
|
||||
z_bounds_rows.append(z_bounds)
|
||||
accounted = (
|
||||
np.count_nonzero(states == 1)
|
||||
+ np.count_nonzero(states == 2)
|
||||
+ np.count_nonzero(states == 3)
|
||||
== points.shape[0]
|
||||
)
|
||||
summaries.append(
|
||||
{
|
||||
"profile_id": profile_id,
|
||||
"slot": slot,
|
||||
"frame_index": int(record["frame_index"]),
|
||||
"source_frame_index": int(record["source_frame_index"]),
|
||||
"source_sequence": int(record["source_sequence"]),
|
||||
"session_seconds": float(record["session_seconds"]),
|
||||
"point_count": int(points.shape[0]),
|
||||
"ground_point_count": int(np.count_nonzero(states == 1)),
|
||||
"nonground_point_count": int(np.count_nonzero(states == 2)),
|
||||
"rejected_point_count": int(np.count_nonzero(states == 3)),
|
||||
"ground_cell_count": int(np.count_nonzero(grid_state == 1)),
|
||||
"nonground_cell_count": int(np.count_nonzero(grid_state == 2)),
|
||||
"rejected_cell_count": int(np.count_nonzero(grid_state == 3)),
|
||||
"unobserved_cell_count": int(np.count_nonzero(grid_state == 0)),
|
||||
"all_points_accounted": bool(accounted),
|
||||
}
|
||||
)
|
||||
arrays[f"{profile_id}_points_xyz_m"] = np.concatenate(all_points)
|
||||
arrays[f"{profile_id}_point_states"] = np.concatenate(all_states)
|
||||
arrays[f"{profile_id}_point_offsets"] = np.asarray(offsets, dtype=np.int64)
|
||||
arrays[f"{profile_id}_costmap_states"] = np.stack(grid_states)
|
||||
arrays[f"{profile_id}_costmap_ground_point_counts"] = np.stack(ground_counts)
|
||||
arrays[f"{profile_id}_costmap_nonground_point_counts"] = np.stack(
|
||||
nonground_counts
|
||||
)
|
||||
arrays[f"{profile_id}_costmap_rejected_point_counts"] = np.stack(
|
||||
rejected_counts
|
||||
)
|
||||
arrays[f"{profile_id}_costmap_z_bounds_m"] = np.stack(z_bounds_rows)
|
||||
if not all(bool(row["all_points_accounted"]) for row in summaries):
|
||||
raise TgsEvidenceError("mixed-route TGS lost an eligible point")
|
||||
|
||||
output_root.mkdir(parents=True)
|
||||
evidence_path = output_root / "evidence.npz"
|
||||
write_deterministic_npz(evidence_path, arrays)
|
||||
timing = _timing(run_root / "tgs-timing.tsv")
|
||||
result = {
|
||||
"schema_version": RESULT_SCHEMA,
|
||||
"status": "passed-review-only",
|
||||
"source": {
|
||||
"source_id": source["source_id"],
|
||||
"session_id": source["session_id"],
|
||||
"review_pack_id": source["review_pack_id"],
|
||||
"source_pack_id": source["source_pack_id"],
|
||||
"source_pack_sha256": source["source_pack_sha256"],
|
||||
},
|
||||
"config_sha256": sha256_file(config_path),
|
||||
"input_manifest_sha256": sha256_file(input_manifest_path),
|
||||
"evidence": {
|
||||
"path": "evidence.npz",
|
||||
"bytes": evidence_path.stat().st_size,
|
||||
"sha256": sha256_file(evidence_path),
|
||||
},
|
||||
"costmap": {
|
||||
"coordinate_frame": "map-gravity-local",
|
||||
"cell_size_m": cell_size,
|
||||
"radius_m": radius,
|
||||
"cell_count": int(grid.shape[0]),
|
||||
},
|
||||
"anchors": summaries,
|
||||
"timing": timing,
|
||||
"summary": {
|
||||
"frame_count": FRAME_COUNT,
|
||||
"anchor_profile_count": len(summaries),
|
||||
"all_eligible_points_accounted": True,
|
||||
"aos_used": False,
|
||||
"primary_profile": "causal_rolling_1s",
|
||||
},
|
||||
"limitations": [
|
||||
"Selected review islands are not a complete route timeline.",
|
||||
(
|
||||
"TGS separates local ground support from non-ground evidence; it does not "
|
||||
"prove ditch or negative-obstacle detection."
|
||||
),
|
||||
"Camera projection is visual evidence only and cannot clear rigid geometry.",
|
||||
],
|
||||
"authority": {
|
||||
"visual_quality_accepted": False,
|
||||
"traversability_accepted": False,
|
||||
"realtime_accepted": False,
|
||||
"navigation_or_safety_accepted": False,
|
||||
"actuation_allowed": False,
|
||||
},
|
||||
}
|
||||
(output_root / "result.json").write_text(
|
||||
json.dumps(result, indent=2, sort_keys=True) + "\n",
|
||||
encoding="utf-8",
|
||||
)
|
||||
return result
|
||||
|
||||
|
||||
def main() -> int:
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument("--run-root", type=Path, required=True)
|
||||
parser.add_argument("--config", type=Path, required=True)
|
||||
parser.add_argument("--output-root", type=Path, required=True)
|
||||
args = parser.parse_args()
|
||||
result = build(args.run_root, args.config, args.output_root)
|
||||
print(json.dumps(result["summary"], sort_keys=True))
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,243 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Prepare exact mixed-route LiDAR islands for isolated TRAVEL/TGS review."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import hashlib
|
||||
import json
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
from prepare_tgs_fail_closed_inputs import TgsInputError, gravity_local_xyzi
|
||||
|
||||
CONFIG_SCHEMA = "missioncore.mixed-route-tgs-review-profile/v1"
|
||||
PACK_SCHEMA = "missioncore.mixed-route-lidar-pack/v1"
|
||||
INPUT_SCHEMA = "missioncore.mixed-route-tgs-input/v1"
|
||||
FRAME_COUNT = 10
|
||||
|
||||
|
||||
def sha256_file(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 _slice(points: np.ndarray, offsets: np.ndarray, index: int) -> np.ndarray:
|
||||
return points[int(offsets[index]) : int(offsets[index + 1])]
|
||||
|
||||
|
||||
def _validate_offsets(offsets: np.ndarray, point_count: int) -> bool:
|
||||
return bool(
|
||||
offsets.shape == (FRAME_COUNT + 1,)
|
||||
and offsets.dtype == np.int64
|
||||
and int(offsets[0]) == 0
|
||||
and int(offsets[-1]) == point_count
|
||||
and np.all(np.diff(offsets) > 0)
|
||||
)
|
||||
|
||||
|
||||
def prepare(source_root: Path, config_path: Path, output_root: Path) -> dict[str, object]:
|
||||
if output_root.exists():
|
||||
raise TgsInputError("mixed-route TGS output already exists")
|
||||
source = source_root.resolve(strict=True)
|
||||
config = json.loads(config_path.read_text(encoding="utf-8"))
|
||||
manifest = json.loads((source / "manifest.json").read_text(encoding="utf-8"))
|
||||
identity = manifest.get("identity") if isinstance(manifest, dict) else None
|
||||
artifact = manifest.get("artifact") if isinstance(manifest, dict) else None
|
||||
source_config = config.get("source") if isinstance(config, dict) else None
|
||||
invariants = config.get("invariants") if isinstance(config, dict) else None
|
||||
profiles = config.get("profiles") if isinstance(config, dict) else None
|
||||
if (
|
||||
config.get("schema_version") != CONFIG_SCHEMA
|
||||
or not isinstance(source_config, dict)
|
||||
or not isinstance(invariants, dict)
|
||||
or not isinstance(profiles, dict)
|
||||
or set(profiles) != {"current_increment", "causal_rolling_1s"}
|
||||
or source_config.get("input_coordinate_frame")
|
||||
!= "map-gravity-local-translation-only"
|
||||
or invariants.get("lidar_orientation_applied_to_tgs_input") is not False
|
||||
or invariants.get("future_frames_used") is not False
|
||||
or invariants.get("navigation_or_actuation_allowed") is not False
|
||||
or manifest.get("schema_version") != PACK_SCHEMA
|
||||
or not isinstance(identity, dict)
|
||||
or identity.get("schema_version") != PACK_SCHEMA
|
||||
or identity.get("session_id") != source_config.get("session_id")
|
||||
or identity.get("review_pack_id") != source_config.get("review_pack_id")
|
||||
or manifest.get("pack_id") != source_config.get("source_pack_id")
|
||||
or not isinstance(artifact, dict)
|
||||
or artifact.get("path") != "lidar-pack.npz"
|
||||
or artifact.get("sha256") != source_config.get("source_pack_sha256")
|
||||
or identity.get("frame_count") != FRAME_COUNT
|
||||
or identity.get("available_lidar_frames") != FRAME_COUNT
|
||||
or identity.get("causal_history_seconds")
|
||||
!= float(profiles["causal_rolling_1s"]["history_seconds"])
|
||||
or identity.get("ground_truth") is not False
|
||||
):
|
||||
raise TgsInputError("mixed-route TGS source contract changed")
|
||||
pack_path = source / "lidar-pack.npz"
|
||||
if (
|
||||
not pack_path.is_file()
|
||||
or pack_path.stat().st_size != artifact.get("byte_length")
|
||||
or sha256_file(pack_path) != artifact.get("sha256")
|
||||
):
|
||||
raise TgsInputError("mixed-route LiDAR pack changed")
|
||||
|
||||
required = {
|
||||
"frame_indices",
|
||||
"source_frame_indices",
|
||||
"session_seconds",
|
||||
"lidar_session_seconds",
|
||||
"sample_available",
|
||||
"cloud_offsets",
|
||||
"cloud_points_map",
|
||||
"pose_positions_map",
|
||||
"lidar_camera_delta_ms",
|
||||
"pose_point_delta_ms",
|
||||
"causal_history_seconds",
|
||||
"causal_history_offsets",
|
||||
"causal_history_points_map",
|
||||
}
|
||||
with np.load(pack_path, allow_pickle=False) as archive:
|
||||
if not required.issubset(archive.files):
|
||||
raise TgsInputError("mixed-route LiDAR pack members changed")
|
||||
arrays = {name: archive[name] for name in required}
|
||||
current_points = arrays["cloud_points_map"]
|
||||
history_points = arrays["causal_history_points_map"]
|
||||
if (
|
||||
arrays["frame_indices"].shape != (FRAME_COUNT,)
|
||||
or arrays["frame_indices"].dtype != np.int64
|
||||
or not np.array_equal(arrays["frame_indices"], np.arange(FRAME_COUNT))
|
||||
or arrays["source_frame_indices"].shape != (FRAME_COUNT,)
|
||||
or arrays["source_frame_indices"].dtype != np.int64
|
||||
or np.any(np.diff(arrays["source_frame_indices"]) <= 0)
|
||||
or arrays["session_seconds"].shape != (FRAME_COUNT,)
|
||||
or arrays["session_seconds"].dtype != np.float64
|
||||
or np.any(np.diff(arrays["session_seconds"]) <= 0)
|
||||
or arrays["lidar_session_seconds"].shape != (FRAME_COUNT,)
|
||||
or arrays["lidar_session_seconds"].dtype != np.float64
|
||||
or arrays["sample_available"].shape != (FRAME_COUNT,)
|
||||
or arrays["sample_available"].dtype != np.bool_
|
||||
or not arrays["sample_available"].all()
|
||||
or current_points.ndim != 2
|
||||
or current_points.shape[1:] != (3,)
|
||||
or current_points.dtype != np.float32
|
||||
or history_points.ndim != 2
|
||||
or history_points.shape[1:] != (3,)
|
||||
or history_points.dtype != np.float32
|
||||
or not np.isfinite(current_points).all()
|
||||
or not np.isfinite(history_points).all()
|
||||
or not _validate_offsets(arrays["cloud_offsets"], current_points.shape[0])
|
||||
or not _validate_offsets(
|
||||
arrays["causal_history_offsets"], history_points.shape[0]
|
||||
)
|
||||
or arrays["pose_positions_map"].shape != (FRAME_COUNT, 3)
|
||||
or arrays["pose_positions_map"].dtype != np.float64
|
||||
or not np.isfinite(arrays["pose_positions_map"]).all()
|
||||
or arrays["causal_history_seconds"].shape != (1,)
|
||||
or float(arrays["causal_history_seconds"][0])
|
||||
!= float(profiles["causal_rolling_1s"]["history_seconds"])
|
||||
or np.any(np.abs(arrays["lidar_camera_delta_ms"]) > 100.0)
|
||||
or np.any(np.abs(arrays["pose_point_delta_ms"]) > 100.0)
|
||||
):
|
||||
raise TgsInputError("mixed-route LiDAR arrays changed")
|
||||
|
||||
records: list[dict[str, object]] = []
|
||||
for profile_id in ("current_increment", "causal_rolling_1s"):
|
||||
for slot in range(FRAME_COUNT):
|
||||
if profile_id == "current_increment":
|
||||
points_map = _slice(
|
||||
current_points, arrays["cloud_offsets"], slot
|
||||
)
|
||||
else:
|
||||
points_map = _slice(
|
||||
history_points, arrays["causal_history_offsets"], slot
|
||||
)
|
||||
radius = float(profiles[profile_id]["local_radius_m"])
|
||||
relative_xy = (
|
||||
points_map[:, :2].astype(np.float64)
|
||||
- arrays["pose_positions_map"][slot, :2]
|
||||
)
|
||||
points_map = points_map[np.linalg.norm(relative_xy, axis=1) <= radius]
|
||||
native = gravity_local_xyzi(
|
||||
points_map, arrays["pose_positions_map"][slot]
|
||||
)
|
||||
if native.shape[0] == 0:
|
||||
raise TgsInputError("mixed-route TGS profile produced an empty cloud")
|
||||
target = (
|
||||
output_root
|
||||
/ "profiles"
|
||||
/ profile_id
|
||||
/ "velodyne"
|
||||
/ f"{slot:06d}.bin"
|
||||
)
|
||||
target.parent.mkdir(parents=True, exist_ok=True)
|
||||
target.write_bytes(np.ascontiguousarray(native).tobytes())
|
||||
records.append(
|
||||
{
|
||||
"profile_id": profile_id,
|
||||
"slot": slot,
|
||||
"frame_index": slot,
|
||||
"source_frame_index": int(
|
||||
arrays["source_frame_indices"][slot]
|
||||
),
|
||||
"source_sequence": int(
|
||||
arrays["source_frame_indices"][slot]
|
||||
)
|
||||
+ 1,
|
||||
"session_seconds": float(arrays["session_seconds"][slot]),
|
||||
"lidar_session_seconds": float(
|
||||
arrays["lidar_session_seconds"][slot]
|
||||
),
|
||||
"lidar_camera_delta_ms": float(
|
||||
arrays["lidar_camera_delta_ms"][slot]
|
||||
),
|
||||
"pose_point_delta_ms": float(
|
||||
arrays["pose_point_delta_ms"][slot]
|
||||
),
|
||||
"point_count": int(native.shape[0]),
|
||||
"relative_path": target.relative_to(output_root).as_posix(),
|
||||
"bytes": target.stat().st_size,
|
||||
"sha256": sha256_file(target),
|
||||
}
|
||||
)
|
||||
manifest_out = {
|
||||
"schema_version": INPUT_SCHEMA,
|
||||
"source_pack_id": manifest["pack_id"],
|
||||
"source_pack_sha256": artifact["sha256"],
|
||||
"config_sha256": sha256_file(config_path),
|
||||
"coordinate_frame": "map-gravity-local",
|
||||
"transform": "translation-only-preserve-map-gravity-axis",
|
||||
"intensity_policy": "zero-filled-algorithm-compatibility-only",
|
||||
"future_frames_used": False,
|
||||
"frame_count": FRAME_COUNT,
|
||||
"profile_count": 2,
|
||||
"records": records,
|
||||
"authority": {
|
||||
"navigation_or_safety_accepted": False,
|
||||
"actuation_allowed": False,
|
||||
},
|
||||
}
|
||||
manifest_path = output_root / "input-manifest.json"
|
||||
manifest_path.write_text(
|
||||
json.dumps(manifest_out, indent=2, sort_keys=True) + "\n",
|
||||
encoding="utf-8",
|
||||
)
|
||||
return manifest_out
|
||||
|
||||
|
||||
def main() -> int:
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument("--source-root", type=Path, required=True)
|
||||
parser.add_argument("--config", type=Path, required=True)
|
||||
parser.add_argument("--output-root", type=Path, required=True)
|
||||
args = parser.parse_args()
|
||||
manifest = prepare(args.source_root, args.config, args.output_root)
|
||||
print(json.dumps({"ok": True, "records": len(manifest["records"])}, sort_keys=True))
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
Reference in New Issue
Block a user