feat: finalize corrected-route planning and Rerun recording review

2026-09-22 10:10:03 +03:00
parent c804d89b18
commit 2e5d52521f
132 changed files with 14141 additions and 898 deletions
@@ -0,0 +1,211 @@
+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"}) == []