feat(planning): consolidate recorded-route localization and spatial scene

Preserve the completed teach-and-repeat laboratory stage: reference preparation, cascaded acquisition, local tracking and recovery, recording lifecycle, replay qualification, and persistent Rerun scene controls. Document the open grid-picking regression and Rerun upgrade contract. No autonomous driving or loop-closure optimization is claimed.
2026-09-21 08:47:19 +03:00
parent be58d589e2
commit e515ab1b8c
189 changed files with 19074 additions and 758 deletions
@@ -0,0 +1,321 @@
+"""Selected-route retrieval is distinct from the old start-neighbourhood search."""
+
+import numpy as np
+import pytest
+
+from k1link.missions.route_relocalization import (
+    ROUTE_RELOCALIZATION_POLICY,
+    ReferenceGrid,
+    choose_route_location,
+    local_submap,
+    rank_route_candidates,
+    relocalize_route,
+    relocalize_start_then_route,
+)
+from k1link.missions.stationary_bootstrap import BOOTSTRAP_POLICY, StationaryBootstrap
+
+
+def scene():
+    """Three deliberately different local places along one 90-m route."""
+    rng = np.random.default_rng(92)
+    parts = []
+    for center, scale in (
+        (10.0, (2.0, 0.7, 1.0)),
+        (45.0, (0.8, 4.0, 2.0)),
+        (80.0, (4.0, 1.2, 0.4)),
+    ):
+        points = rng.normal(size=(900, 3)) * scale
+        points[:, 0] += center
+        parts.append(points)
+    return np.concatenate(parts), np.array([[0.0, 0.0, 0.0], [90.0, 0.0, 0.0]])
+
+
+def candidate_result(matrix, *, overlap=0.95, rmse=0.1):
+    return dict(
+        status="candidate",
+        T_reference_query=np.asarray(matrix).tolist(),
+        initial_T_reference_query=np.asarray(matrix).tolist(),
+        matched_query_indices=[1],
+        overlap=overlap,
+        inlier_rmse_m=rmse,
+        registration_seconds=0.01,
+        localization_confirmed=False,
+        vehicle_control=False,
+    )
+
+
+def attempt(index, progress, matrix, *, overlap=0.95, rmse=0.1):
+    return dict(
+        candidate=dict(
+            index=index,
+            position=[progress, 0, 0],
+            progress_m=float(progress),
+            descriptor_distance=0.1,
+        ),
+        yaw_deg=0.0,
+        result=candidate_result(matrix, overlap=overlap, rmse=rmse),
+    )
+
+
+def test_descriptor_retrieval_covers_full_selected_route_not_its_start():
+    reference, path = scene()
+    # The stationary K1 cloud is at the middle place in another local SLAM frame.
+    query = reference[(reference[:, 0] > 37) & (reference[:, 0] < 53)] - [45.0, 3.0, 0.0]
+    ranked, coverage = rank_route_candidates(reference, path, query)
+
+    assert coverage["descriptor_scope"] == "entire-selected-route"
+    assert coverage["route_anchor_count"] > 10
+    assert ranked
+    assert len(ranked) == coverage["descriptor_covered_anchor_count"]
+    assert len(ranked) > ROUTE_RELOCALIZATION_POLICY["candidate_batch_size"]
+    assert abs(ranked[0].progress_m - 45.0) <= ROUTE_RELOCALIZATION_POLICY["anchor_spacing_m"]
+
+
+def test_full_route_atlas_is_not_rejected_by_one_gicp_target_cap():
+    reference, _ = scene()
+    full_route = np.tile(reference, (40, 1))  # 108,000 map points, not one GICP target.
+    atlas = ReferenceGrid(full_route)
+    target = local_submap(atlas, [45.0, 0.0, 0.0], 28.0, maximum_points=100_000)
+
+    assert len(full_route) > 100_000
+    assert 300 <= len(target) <= 100_000
+
+
+def test_dense_eighty_metre_submap_keeps_its_footprint_under_the_gicp_budget():
+    x, y = np.meshgrid(np.arange(-40.0, 40.0, 0.2), np.arange(-40.0, 40.0, 0.2))
+    reference = np.column_stack((x.ravel(), y.ravel(), np.zeros(x.size)))
+    target = local_submap(reference, [0.0, 0.0, 0.0], 80.0, maximum_points=1_000)
+
+    assert 300 <= len(target) <= 1_000
+
+
+def test_middle_of_route_relocalisation_qualifies_with_real_gicp():
+    pytest.importorskip("small_gicp")
+    reference, path = scene()
+    query = reference[(reference[:, 0] > 37) & (reference[:, 0] < 53)] - [45.0, 3.0, 0.0]
+    output = relocalize_route(reference, path, query, [0.0, 0.0, 0.0])
+
+    assert output["status"] == "candidate", output["reasons"]
+    assert output["initialization"]["complete"]
+    assert abs(output["initialization"]["selected_route_progress_m"] - 45.0) <= 5.0
+    assert output["overlap"] > 0.9 and output["inlier_rmse_m"] < 0.05
+    assert not output["localization_confirmed"] and not output["vehicle_control"]
+
+
+def test_hybrid_accepts_the_dense_start_before_route_retrieval(monkeypatch):
+    """A normal start must not be degraded by global target voxelisation."""
+    import k1link.missions.route_relocalization as module
+
+    reference, path = scene()
+    query = reference[:700]
+    local = candidate_result(np.eye(4))
+    local["initialization"] = dict(complete=True, reason=None, attempts=[{}] * 108)
+    monkeypatch.setattr(module, "acquire_entry", lambda *args, **kwargs: dict(local))
+
+    def unexpected_route(*args, **kwargs):
+        raise AssertionError("A qualified dense start must not enter route recovery.")
+
+    monkeypatch.setattr(module, "relocalize_route", unexpected_route)
+    output = relocalize_start_then_route(reference, path, query, [0.0, 0.0, 0.0])
+
+    assert output["status"] == "candidate"
+    assert output["initialization"]["strategy"] == "dense-start-first-then-route-recovery/v1"
+    assert output["initialization"]["expected_attempts"] == 108
+    assert output["initialization"]["stages"][0]["name"] == "dense-start"
+
+
+def test_hybrid_records_a_route_recovery_only_after_a_dense_start_rejection(monkeypatch):
+    import k1link.missions.route_relocalization as module
+
+    reference, path = scene()
+    query = reference[:700]
+    local = candidate_result(np.eye(4))
+    local.update(status="rejected", reasons=["no-admissible-entry"])
+    local["initialization"] = dict(complete=True, reason="no-admissible-entry", attempts=[{}])
+    recovered = candidate_result(np.eye(4))
+    recovered["initialization"] = dict(
+        complete=True,
+        reason=None,
+        attempts=[{}],
+        selected_candidate_index=4,
+        selected_route_progress_m=20.0,
+    )
+    monkeypatch.setattr(module, "acquire_entry", lambda *args, **kwargs: dict(local))
+    monkeypatch.setattr(module, "relocalize_route", lambda *args, **kwargs: dict(recovered))
+
+    output = relocalize_start_then_route(reference, path, query, [0.0, 0.0, 0.0])
+
+    assert output["status"] == "candidate"
+    assert [stage["name"] for stage in output["initialization"]["stages"]] == [
+        "dense-start",
+        "route-recovery",
+    ]
+    assert output["initialization"]["expected_attempts"] == 2
+
+
+def test_equally_good_distinct_route_places_are_rejected_as_ambiguous():
+    first, second = np.eye(4), np.eye(4)
+    second[0, 3] = 50.0
+    output = choose_route_location(
+        [attempt(0, 0.0, first), attempt(10, 50.0, second, overlap=0.94, rmse=0.11)],
+        [0.0, 0.0, 0.0],
+        complete=True,
+    )
+
+    assert output["status"] == "rejected"
+    assert output["reasons"] == ["ambiguous-route-location"]
+    assert output["matched_query_indices"] == []
+    assert not output["localization_confirmed"] and not output["vehicle_control"]
+
+
+def test_no_qualified_candidate_is_a_route_location_failure_not_a_fake_prior():
+    output = choose_route_location([], [0.0, 0.0, 0.0], complete=True)
+
+    assert output["status"] == "rejected"
+    assert output["reasons"] == ["no-route-location"]
+    assert output["initialization"]["complete"]
+    assert output["initialization"]["expected_attempts"] == 0
+
+
+def test_global_result_must_account_for_every_generated_hypothesis_before_prior():
+    boot = StationaryBootstrap(
+        np.array([[0.0, 0, 0], [40.0, 0, 0]]),
+        initialization_policy=ROUTE_RELOCALIZATION_POLICY,
+    )
+    # Exercise the policy contract directly; no old 108-seed local-start
+    # assumption is permitted for whole-route retrieval.
+    initialization = dict(
+        policy=ROUTE_RELOCALIZATION_POLICY,
+        scope="selected-route",
+        complete=True,
+        expected_attempts=2,
+        attempts=[{}, {}],
+    )
+    boot.phase = "searching"
+    boot.initialization_sample = dict(monotonic_ns=1, segment=0)
+    boot.search_started_ns = 1
+    accepted = boot.offer_prior(
+        dict(
+            status="candidate",
+            T_reference_query=np.eye(4).tolist(),
+            initialization=initialization,
+        ),
+        1_000_000_000,
+        0,
+    )
+    assert accepted["provisional"]
+
+    second = StationaryBootstrap(
+        np.array([[0.0, 0, 0], [40.0, 0, 0]]),
+        initialization_policy=ROUTE_RELOCALIZATION_POLICY,
+    )
+    second.phase = "searching"
+    second.initialization_sample = dict(monotonic_ns=1, segment=0)
+    second.search_started_ns = 1
+    initialization["expected_attempts"] = 3
+    rejected = second.offer_prior(
+        dict(
+            status="candidate",
+            T_reference_query=np.eye(4).tolist(),
+            initialization=initialization,
+        ),
+        1_000_000_000,
+        0,
+    )
+    assert not rejected["provisional"] and rejected["reason"] == "initialization-incomplete"
+
+
+def test_global_ambiguity_has_a_specific_operator_message():
+    from k1link.missions.stationary_live import phase_message
+
+    boot = StationaryBootstrap(
+        np.array([[0.0, 0, 0], [40.0, 0, 0]]),
+        initialization_policy=ROUTE_RELOCALIZATION_POLICY,
+    )
+    boot.phase, boot.reason = "lost", "initialization-ambiguous"
+    assert "несколько похожих участков" in phase_message(boot)
+
+
+def test_route_search_deadline_cannot_outlive_the_stationary_prefix_freshness_fence():
+    assert (
+        ROUTE_RELOCALIZATION_POLICY["deadline_s"] < BOOTSTRAP_POLICY["maximum_prior_source_age_s"]
+    )
+    assert (
+        ROUTE_RELOCALIZATION_POLICY["maximum_search_wall_s"]
+        <= BOOTSTRAP_POLICY["maximum_prior_source_age_s"]
+    )
+
+
+def test_precise_search_reaches_the_last_ranked_anchor(monkeypatch):
+    import k1link.missions.route_relocalization as module
+
+    reference, path = scene()
+    query = reference[:700]
+    ranked, _ = rank_route_candidates(reference, path, query)
+    calls = []
+    prepared_targets = []
+    original_prepare = module.PreparedReference.__init__
+
+    def prepare(self, target):
+        prepared_targets.append(target)
+        original_prepare(self, target)
+
+    def register(self, query, initial, *, policy):
+        calls.append(policy)
+        result = candidate_result(np.eye(4))
+        if len(calls) < (len(ranked) - 1) * 3 + 1:
+            result.update(status="rejected", reasons=["fixture mismatch"])
+        return result
+
+    monkeypatch.setattr(module.PreparedReference, "register", register)
+    monkeypatch.setattr(module.PreparedReference, "__init__", prepare)
+    result = relocalize_route(reference, path, query, [0, 0, 0])
+    info = result["initialization"]
+    assert result["status"] == "candidate"
+    assert info["selected_candidate_index"] == ranked[-1].index
+    assert len(calls) == info["expected_attempts"] == len(ranked) * 3
+    assert len(prepared_targets) == len(ranked)
+    assert not info["remaining_candidate_indices"]
+    assert len(info["candidate_batches"]) > 1
+    assert all(policy == ROUTE_RELOCALIZATION_POLICY["registration_policy"] for policy in calls)
+
+
+def test_budget_exhaustion_is_not_a_completed_negative_or_provisional_fit(monkeypatch):
+    import k1link.missions.route_relocalization as module
+
+    reference, path = scene()
+    elapsed = [0.0]
+
+    def register(self, query, initial, *, policy):
+        elapsed[0] += 12
+        return candidate_result(np.eye(4))
+
+    monkeypatch.setattr(module.PreparedReference, "register", register)
+    result = relocalize_route(reference, path, reference[:700], [0, 0, 0],
+                              clock=lambda: elapsed[0])
+    info = result["initialization"]
+    assert result["reasons"] == ["incomplete-route-search"]
+    assert not info["complete"] and not info["candidate_queue_exhausted"]
+    assert info["remaining_candidate_indices"]
+    assert info["expected_attempts"] > len(info["attempts"])
+    assert info["candidate_queue"] == []
+
+
+def test_ambiguity_uses_fitted_places_even_when_retrieval_anchor_is_same():
+    first, second = np.eye(4), np.eye(4)
+    second[0, 3] = 40
+    result = choose_route_location([attempt(0, 0, first), attempt(0, 0, second)],
+                                    [0, 0, 0], complete=True)
+    assert result["reasons"] == ["ambiguous-route-location"]
+
+
+def test_fresh_confirmation_fallback_skips_dense_search(monkeypatch):
+    import k1link.missions.route_relocalization as module
+
+    reference, path = scene()
+    def no_dense(*args, **kwargs):
+        raise AssertionError("Already disproved dense-start prior must not repeat.")
+    monkeypatch.setattr(module, "acquire_entry", no_dense)
+    monkeypatch.setattr(module, "relocalize_route", lambda *a, **k: {"route_only": True})
+    assert relocalize_start_then_route(reference, path, reference[:700], [0, 0, 0],
+                                      route_only=True) == {"route_only": True}