feat: finalize corrected-route planning and Rerun recording review
This commit is contained in:
DCCONSTRUCTIONS
@@ -0,0 +1,33 @@
|
||||
"""Admit a reviewed full map as the physical session's operator/LAB default.
|
||||
|
||||
Explicit offline maintenance command; no solver, no device commands, no new
|
||||
catalog session and no change to raw capture or already pinned studies.
|
||||
"""
|
||||
|
||||
import argparse
|
||||
from pathlib import Path
|
||||
|
||||
from package_recorded_map_version import RecordedParent
|
||||
|
||||
from k1link.reconstruction.map_version import MapVersion
|
||||
from k1link.reconstruction.session_versions import SessionMapVersions
|
||||
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--version", type=Path, required=True)
|
||||
parser.add_argument("--raw", type=Path, required=True)
|
||||
parser.add_argument("--data-dir", type=Path, required=True)
|
||||
args = parser.parse_args()
|
||||
version = MapVersion(args.version, args.version.name)
|
||||
parent = version.document["source"]
|
||||
original = RecordedParent(args.raw, parent["session_id"], parent["generation"])
|
||||
admitted = SessionMapVersions(args.data_dir).activate(version, original)
|
||||
print(
|
||||
f"Session: {parent['session_id']}\nDefault map: {admitted.generation}\n"
|
||||
"Uses: recorded playback, laboratory reference. Vehicle control: false."
|
||||
)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,40 @@
|
||||
"""Read-only, chunk-bounded check of the actual point rows in a derived RRD."""
|
||||
|
||||
import argparse
|
||||
import json
|
||||
from pathlib import Path
|
||||
|
||||
import pyarrow.compute as pc
|
||||
from rerun.experimental import RrdReader
|
||||
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("recording", type=Path)
|
||||
parser.add_argument("--expected-frames", type=int, required=True)
|
||||
parser.add_argument("--expected-points", type=int, required=True)
|
||||
args = parser.parse_args()
|
||||
frames = points = 0
|
||||
for chunk in RrdReader(args.recording).stream():
|
||||
if chunk.entity_path != "/world/points":
|
||||
continue
|
||||
batch = chunk.to_record_batch()
|
||||
if "Points3D:positions" not in batch.schema.names:
|
||||
continue
|
||||
positions = batch.column("Points3D:positions")
|
||||
frames += len(positions) - positions.null_count
|
||||
points += pc.sum(pc.list_value_length(positions)).as_py() or 0
|
||||
result = {
|
||||
"recording": str(args.recording),
|
||||
"point_frames": frames,
|
||||
"points": points,
|
||||
"expected_point_frames": args.expected_frames,
|
||||
"expected_points": args.expected_points,
|
||||
"passed": frames == args.expected_frames and points == args.expected_points,
|
||||
}
|
||||
print(json.dumps(result, indent=2))
|
||||
return 0 if result["passed"] else 1
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,310 @@
|
||||
"""Package the reviewed ring derivative for explicit offline planning consumption.
|
||||
|
||||
Reads the canonical source API, but never writes to it or imports the web app.
|
||||
Verifies the raw transport, clocks, reviewed code/artifacts and all corrected
|
||||
frames against the frozen correction field before publishing a separate bundle.
|
||||
No refit, capture, reference switch, source-catalog entry or vehicle authority.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import time
|
||||
from pathlib import Path
|
||||
from tempfile import TemporaryDirectory
|
||||
from urllib.parse import quote
|
||||
from urllib.request import urlopen
|
||||
|
||||
import numpy as np
|
||||
|
||||
from k1link.missions.versioned_sources import VersionedPlanningSources
|
||||
from k1link.reconstruction.map_version import publish_map_version, sha256
|
||||
from k1link.reconstruction.smooth_correction import CorrectionField
|
||||
|
||||
|
||||
class RecordedParent:
|
||||
"""A read-only generation-bound parent; all native clock artifacts are checked."""
|
||||
|
||||
def __init__(self, raw, session_id, generation):
|
||||
self.raw, self.session_id, self.generation = raw, session_id, generation
|
||||
|
||||
def get(self, session_id):
|
||||
if session_id != self.session_id:
|
||||
raise ValueError("Unexpected source session.")
|
||||
url = (
|
||||
"http://127.0.0.1:8000/api/v1/mission-planner/sources/"
|
||||
+ quote(session_id, safe="")
|
||||
+ "?generation="
|
||||
+ quote(self.generation, safe="")
|
||||
)
|
||||
with urlopen(url, timeout=30) as response:
|
||||
source = json.load(response)
|
||||
if source["generation"] != self.generation:
|
||||
raise ValueError("Original source generation changed.")
|
||||
return source
|
||||
|
||||
def verify(self, session_id, generation):
|
||||
if generation != self.generation:
|
||||
raise ValueError("Original source generation changed.")
|
||||
source = self.get(session_id)
|
||||
# Plugin-specific recorded-ring adapter, not a generic Core discovery rule.
|
||||
expected = source["source_digests"]
|
||||
if expected.get("raw-transport-primary") != sha256(self.raw):
|
||||
raise ValueError("Original transport identity changed.")
|
||||
index = self.raw.with_name("mqtt.metadata.jsonl")
|
||||
origin = self.raw.with_name("mqtt.timeline.origin.json")
|
||||
if sha256(index) != expected.get("raw-transport-index") or sha256(origin) != expected.get(
|
||||
"raw-transport-clock-origin"
|
||||
):
|
||||
raise ValueError("Original receipt clocks changed.")
|
||||
clocks = [
|
||||
self.raw.with_name("mqtt.timeline.json"),
|
||||
self.raw.with_name(
|
||||
"mqtt.timeline.session-" + expected["raw-transport-clock"] + ".json"
|
||||
),
|
||||
]
|
||||
if not any(
|
||||
p.is_file() and sha256(p) == expected.get("raw-transport-clock") for p in clocks
|
||||
):
|
||||
raise ValueError("Original capture clock changed.")
|
||||
if self.get(session_id) != source:
|
||||
raise ValueError("Original source changed during verification.")
|
||||
return source
|
||||
|
||||
bound = verify
|
||||
|
||||
|
||||
def check_closure_review(result, summary):
|
||||
"""New producers must carry a complete acquisition and a positive frozen review.
|
||||
|
||||
Retain explicit v1 compatibility for previously reviewed/pinned bundles.
|
||||
A missing v2 review cannot silently fall back to the legacy contract.
|
||||
"""
|
||||
schema = summary.get("schema_version")
|
||||
if schema == "missioncore.recorded-ring-experiment/v1":
|
||||
return []
|
||||
if schema != "missioncore.recorded-ring-experiment/v2":
|
||||
raise ValueError("Unsupported closure producer contract.")
|
||||
search = json.loads((result / "closure-search.json").read_text())
|
||||
review = json.loads((result / "review.json").read_text())
|
||||
acceptance = review.get("acceptance", {})
|
||||
required = {
|
||||
"all_local_windows_qualified",
|
||||
"heldout_seam_present",
|
||||
"heldout_seam_quality",
|
||||
"heldout_seam_not_degraded",
|
||||
}
|
||||
if (
|
||||
search.get("status") != "candidate"
|
||||
or search.get("complete") is not True
|
||||
or len(search.get("attempts", [])) != search.get("expected_attempts")
|
||||
or acceptance.get("schema_version") != "missioncore.closure-review/v1"
|
||||
or acceptance.get("accepted") is not True
|
||||
or set(acceptance.get("checks", {})) != required
|
||||
or any(acceptance["checks"][key] is not True for key in required)
|
||||
or summary.get("closure_acquisition") != "candidate"
|
||||
):
|
||||
raise ValueError("Closure acquisition or held-out review is not qualified.")
|
||||
return ["closure-search.json"]
|
||||
|
||||
|
||||
def prepare(args):
|
||||
started = time.monotonic()
|
||||
result = args.result.resolve()
|
||||
seal_path = result / "review.json.seal.json"
|
||||
if sha256(seal_path) != args.review_seal_sha256:
|
||||
raise ValueError("Reviewed evidence seal identity differs.")
|
||||
sealed = json.loads(seal_path.read_text())
|
||||
for path, expected in sealed.items():
|
||||
if sha256(Path(path)) != expected:
|
||||
raise ValueError("Reviewed artifact or producer changed: " + Path(path).name)
|
||||
names = [
|
||||
"summary.json",
|
||||
"correction.json",
|
||||
"validation.json",
|
||||
"registrations.json",
|
||||
"surface-links.json",
|
||||
"review.json",
|
||||
"corrected-points.f32",
|
||||
"corrected-trajectory.npz",
|
||||
]
|
||||
summary = json.loads((result / "summary.json").read_text())
|
||||
additional_evidence = check_closure_review(result, summary)
|
||||
if any(str(result / name) not in sealed for name in names + additional_evidence):
|
||||
raise ValueError("Review seal does not bind the complete candidate.")
|
||||
if (
|
||||
summary["status"] != "experimental-candidate-not-promoted"
|
||||
or summary["production_promotion"]
|
||||
or summary["vehicle_control"]
|
||||
):
|
||||
raise ValueError("Unsupported experiment contract or authority.")
|
||||
parent = RecordedParent(args.raw, summary["source_session"], args.source_generation)
|
||||
source = parent.verify(summary["source_session"], args.source_generation)
|
||||
if sha256(args.raw) != summary["source"]["source_sha256"]:
|
||||
raise ValueError("Experiment belongs to another physical recording.")
|
||||
correction = json.loads((result / "correction.json").read_text())
|
||||
if (
|
||||
correction["schema_version"] != "missioncore.smooth-map-correction/v2"
|
||||
or not correction["converged"]
|
||||
):
|
||||
raise ValueError("A converged, reviewed v2 correction is required.")
|
||||
field = CorrectionField(correction["knots_m"], correction["parameters"], correction["origin_m"])
|
||||
cache = args.cache.resolve()
|
||||
cache_seal = json.loads((cache / "seal.json").read_text())
|
||||
for name in ("source-points.f32", "source-intensity.u8", "index.npz", "source.json"):
|
||||
if sha256(cache / name) != cache_seal[name]:
|
||||
raise ValueError("Decoded source cache changed.")
|
||||
if json.loads((cache / "source.json").read_text()) != summary["source"]:
|
||||
raise ValueError("Decoded cache belongs to another experiment source.")
|
||||
with np.load(cache / "index.npz", allow_pickle=False) as original:
|
||||
poses, frames = original["poses"], original["frames"]
|
||||
distances, frame_distances = original["distance"], original["frame_distance"]
|
||||
with np.load(result / "corrected-trajectory.npz", allow_pickle=False) as data:
|
||||
trajectory = {k: data[k] for k in data.files}
|
||||
if (
|
||||
len(poses) != len(source["poses"])
|
||||
or not np.allclose(
|
||||
poses[:, 1:4], [p["position"] for p in source["poses"]], atol=1e-10, rtol=0
|
||||
)
|
||||
or not np.allclose(
|
||||
poses[:, 0] - poses[0, 0], [p["elapsed_s"] for p in source["poses"]], atol=1e-6, rtol=0
|
||||
)
|
||||
):
|
||||
raise ValueError("Source trajectory or clock ownership differs from the reviewed cache.")
|
||||
positions, orientations = field.poses(poses[:, 1:4], poses[:, 4:8], distances)
|
||||
if (
|
||||
not np.allclose(positions, trajectory["positions"], atol=1e-9, rtol=0)
|
||||
or not np.allclose(orientations, trajectory["orientations_xyzw"], atol=1e-9, rtol=0)
|
||||
or not np.array_equal(frames, trajectory["frames"])
|
||||
or not np.array_equal(poses[:, 0], trajectory["receipt_time_s"])
|
||||
or not np.array_equal(distances, trajectory["distance_m"])
|
||||
or not np.array_equal(frame_distances, trajectory["frame_distance_m"])
|
||||
):
|
||||
raise ValueError("Corrected poses/frames do not reproduce the reviewed field.")
|
||||
maximum_error = 0.0
|
||||
with (
|
||||
(cache / "source-points.f32").open("rb") as raw_points,
|
||||
(result / "corrected-points.f32").open("rb") as corrected,
|
||||
):
|
||||
for frame, distance in zip(frames, frame_distances, strict=True):
|
||||
count = int(frame[3])
|
||||
original = np.frombuffer(raw_points.read(count * 12), dtype="<f4").reshape(-1, 3)
|
||||
actual = np.frombuffer(corrected.read(count * 12), dtype="<f4").reshape(-1, 3)
|
||||
expected = field.points(original, distance).astype("<f4")
|
||||
if not np.array_equal(expected, actual):
|
||||
raise ValueError("Corrected full-resolution frame differs from its frozen field.")
|
||||
maximum_error = max(
|
||||
maximum_error, float(np.max(abs(actual - field.points(original, distance))))
|
||||
)
|
||||
if raw_points.read(1) or corrected.read(1):
|
||||
raise ValueError("Unindexed points remain after complete frame verification.")
|
||||
print(f"Verified {len(frames)} full-resolution frames and {len(poses)} poses.", flush=True)
|
||||
args.output.mkdir(parents=True, exist_ok=True)
|
||||
with TemporaryDirectory(prefix=".package-", dir=args.output) as temporary:
|
||||
stage = Path(temporary)
|
||||
normalized = stage / "trajectory.npz"
|
||||
np.savez(
|
||||
normalized,
|
||||
positions=positions,
|
||||
orientations_xyzw=orientations,
|
||||
receipt_time_s=poses[:, 0],
|
||||
source_distance_m=distances,
|
||||
frame_source_distance_m=frame_distances,
|
||||
frames=frames,
|
||||
)
|
||||
# Preserve measured reports byte-for-byte; the normalized trajectory is new.
|
||||
evidence = {
|
||||
name: (result / name, sealed[str(result / name)])
|
||||
for name in names[:6] + additional_evidence
|
||||
}
|
||||
evidence["source-intensity.u8"] = (
|
||||
cache / "source-intensity.u8",
|
||||
cache_seal["source-intensity.u8"],
|
||||
)
|
||||
producers = {
|
||||
Path(path).name: digest for path, digest in sealed.items() if path.endswith(".py")
|
||||
}
|
||||
receipt = dict(
|
||||
schema_version="missioncore.map-version-packaging-check/v1",
|
||||
review_seal_sha256=args.review_seal_sha256,
|
||||
source=source["source_digests"],
|
||||
checked_frames=len(frames),
|
||||
checked_poses=len(poses),
|
||||
full_resolution_frames_equal=True,
|
||||
maximum_float32_error_m=maximum_error,
|
||||
producer_sha256=sha256(Path(__file__)),
|
||||
bundle_contract_sha256=sha256(
|
||||
Path(__file__).parents[1] / "src/k1link/reconstruction/map_version.py"
|
||||
),
|
||||
source_cache_seal_sha256=sha256(cache / "seal.json"),
|
||||
)
|
||||
(stage / "packaging-check.json").write_text(json.dumps(receipt, indent=2, allow_nan=False))
|
||||
evidence["packaging-check.json"] = (
|
||||
stage / "packaging-check.json",
|
||||
sha256(stage / "packaging-check.json"),
|
||||
)
|
||||
version = publish_map_version(
|
||||
args.output,
|
||||
source,
|
||||
result / "corrected-points.f32",
|
||||
normalized,
|
||||
expected_points_sha256=sealed[str(result / "corrected-points.f32")],
|
||||
expected_trajectory_sha256=sha256(normalized),
|
||||
evidence=evidence,
|
||||
method=dict(
|
||||
algorithm=correction["policy"],
|
||||
producer_sha256=producers,
|
||||
review_seal_sha256=args.review_seal_sha256,
|
||||
),
|
||||
label=source["label"] + " · коррекция v2",
|
||||
)
|
||||
parent.verify(source["session_id"], source["generation"])
|
||||
for path, expected in sealed.items():
|
||||
if sha256(Path(path)) != expected:
|
||||
raise ValueError("Experiment changed while packaging; do not consume this candidate.")
|
||||
if args.check_map:
|
||||
adapter = VersionedPlanningSources(parent, version, args.output / "scratch")
|
||||
doc = adapter.bound(source["session_id"], version.generation)
|
||||
cloud, provenance = adapter.reference_map(
|
||||
source["session_id"], version.generation, 0, len(doc["poses"]) - 1
|
||||
)
|
||||
print(
|
||||
json.dumps(
|
||||
dict(
|
||||
map_points=len(cloud),
|
||||
tiles=len(provenance["tiles"]),
|
||||
corrected_path_m=doc["path_m"],
|
||||
source_path_m=source["path_m"],
|
||||
)
|
||||
),
|
||||
flush=True,
|
||||
)
|
||||
version.verify()
|
||||
print(
|
||||
json.dumps(
|
||||
dict(
|
||||
version_sha256=version.generation,
|
||||
directory=str(version.directory),
|
||||
elapsed_s=time.monotonic() - started,
|
||||
runtime_promoted=False,
|
||||
)
|
||||
),
|
||||
flush=True,
|
||||
)
|
||||
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--result", required=True, type=Path)
|
||||
parser.add_argument("--review-seal-sha256", required=True)
|
||||
parser.add_argument("--source-generation", required=True)
|
||||
parser.add_argument("--raw", required=True, type=Path)
|
||||
parser.add_argument("--cache", required=True, type=Path)
|
||||
parser.add_argument("--output", required=True, type=Path)
|
||||
parser.add_argument("--check-map", action="store_true")
|
||||
prepare(parser.parse_args())
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,119 @@
|
||||
"""Attach a source-bound paired preview to an existing session overview.
|
||||
|
||||
No fitting, raw/session writes, planner selection or map promotion. The runtime
|
||||
reads the resulting small vendor-neutral pair, never experiment cache paths.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import json
|
||||
from pathlib import Path
|
||||
from urllib.parse import quote
|
||||
from urllib.request import urlopen
|
||||
|
||||
import numpy as np
|
||||
from package_recorded_map_version import RecordedParent
|
||||
|
||||
from k1link.reconstruction.map_version import MapVersion, sha256
|
||||
from k1link.sessions.overview_comparison import MAX_POINTS, MAX_POSES, publish_comparison
|
||||
|
||||
|
||||
def publish(args):
|
||||
version = MapVersion(args.version, args.version.name)
|
||||
doc = version.verify()
|
||||
source_id = doc["source"]
|
||||
parent = RecordedParent(args.raw, source_id["session_id"], source_id["generation"])
|
||||
source = parent.verify(source_id["session_id"], source_id["generation"])
|
||||
if source["source_digests"] != source_id["source_digests"]:
|
||||
raise ValueError("Map source identity differs.")
|
||||
cache = args.cache
|
||||
check = json.loads((version.directory / "packaging-check.json").read_text())
|
||||
seal_path = cache / "seal.json"
|
||||
if sha256(seal_path) != check["source_cache_seal_sha256"]:
|
||||
raise ValueError("Decoded source cache seal differs from admitted map.")
|
||||
sealed = json.loads(seal_path.read_text())
|
||||
|
||||
def verify_cache():
|
||||
for name in ("source.json", "source-points.f32", "index.npz"):
|
||||
if sha256(cache / name) != sealed[name]:
|
||||
raise ValueError("Decoded source cache changed: " + name)
|
||||
|
||||
verify_cache()
|
||||
original_doc = json.loads((cache / "source.json").read_text())
|
||||
if original_doc["source_sha256"] != source_id["source_digests"]["raw-transport-primary"]:
|
||||
raise ValueError("Decoded source belongs to another recording.")
|
||||
points = np.memmap(cache / "source-points.f32", dtype="<f4", mode="r").reshape(-1, 3)
|
||||
corrected = np.memmap(version.directory / "points.f32", dtype="<f4", mode="r").reshape(-1, 3)
|
||||
if points.shape != corrected.shape or len(points) != doc["point_count"]:
|
||||
raise ValueError("Point correspondence differs.")
|
||||
arrays = version.arrays()
|
||||
with np.load(cache / "index.npz", allow_pickle=False) as data:
|
||||
poses = data["poses"]
|
||||
if (
|
||||
not np.array_equal(data["frames"], arrays["frames"])
|
||||
or not np.array_equal(poses[:, 0], arrays["receipt_time_s"])
|
||||
or not np.allclose(
|
||||
poses[:, 1:4], [p["position"] for p in source["poses"]], rtol=0, atol=1e-10
|
||||
)
|
||||
):
|
||||
raise ValueError("Source observation ownership differs.")
|
||||
point_indices = np.linspace(0, len(points) - 1, min(MAX_POINTS, len(points)), dtype=np.int64)
|
||||
pose_indices = np.linspace(0, len(poses) - 1, min(MAX_POSES, len(poses)), dtype=np.int64)
|
||||
pair = dict(
|
||||
original=np.array(points[point_indices]),
|
||||
corrected=np.array(corrected[point_indices]),
|
||||
original_route=poses[pose_indices, 1:4],
|
||||
corrected_route=arrays["positions"][pose_indices],
|
||||
)
|
||||
del points, corrected
|
||||
url = (
|
||||
"http://127.0.0.1:8000/api/v1/observation-sessions/"
|
||||
+ quote(source_id["session_id"], safe="")
|
||||
+ "/overview"
|
||||
)
|
||||
with urlopen(url, timeout=30) as response:
|
||||
overview = json.load(response)
|
||||
if overview["state"] != "ready":
|
||||
raise ValueError("Prepare the existing source overview before attaching a comparison.")
|
||||
generation = overview["generation"]
|
||||
report = json.loads(
|
||||
(args.data_dir / "session-overviews" / generation / "overview.json").read_text()
|
||||
)
|
||||
if report["source_digests"] != source_id["source_digests"]:
|
||||
raise ValueError("Overview source differs from map source.")
|
||||
# Recheck every mutable input after sampling, before atomic view-only publication.
|
||||
verify_cache()
|
||||
version.verify()
|
||||
if parent.verify(source_id["session_id"], source_id["generation"]) != source:
|
||||
raise ValueError("Source changed during preparation.")
|
||||
preview = publish_comparison(
|
||||
args.data_dir / "session-map-previews",
|
||||
session_id=source_id["session_id"],
|
||||
overview_generation=generation,
|
||||
source_digests=source_id["source_digests"],
|
||||
map_generation=version.generation,
|
||||
source_points=doc["point_count"],
|
||||
original_path_m=source["path_m"],
|
||||
corrected_path_m=doc["path_m"],
|
||||
**pair,
|
||||
)
|
||||
print(
|
||||
json.dumps(
|
||||
dict(
|
||||
overview_generation=generation,
|
||||
comparison_generation=preview,
|
||||
sampled_points=len(point_indices),
|
||||
sampled_poses=len(pose_indices),
|
||||
view_only=True,
|
||||
),
|
||||
indent=2,
|
||||
)
|
||||
)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
for name in ("version", "cache", "raw", "data-dir"):
|
||||
parser.add_argument("--" + name, type=Path, required=True)
|
||||
publish(parser.parse_args())
|
||||
@@ -0,0 +1,452 @@
|
||||
"""Offline first-loop experiment on an immutable K1 recording, never an API action.
|
||||
|
||||
Run with the project's map-correction extra. All outputs are private derivatives;
|
||||
the caller supplies a fresh output directory, source session identity and digest.
|
||||
No raw overwrites, scene publication, hardware commands or planner mutations.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import hashlib
|
||||
import json
|
||||
import time
|
||||
from dataclasses import asdict
|
||||
from datetime import UTC, datetime
|
||||
from importlib.metadata import version
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
from scipy.spatial import cKDTree
|
||||
from scipy.spatial.transform import Rotation
|
||||
|
||||
from k1link.device_plugins.xgrids_k1.protocol.streams import decode_lio_pcl, decode_lio_pose
|
||||
from k1link.device_plugins.xgrids_k1.viewer.replay import iter_replay_messages
|
||||
from k1link.missions.registration import POLICY, PreparedReference, transform
|
||||
from k1link.reconstruction.closure import ClosurePolicy, ClosureUnavailable, acquire_closure
|
||||
from k1link.reconstruction.smooth_correction import (
|
||||
CorrectionField,
|
||||
CorrectionPolicy,
|
||||
SurfaceLink,
|
||||
fit_correction,
|
||||
)
|
||||
|
||||
PROFILE = dict(
|
||||
version="recorded-ring-experiment/v2",
|
||||
sample_stride=8,
|
||||
holdout_period_s=10.0,
|
||||
holdout_start_s=4.0,
|
||||
holdout_duration_s=2.0,
|
||||
seam_reference_s=20.0,
|
||||
seam_query_s=5.0,
|
||||
seam_radius_m=25.0,
|
||||
local_validation_radius_m=40.0,
|
||||
neighbor_radius_m=30.0,
|
||||
voxel_m=0.25,
|
||||
seam_translation_weight_m=0.03,
|
||||
seam_rotation_weight_deg=0.05,
|
||||
neighbor_translation_weight_m=0.2,
|
||||
neighbor_rotation_weight_deg=0.5,
|
||||
)
|
||||
|
||||
# A separate first-fit policy, never a mutation of the live tracking policy.
|
||||
# Quality/shape/information/correspondence gates remain identical.
|
||||
CLOSURE_REGISTRATION_POLICY = {
|
||||
**POLICY,
|
||||
"version": "offline-closure-gicp/v1",
|
||||
"maximum_correction_m": 25.0,
|
||||
"maximum_correction_deg": 180.0,
|
||||
}
|
||||
DECODE_KEYS = ("sample_stride", "holdout_period_s", "holdout_start_s", "holdout_duration_s")
|
||||
|
||||
|
||||
def compatible_cache_profile(profile):
|
||||
# Earlier sealed caches include fitting settings, though decoding never uses
|
||||
# them. Reuse is safe only when every actual decode/split setting agrees.
|
||||
return all(profile.get(key) == PROFILE[key] for key in DECODE_KEYS)
|
||||
|
||||
|
||||
def digest(path):
|
||||
with path.open("rb") as stream:
|
||||
return hashlib.file_digest(stream, "sha256").hexdigest()
|
||||
|
||||
|
||||
def write_json(path, value):
|
||||
with path.open("x") as stream:
|
||||
json.dump(value, stream, indent=2, allow_nan=False)
|
||||
|
||||
|
||||
def extract(raw, output, expected):
|
||||
if digest(raw) != expected:
|
||||
raise ValueError("Source digest mismatch before decoding.")
|
||||
output.mkdir() # Exclusive new directory, never replace an earlier experiment.
|
||||
started = time.monotonic()
|
||||
poses, frames, samples, sample_ids = [], [], [], []
|
||||
first = None
|
||||
total = 0
|
||||
sequences = {"lio_pose": [], "lio_pcl": []}
|
||||
with (
|
||||
(output / "source-points.f32").open("xb") as points_file,
|
||||
(output / "source-intensity.u8").open("xb") as intensity_file,
|
||||
):
|
||||
for message in iter_replay_messages(raw):
|
||||
if message.received_monotonic_ns is None:
|
||||
raise ValueError("Source has no monotonic receipt timestamp.")
|
||||
clock = message.received_monotonic_ns / 1e9
|
||||
if first is None:
|
||||
first = clock
|
||||
t = clock - first
|
||||
if message.topic.endswith("/lio_pose"):
|
||||
pose = decode_lio_pose(message.payload)
|
||||
sequences["lio_pose"].append(pose.header.seq)
|
||||
poses.append(
|
||||
[
|
||||
t,
|
||||
*pose.position_xyz,
|
||||
*pose.orientation_xyzw,
|
||||
pose.pose_stamp,
|
||||
pose.header.seq,
|
||||
]
|
||||
)
|
||||
elif message.topic.endswith("/lio_pcl"):
|
||||
cloud = decode_lio_pcl(message.payload)
|
||||
sequences["lio_pcl"].append(cloud.header.seq)
|
||||
data = np.asarray(cloud.points, dtype=np.int64).reshape(-1, 4)
|
||||
xyz = (data[:, :3] / cloud.header.scaler).astype("<f4")
|
||||
if not np.isfinite(xyz).all():
|
||||
raise ValueError("Non-finite source geometry.")
|
||||
xyz.tofile(points_file)
|
||||
(data[:, 3] & 255).astype("u1").tofile(intensity_file)
|
||||
sampled = xyz[:: PROFILE["sample_stride"]]
|
||||
samples.append(sampled)
|
||||
sample_ids.append(np.full(len(sampled), len(frames), dtype=np.int32))
|
||||
frames.append([t, cloud.header.seq, total, len(xyz)])
|
||||
total += len(xyz)
|
||||
if len(frames) % 500 == 0:
|
||||
print(f"decode {len(frames)} frames, {total} points", flush=True)
|
||||
p, f = np.asarray(poses), np.asarray(frames)
|
||||
if min(len(p), len(f)) < 2 or (np.diff(p[:, 0]) <= 0).any():
|
||||
raise ValueError("Missing or unordered source trajectory.")
|
||||
if (np.diff(f[:, 0]) < 0).any():
|
||||
raise ValueError("Cloud receipt clock moved backwards.")
|
||||
if any((np.diff(seq) != 1).any() for seq in sequences.values()):
|
||||
raise ValueError("Source sequence gaps or resets require a separate review.")
|
||||
distance = np.r_[0.0, np.cumsum(np.linalg.norm(np.diff(p[:, 1:4], axis=0), axis=1))]
|
||||
frame_distance = np.interp(f[:, 0], p[:, 0], distance)
|
||||
elapsed = f[:, 0] - f[0, 0]
|
||||
phase = elapsed % PROFILE["holdout_period_s"]
|
||||
held = (phase >= PROFILE["holdout_start_s"]) & (
|
||||
phase < PROFILE["holdout_start_s"] + PROFILE["holdout_duration_s"]
|
||||
)
|
||||
nearest = np.clip(np.searchsorted(p[:, 0], f[:, 0]), 1, len(p) - 1)
|
||||
gap = np.minimum(abs(p[nearest, 0] - f[:, 0]), abs(p[nearest - 1, 0] - f[:, 0]))
|
||||
if digest(raw) != expected:
|
||||
raise ValueError("Source changed while decoding; derivative is not admissible.")
|
||||
np.savez(
|
||||
output / "index.npz",
|
||||
poses=p,
|
||||
frames=f,
|
||||
distance=distance,
|
||||
frame_distance=frame_distance,
|
||||
heldout=held,
|
||||
sample_points=np.concatenate(samples),
|
||||
sample_frame=np.concatenate(sample_ids),
|
||||
)
|
||||
meta = dict(
|
||||
source_sha256=expected,
|
||||
profile=PROFILE,
|
||||
frames=len(f),
|
||||
poses=len(p),
|
||||
points=total,
|
||||
seconds=time.monotonic() - started,
|
||||
path_m=float(distance[-1]),
|
||||
receipt_pose_nearest_gap_p95_s=float(np.quantile(gap, 0.95)),
|
||||
receipt_pose_nearest_gap_max_s=float(gap.max()),
|
||||
clock_binding="host-monotonic interpolation; NOT hardware synchronization",
|
||||
mapped_increment_not_native_sweep=True,
|
||||
heldout_frames=int(held.sum()),
|
||||
training_frames=int((~held).sum()),
|
||||
)
|
||||
write_json(output / "source.json", meta)
|
||||
return meta
|
||||
|
||||
|
||||
def voxel(points):
|
||||
if not len(points):
|
||||
return points
|
||||
_, idx = np.unique(
|
||||
np.floor(points / PROFILE["voxel_m"]).astype(np.int64), axis=0, return_index=True
|
||||
)
|
||||
return points[np.sort(idx)]
|
||||
|
||||
|
||||
def register(reference, query, seed, *, acquisition=False):
|
||||
try:
|
||||
result = PreparedReference(reference).register(
|
||||
query, seed, policy=CLOSURE_REGISTRATION_POLICY if acquisition else POLICY
|
||||
)
|
||||
result.pop("matched_query_indices", None)
|
||||
return result
|
||||
except ValueError as exc:
|
||||
return dict(status="unavailable", reasons=[str(exc)])
|
||||
|
||||
|
||||
def links_for(data):
|
||||
p, s = data["poses"], data["frame_distance"]
|
||||
pts, ids, held = data["sample_points"], data["sample_frame"], data["heldout"]
|
||||
training = ~held[ids]
|
||||
|
||||
def take(frame_mask, center, radius):
|
||||
mask = training & frame_mask[ids]
|
||||
chunk = pts[mask]
|
||||
return chunk[np.linalg.norm(chunk - center, axis=1) <= radius]
|
||||
|
||||
link, search = acquire_closure(data, register)
|
||||
selected = search["attempts"][search["selected_attempt"]]
|
||||
links = [link]
|
||||
audits = [dict(kind="seam", **selected, search=search)]
|
||||
# Disjoint time/distance windows: no shared frame can self-match across an edge.
|
||||
centers = np.linspace(0, data["distance"][-1], int(np.ceil(data["distance"][-1] / 20)) + 1)
|
||||
for i, (sa, sb) in enumerate(zip(centers[:-1], centers[1:], strict=True)):
|
||||
width = (sb - sa) / 3
|
||||
amask, bmask = abs(s - sa) <= width, abs(s - sb) <= width
|
||||
assert not np.any(amask & bmask)
|
||||
pivot = np.array([np.interp((sa + sb) / 2, data["distance"], p[:, j]) for j in range(1, 4)])
|
||||
a = take(amask, pivot, PROFILE["neighbor_radius_m"])
|
||||
b = take(bmask, pivot, PROFILE["neighbor_radius_m"])
|
||||
fit = register(a, b, np.eye(4))
|
||||
audits.append(dict(kind="neighbor", distances_m=[float(sa), float(sb)], fit=fit))
|
||||
if fit["status"] == "candidate":
|
||||
links.append(
|
||||
SurfaceLink(
|
||||
float(np.mean(s[amask & ~held])),
|
||||
float(np.mean(s[bmask & ~held])),
|
||||
np.asarray(fit["T_reference_query"]),
|
||||
np.median(b, axis=0),
|
||||
PROFILE["neighbor_translation_weight_m"],
|
||||
PROFILE["neighbor_rotation_weight_deg"],
|
||||
f"neighbor-{i}",
|
||||
)
|
||||
)
|
||||
print(f"neighbor {i + 1}/{len(centers) - 1}: {fit['status']}", flush=True)
|
||||
return links, audits
|
||||
|
||||
|
||||
def evaluate(data, field, label):
|
||||
pts, ids = data["sample_points"], data["sample_frame"]
|
||||
s, f, p = data["frame_distance"], data["frames"], data["poses"]
|
||||
train = ~data["heldout"][ids]
|
||||
corrected = np.empty_like(pts)
|
||||
offsets = np.searchsorted(ids, np.arange(len(f) + 1))
|
||||
for i, distance in enumerate(s):
|
||||
start, end = offsets[i : i + 2]
|
||||
corrected[start:end] = field.points(pts[start:end], distance)
|
||||
target = voxel(corrected[train])
|
||||
tree = cKDTree(target)
|
||||
groups = np.floor((f[:, 0] - f[0, 0]) / PROFILE["holdout_period_s"]).astype(int)
|
||||
seed, rows = np.eye(4), []
|
||||
for group in np.unique(groups[data["heldout"]]):
|
||||
frames = data["heldout"] & (groups == group)
|
||||
seconds, distance = float(np.mean(f[frames, 0])), float(np.mean(s[frames]))
|
||||
position = np.array([np.interp(seconds, p[:, 0], p[:, j]) for j in range(1, 4)])
|
||||
query = pts[frames[ids]] # Uncorrected, held-out scanner output.
|
||||
radius = PROFILE["local_validation_radius_m"]
|
||||
query = query[np.linalg.norm(query - position, axis=1) <= radius]
|
||||
estimated = transform(position[None], seed)[0]
|
||||
reference = target[tree.query_ball_point(estimated, radius + 5)]
|
||||
fit = register(reference, query, seed)
|
||||
row = dict(
|
||||
group=int(group),
|
||||
distance_m=distance,
|
||||
source_frames=int(frames.sum()),
|
||||
source_query_points=len(query),
|
||||
fit=fit,
|
||||
)
|
||||
if "T_reference_query" in fit:
|
||||
fitted = np.asarray(fit["T_reference_query"])
|
||||
implied = field.matrices(distance)[0]
|
||||
row["model_consistency_m"] = float(
|
||||
np.linalg.norm(
|
||||
transform(position[None], fitted) - transform(position[None], implied)
|
||||
)
|
||||
)
|
||||
row["model_consistency_deg"] = float(
|
||||
np.rad2deg(
|
||||
np.linalg.norm(
|
||||
Rotation.from_matrix(fitted[:3, :3].T @ implied[:3, :3]).as_rotvec()
|
||||
)
|
||||
)
|
||||
)
|
||||
# All-point tails remain visible, not just accepted correspondences.
|
||||
dist, _ = tree.query(transform(query, fitted), workers=1)
|
||||
row["all_point_distance_p95_m"] = float(np.quantile(dist, 0.95))
|
||||
if fit["status"] == "candidate":
|
||||
seed = fitted # causal last accepted transform, never field oracle.
|
||||
rows.append(row)
|
||||
if len(rows) % 10 == 0:
|
||||
print(f"validation {label}: {len(rows)} windows", flush=True)
|
||||
return dict(
|
||||
label=label,
|
||||
training_map_points=len(target),
|
||||
windows=rows,
|
||||
candidate_count=sum(r["fit"]["status"] == "candidate" for r in rows),
|
||||
total=len(rows),
|
||||
interpretation="same-source held-out-frame local matching, not independent truth",
|
||||
seed="identity then previous accepted transform; no current-field seed",
|
||||
)
|
||||
|
||||
|
||||
def materialize(cache, data, field, output):
|
||||
frames = data["frames"]
|
||||
count = int(frames[-1, 2] + frames[-1, 3])
|
||||
source = np.memmap(cache / "source-points.f32", dtype="<f4", mode="r", shape=(count, 3))
|
||||
with (output / "corrected-points.f32").open("xb") as stream:
|
||||
for frame, distance in zip(frames, data["frame_distance"], strict=True):
|
||||
offset, size = int(frame[2]), int(frame[3])
|
||||
field.points(source[offset : offset + size], distance).astype("<f4").tofile(stream)
|
||||
pos, q = field.poses(data["poses"][:, 1:4], data["poses"][:, 4:8], data["distance"])
|
||||
np.savez(
|
||||
output / "corrected-trajectory.npz",
|
||||
positions=pos,
|
||||
orientations_xyzw=q,
|
||||
receipt_time_s=data["poses"][:, 0],
|
||||
distance_m=data["distance"],
|
||||
frame_distance_m=data["frame_distance"],
|
||||
frames=frames,
|
||||
)
|
||||
return dict(
|
||||
points=count,
|
||||
path_before_m=float(data["distance"][-1]),
|
||||
path_after_m=float(np.linalg.norm(np.diff(pos, axis=0), axis=1).sum()),
|
||||
endpoint_delta_before_m=(data["poses"][-1, 1:4] - data["poses"][0, 1:4]).tolist(),
|
||||
endpoint_delta_after_m=(pos[-1] - pos[0]).tolist(),
|
||||
corrected_points_sha256=digest(output / "corrected-points.f32"),
|
||||
corrected_trajectory_sha256=digest(output / "corrected-trajectory.npz"),
|
||||
)
|
||||
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--raw", type=Path, required=True)
|
||||
parser.add_argument("--sha256", required=True)
|
||||
parser.add_argument("--session-id", required=True)
|
||||
parser.add_argument("--output", type=Path, required=True)
|
||||
parser.add_argument("--cache", type=Path)
|
||||
args = parser.parse_args()
|
||||
if args.raw.resolve().is_relative_to(args.output.resolve()):
|
||||
raise ValueError("Output must not contain the source recording.")
|
||||
args.output.mkdir(parents=True, exist_ok=False)
|
||||
started = time.monotonic()
|
||||
cache = args.cache or args.output / "decoded"
|
||||
if args.cache:
|
||||
meta = json.loads((cache / "source.json").read_text())
|
||||
if (
|
||||
meta["source_sha256"] != args.sha256
|
||||
or digest(args.raw) != args.sha256
|
||||
or not compatible_cache_profile(meta["profile"])
|
||||
):
|
||||
raise ValueError("Cached source identity mismatch.")
|
||||
seal = json.loads((cache / "seal.json").read_text())
|
||||
if any(digest(cache / name) != value for name, value in seal.items()):
|
||||
raise ValueError("Decoded cache integrity mismatch.")
|
||||
else:
|
||||
meta = extract(args.raw, cache, args.sha256)
|
||||
write_json(
|
||||
cache / "seal.json",
|
||||
{
|
||||
name: digest(cache / name)
|
||||
for name in ["source-points.f32", "source-intensity.u8", "index.npz", "source.json"]
|
||||
},
|
||||
)
|
||||
with np.load(cache / "index.npz") as stored:
|
||||
data = dict(stored)
|
||||
try:
|
||||
links, audits = links_for(data)
|
||||
except ClosureUnavailable as exc:
|
||||
write_json(args.output / "closure-search.json", exc.report)
|
||||
raise
|
||||
write_json(args.output / "closure-search.json", audits[0]["search"])
|
||||
write_json(args.output / "registrations.json", audits)
|
||||
# Keep each measured link and its explicit weights reproducible.
|
||||
link_doc = []
|
||||
for e in links:
|
||||
d = asdict(e)
|
||||
d["T_reference_query"] = e.T_reference_query.tolist()
|
||||
d["query_center"] = e.query_center.tolist()
|
||||
link_doc.append(d)
|
||||
write_json(args.output / "surface-links.json", link_doc)
|
||||
length = float(data["distance"][-1])
|
||||
field, fit = fit_correction(length, links)
|
||||
write_json(args.output / "correction.json", fit)
|
||||
if not fit["converged"]:
|
||||
raise ValueError("Correction solver did not converge; do not materialize.")
|
||||
original = CorrectionField([0, length], np.zeros((2, 6)))
|
||||
validation = [evaluate(data, original, "original"), evaluate(data, field, "corrected")]
|
||||
write_json(args.output / "validation.json", validation)
|
||||
sensitivity = []
|
||||
grid = np.linspace(0, length, 1001)
|
||||
route = np.stack(
|
||||
[np.interp(grid, data["distance"], data["poses"][:, j]) for j in range(1, 4)], axis=1
|
||||
)
|
||||
for strength in [0.5, 2.0]:
|
||||
other, report = fit_correction(length, links, CorrectionPolicy(strain_weight=strength))
|
||||
delta = np.linalg.norm(other.points(route, grid) - field.points(route, grid), axis=1)
|
||||
sensitivity.append(
|
||||
dict(
|
||||
strain_weight=strength,
|
||||
converged=report["converged"],
|
||||
maximum_route_difference_m=float(delta.max()),
|
||||
p95_route_difference_m=float(np.quantile(delta, 0.95)),
|
||||
)
|
||||
)
|
||||
corrected_route = field.points(route, grid)
|
||||
displacement = corrected_route - route
|
||||
gradient = np.linalg.norm(np.diff(displacement, axis=0), axis=1) / np.diff(grid)
|
||||
rotation_gradient = np.rad2deg(np.linalg.norm(field.spline(grid, 1)[:, 3:], axis=1))
|
||||
product = materialize(cache, data, field, args.output)
|
||||
if digest(args.raw) != args.sha256:
|
||||
raise ValueError("Raw source changed during experiment.")
|
||||
summary = dict(
|
||||
schema_version="missioncore.recorded-ring-experiment/v2",
|
||||
created_at=datetime.now(UTC).isoformat(),
|
||||
source_session=args.session_id,
|
||||
source=meta,
|
||||
profile=PROFILE,
|
||||
registration_policy=POLICY,
|
||||
closure_registration_policy=CLOSURE_REGISTRATION_POLICY,
|
||||
closure_policy=asdict(ClosurePolicy()),
|
||||
closure_acquisition=audits[0]["search"]["status"],
|
||||
versions={m: version(m) for m in ["numpy", "scipy", "small-gicp"]},
|
||||
elapsed_s=time.monotonic() - started,
|
||||
source_cache=str(cache.resolve()),
|
||||
product=product,
|
||||
surface_links=len(links),
|
||||
sensitivity=sensitivity,
|
||||
deformation=dict(
|
||||
maximum_route_displacement_m=float(np.linalg.norm(displacement, axis=1).max()),
|
||||
max_route_displacement_gradient_m_per_m=float(gradient.max()),
|
||||
p95_route_displacement_gradient_m_per_m=float(np.quantile(gradient, 0.95)),
|
||||
max_rotation_parameter_gradient_deg_per_m=float(rotation_gradient.max()),
|
||||
individual_frame_transform="rigid; no internal scale/shear",
|
||||
),
|
||||
validation=[
|
||||
dict(label=v["label"], candidates=v["candidate_count"], windows=v["total"])
|
||||
for v in validation
|
||||
],
|
||||
status="experimental-candidate-not-promoted",
|
||||
production_promotion=False,
|
||||
vehicle_control=False,
|
||||
original_modified=False,
|
||||
limitations=[
|
||||
"Single source: frame holdout is not an independent pass or ground truth.",
|
||||
"K1 mapped increments, no per-point motion reconstruction.",
|
||||
"Known start-area revisit; no automatic arbitrary-loop discovery.",
|
||||
"Weights are engineering priors, not calibrated sensor covariance.",
|
||||
],
|
||||
)
|
||||
write_json(args.output / "summary.json", summary)
|
||||
print(json.dumps(summary, indent=2), flush=True)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,144 @@
|
||||
"""Additional checks for an existing correction, never a re-fit or threshold change."""
|
||||
|
||||
import argparse
|
||||
import json
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
from reconstruct_recorded_ring import PROFILE, digest, register, voxel, write_json
|
||||
from scipy.spatial import cKDTree
|
||||
|
||||
from k1link.missions.registration import transform
|
||||
from k1link.reconstruction.closure import review_acceptance
|
||||
from k1link.reconstruction.smooth_correction import CorrectionField
|
||||
|
||||
|
||||
def stats(values):
|
||||
return dict(
|
||||
zip(
|
||||
["min", "median", "p95", "max"],
|
||||
np.quantile(values, [0, 0.5, 0.95, 1]).tolist(),
|
||||
strict=True,
|
||||
)
|
||||
)
|
||||
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("result", type=Path)
|
||||
parser.add_argument("--output-name", default="review.json")
|
||||
parser.add_argument("--include-group", type=int, action="append", default=[])
|
||||
args = parser.parse_args()
|
||||
root = args.result
|
||||
summary = json.loads((root / "summary.json").read_text())
|
||||
fit = json.loads((root / "correction.json").read_text())
|
||||
with np.load(Path(summary["source_cache"]) / "index.npz") as d:
|
||||
data = dict(d)
|
||||
p, f, s = data["poses"], data["frames"], data["frame_distance"]
|
||||
pts, ids, held = data["sample_points"], data["sample_frame"], data["heldout"]
|
||||
field = CorrectionField(fit["knots_m"], fit["parameters"], fit.get("origin_m"))
|
||||
length = float(data["distance"][-1])
|
||||
baseline = CorrectionField([0, length], np.zeros((2, 6)))
|
||||
runs = json.loads((root / "validation.json").read_text())
|
||||
failures = sorted(
|
||||
set(args.include_group)
|
||||
| {r["group"] for run in runs for r in run["windows"] if r["fit"]["status"] != "candidate"}
|
||||
)
|
||||
offsets = np.searchsorted(ids, np.arange(len(f) + 1))
|
||||
results = []
|
||||
for label, correction, run in zip(
|
||||
["original", "corrected"], [baseline, field], runs, strict=True
|
||||
):
|
||||
transformed = np.empty_like(pts)
|
||||
for i, distance in enumerate(s):
|
||||
start, end = offsets[i : i + 2]
|
||||
transformed[start:end] = correction.points(pts[start:end], distance)
|
||||
target = voxel(transformed[~held[ids]])
|
||||
tree = cKDTree(target)
|
||||
# Cross-visit check: last held-out frames against ONLY first training window.
|
||||
first = (f[:, 0] <= f[0, 0] + PROFILE["seam_reference_s"]) & ~held
|
||||
last = (f[:, 0] >= f[-1, 0] - 15) & held
|
||||
# Identical source point membership before/after, selected before correction.
|
||||
near = np.linalg.norm(pts - p[0, 1:4], axis=1) <= 25
|
||||
first_pts = transformed[first[ids] & near]
|
||||
last_pts = transformed[last[ids] & near]
|
||||
distances, _ = cKDTree(voxel(first_pts)).query(last_pts, workers=1)
|
||||
seam = dict(
|
||||
query_frames=int(last.sum()),
|
||||
points=len(last_pts),
|
||||
overlap_05m=float(np.mean(distances <= 0.5)),
|
||||
all_point_distances_m=stats(distances),
|
||||
inlier_rmse_m=float(np.sqrt(np.mean(distances[distances <= 0.5] ** 2))),
|
||||
refit=False,
|
||||
)
|
||||
focused = []
|
||||
for group in failures:
|
||||
# Same prior as the original two-second query; then only fresh held-out halves.
|
||||
full = next(r for r in run["windows"] if r["group"] == group)
|
||||
prior = np.asarray(full["fit"]["initial_T_reference_query"])
|
||||
t0 = f[0, 0] + group * PROFILE["holdout_period_s"] + PROFILE["holdout_start_s"]
|
||||
for half in [0, 1]:
|
||||
mask = held & (f[:, 0] >= t0 + half) & (f[:, 0] < t0 + half + 1)
|
||||
position = np.array(
|
||||
[np.interp(float(np.mean(f[mask, 0])), p[:, 0], p[:, j]) for j in range(1, 4)]
|
||||
)
|
||||
query = pts[mask[ids]]
|
||||
query = query[np.linalg.norm(query - position, axis=1) <= 40]
|
||||
estimated = transform(position[None], prior)[0]
|
||||
reference = target[tree.query_ball_point(estimated, 45)]
|
||||
match = register(reference, query, prior)
|
||||
focused.append(dict(group=group, half=half, frames=int(mask.sum()), fit=match))
|
||||
if match["status"] == "candidate":
|
||||
prior = np.asarray(match["T_reference_query"])
|
||||
results.append(
|
||||
dict(
|
||||
label=label,
|
||||
seam_holdout=seam,
|
||||
focused=focused,
|
||||
overlap=stats([r["fit"]["overlap"] for r in run["windows"]]),
|
||||
inlier_rmse_m=stats([r["fit"]["inlier_rmse_m"] for r in run["windows"]]),
|
||||
model_consistency_m=stats([r["model_consistency_m"] for r in run["windows"]]),
|
||||
)
|
||||
)
|
||||
acceptance = review_acceptance(results, runs)
|
||||
write_json(
|
||||
root / args.output_name,
|
||||
dict(
|
||||
frozen_correction=True,
|
||||
unchanged_registration_policy=True,
|
||||
selected_failure_groups=failures,
|
||||
results=results,
|
||||
acceptance=acceptance,
|
||||
),
|
||||
)
|
||||
sources = [
|
||||
Path(__file__),
|
||||
Path(__file__).with_name("reconstruct_recorded_ring.py"),
|
||||
Path(__file__).parents[1] / "src/k1link/reconstruction/smooth_correction.py",
|
||||
Path(__file__).parents[1] / "src/k1link/reconstruction/closure.py",
|
||||
Path(__file__).parents[1] / "src/k1link/missions/registration.py",
|
||||
]
|
||||
evidence = [
|
||||
root / name
|
||||
for name in [
|
||||
"summary.json",
|
||||
"correction.json",
|
||||
"validation.json",
|
||||
"registrations.json",
|
||||
"surface-links.json",
|
||||
"corrected-points.f32",
|
||||
"corrected-trajectory.npz",
|
||||
args.output_name,
|
||||
]
|
||||
]
|
||||
if summary["schema_version"] == "missioncore.recorded-ring-experiment/v2":
|
||||
evidence.append(root / "closure-search.json")
|
||||
write_json(
|
||||
root / (args.output_name + ".seal.json"),
|
||||
{str(path.resolve()): digest(path) for path in sources + evidence},
|
||||
)
|
||||
print(json.dumps(results, indent=2))
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
Reference in New Issue
Block a user