212 lines
7.8 KiB
Python
212 lines
7.8 KiB
Python
import importlib.util
|
|
import json
|
|
from pathlib import Path
|
|
|
|
import numpy as np
|
|
import pytest
|
|
|
|
pytest.importorskip("scipy")
|
|
from k1link.reconstruction.closure import (
|
|
ClosurePolicy,
|
|
ClosureUnavailable,
|
|
acquire_closure,
|
|
review_acceptance,
|
|
)
|
|
|
|
|
|
def source():
|
|
t = np.arange(121.0)
|
|
ids = np.repeat(np.arange(len(t)), 100)
|
|
pts = np.random.default_rng(72).normal(size=(len(ids), 3))
|
|
pts[:, 0] = ids / 10
|
|
return dict(
|
|
frames=np.column_stack([t, t, t * 100, np.full(len(t), 100)]),
|
|
poses=np.column_stack([t, np.zeros((len(t), 3))]),
|
|
frame_distance=t * 2,
|
|
sample_frame=ids,
|
|
sample_points=pts,
|
|
heldout=(t % 10 >= 4) & (t % 10 < 6),
|
|
)
|
|
|
|
|
|
def exact(reference, query, seed, *, acquisition):
|
|
if acquisition:
|
|
np.testing.assert_array_equal(seed, np.eye(4))
|
|
seed = seed.copy()
|
|
seed[2, 3] = 4.2
|
|
return dict(status="candidate", T_reference_query=seed.tolist())
|
|
|
|
|
|
def test_complete_search_selects_support_without_heldout_or_endpoint_seed():
|
|
data = source()
|
|
data["poses"][-1, 1:4] = [5, 0, 2] # physical overshoot is not a constraint
|
|
original = data["sample_points"].copy()
|
|
calls = []
|
|
|
|
def register(a, b, seed, *, acquisition):
|
|
for cloud in (a, b):
|
|
frame_ids = np.rint(cloud[:, 0] * 10).astype(int)
|
|
assert not data["heldout"][frame_ids].any()
|
|
assert not set(a[:, 0]) & set(b[:, 0])
|
|
calls.append(acquisition)
|
|
return exact(a, b, seed, acquisition=acquisition)
|
|
|
|
link, report = acquire_closure(data, register)
|
|
assert report["complete"] and len(report["attempts"]) == 12
|
|
selected = report["attempts"][report["selected_attempt"]]
|
|
assert (selected["reference_s"], selected["query_s"]) == (20, 5)
|
|
assert calls == [True, False] * 12
|
|
assert not report["endpoint_constraint"]
|
|
assert link.T_reference_query[2, 3] == 4.2
|
|
np.testing.assert_array_equal(data["sample_points"], original)
|
|
|
|
|
|
def test_unavailable_short_windows_do_not_stop_full_support_search():
|
|
def register(a, b, seed, *, acquisition):
|
|
if acquisition and (len(a) < 3000 or len(b) < 2000):
|
|
return dict(status="rejected", reasons=["synthetic short-window failure"])
|
|
return exact(a, b, seed, acquisition=acquisition)
|
|
|
|
_, report = acquire_closure(source(), register)
|
|
assert report["status"] == "candidate" and report["complete"]
|
|
assert not report["attempts"][0]["qualified"]
|
|
selected = report["attempts"][report["selected_attempt"]]
|
|
assert (selected["reference_s"], selected["query_s"]) == (40, 30)
|
|
|
|
|
|
@pytest.mark.parametrize("mode", ["forward", "reverse", "cycle", "exception"])
|
|
def test_failure_keeps_all_attempts_and_never_materializes_identity(mode):
|
|
def register(a, b, seed, *, acquisition):
|
|
if mode == "exception":
|
|
raise ValueError("unavailable geometry")
|
|
if (mode == "forward" and acquisition) or (mode == "reverse" and not acquisition):
|
|
return dict(status="rejected")
|
|
result = exact(a, b, seed, acquisition=acquisition)
|
|
if mode == "cycle" and not acquisition:
|
|
result["T_reference_query"][0][3] += 0.11
|
|
return result
|
|
|
|
with pytest.raises(ClosureUnavailable) as failure:
|
|
acquire_closure(source(), register)
|
|
assert failure.value.report["complete"]
|
|
assert len(failure.value.report["attempts"]) == 12
|
|
assert failure.value.report["status"] == "rejected"
|
|
|
|
|
|
def test_disagreeing_qualified_windows_reject_instead_of_cherry_picking():
|
|
def register(a, b, seed, *, acquisition):
|
|
if acquisition:
|
|
seed = seed.copy()
|
|
seed[0, 3] = 2 if len(a) < 1200 else 0
|
|
return dict(status="candidate", T_reference_query=seed.tolist())
|
|
|
|
with pytest.raises(ClosureUnavailable, match="ambiguous") as failure:
|
|
acquire_closure(source(), register)
|
|
assert len(failure.value.report["qualified_attempts"]) == 12
|
|
|
|
|
|
def test_short_capture_never_matches_window_to_itself():
|
|
data = source()
|
|
data["frames"][:, 0] /= 100
|
|
|
|
def forbidden(*args, **kwargs):
|
|
pytest.fail("overlapping frames reached registration")
|
|
|
|
with pytest.raises(ClosureUnavailable) as failure:
|
|
acquire_closure(data, forbidden)
|
|
assert all(
|
|
r["reason"] == "overlapping-source-windows" for r in failure.value.report["attempts"]
|
|
)
|
|
|
|
|
|
@pytest.mark.parametrize(
|
|
"kwargs",
|
|
[
|
|
{"reference_seconds": ()},
|
|
{"query_seconds": (0,)},
|
|
{"radius_m": float("nan")},
|
|
{"query_seconds": (5, 5)},
|
|
],
|
|
)
|
|
def test_policy_validation(kwargs):
|
|
with pytest.raises(ValueError):
|
|
ClosurePolicy(**kwargs)
|
|
|
|
|
|
def module(name):
|
|
path = Path(__file__).parents[1] / "experiments" / (name + ".py")
|
|
spec = importlib.util.spec_from_file_location(name, path)
|
|
result = importlib.util.module_from_spec(spec)
|
|
spec.loader.exec_module(result)
|
|
return result
|
|
|
|
|
|
def test_acquisition_does_not_change_live_quality_or_decode_split():
|
|
from k1link.missions.registration import POLICY
|
|
|
|
adapter = module("reconstruct_recorded_ring")
|
|
different = {key for key in POLICY if POLICY[key] != adapter.CLOSURE_REGISTRATION_POLICY[key]}
|
|
assert different == {"version", "maximum_correction_m", "maximum_correction_deg"}
|
|
assert POLICY["maximum_correction_m"] == 3
|
|
legacy = {**adapter.PROFILE, "version": "recorded-ring-experiment/v1"}
|
|
assert adapter.compatible_cache_profile(legacy)
|
|
assert not adapter.compatible_cache_profile({**legacy, "holdout_start_s": 5})
|
|
|
|
|
|
def review():
|
|
before = dict(
|
|
points=400,
|
|
query_frames=4,
|
|
overlap_05m=0.2,
|
|
inlier_rmse_m=0.15,
|
|
all_point_distances_m={"median": 1.2},
|
|
)
|
|
after = {**before, "overlap_05m": 0.95, "all_point_distances_m": {"median": 0.12}}
|
|
results = [dict(seam_holdout=before), dict(seam_holdout=after)]
|
|
validation = [dict(total=12, candidate_count=11), dict(total=12, candidate_count=12)]
|
|
return results, validation
|
|
|
|
|
|
@pytest.mark.parametrize("failure", [None, "local", "quality", "support", "degradation"])
|
|
def test_review_gate_records_failure_without_lowering_quality(failure):
|
|
results, validation = review()
|
|
if failure == "local":
|
|
validation[1]["candidate_count"] = 11
|
|
if failure == "quality":
|
|
results[1]["seam_holdout"]["inlier_rmse_m"] = 0.26
|
|
if failure == "support":
|
|
results[1]["seam_holdout"]["points"] = 20
|
|
if failure == "degradation":
|
|
results[0]["seam_holdout"]["overlap_05m"] = 0.99
|
|
receipt = review_acceptance(results, validation)
|
|
assert receipt["accepted"] == (failure is None)
|
|
assert not receipt["independent_accuracy"] and not receipt["vehicle_control"]
|
|
|
|
|
|
@pytest.mark.parametrize(
|
|
"failure", [None, "partial", "missing-check", "rejected", "missing-review"]
|
|
)
|
|
def test_packaging_v2_requires_complete_acquisition_and_positive_review(tmp_path, failure):
|
|
check = module("package_recorded_map_version").check_closure_review
|
|
search = dict(status="candidate", complete=True, attempts=[{}] * 12, expected_attempts=12)
|
|
receipt = review_acceptance(*review())
|
|
if failure == "partial":
|
|
search["complete"] = False
|
|
if failure == "missing-check":
|
|
receipt["checks"].pop("heldout_seam_quality")
|
|
if failure == "rejected":
|
|
receipt["checks"]["heldout_seam_quality"] = False
|
|
(tmp_path / "closure-search.json").write_text(json.dumps(search))
|
|
(tmp_path / "review.json").write_text(
|
|
json.dumps({} if failure == "missing-review" else dict(acceptance=receipt))
|
|
)
|
|
summary = dict(
|
|
schema_version="missioncore.recorded-ring-experiment/v2", closure_acquisition="candidate"
|
|
)
|
|
if failure:
|
|
with pytest.raises(ValueError):
|
|
check(tmp_path, summary)
|
|
else:
|
|
assert check(tmp_path, summary) == ["closure-search.json"]
|
|
assert check(tmp_path, {"schema_version": "missioncore.recorded-ring-experiment/v1"}) == []
|