feat(perception): add mixed-route vegetation review
This commit is contained in:
DCCONSTRUCTIONS
+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