feat: finalize corrected-route planning and Rerun recording review

tests/test_route_relocalization.py

@@ -12,7 +12,7 @@ from k1link.missions.route_relocalization import (
     relocalize_route,
     relocalize_start_then_route,
 )
-from k1link.missions.stationary_bootstrap import BOOTSTRAP_POLICY, StationaryBootstrap
+from k1link.missions.stationary_bootstrap import StationaryBootstrap
 
 
 def scene():
@@ -102,8 +102,8 @@ def test_middle_of_route_relocalisation_qualifies_with_real_gicp():
     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."""
+def test_hybrid_compares_dense_start_with_entire_route_before_accepting(monkeypatch):
+    """Preserve a precise start hypothesis, but never shortcut global ambiguity."""
     import k1link.missions.route_relocalization as module
 
     reference, path = scene()
@@ -112,15 +112,20 @@ def test_hybrid_accepts_the_dense_start_before_route_retrieval(monkeypatch):
     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.")
+    calls = []
+    def route(*args, **kwargs):
+        calls.append(kwargs)
+        hypotheses = kwargs["additional_hypotheses"]
+        assert len(hypotheses) == 1
+        return choose_route_location(hypotheses, [0, 0, 0], complete=True)
 
-    monkeypatch.setattr(module, "relocalize_route", unexpected_route)
+    monkeypatch.setattr(module, "relocalize_route", 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 calls
+    assert output["initialization"]["strategy"] == ROUTE_RELOCALIZATION_POLICY["strategy"]
+    assert output["initialization"]["expected_attempts"] == 109
     assert output["initialization"]["stages"][0]["name"] == "dense-start"
 
 
@@ -194,6 +199,7 @@ def test_global_result_must_account_for_every_generated_hypothesis_before_prior(
     boot.phase = "searching"
     boot.initialization_sample = dict(monotonic_ns=1, segment=0)
     boot.search_started_ns = 1
+    boot.last_pose_ns = boot.last_cloud_ns = 1
     accepted = boot.offer_prior(
         dict(
             status="candidate",
@@ -212,6 +218,7 @@ def test_global_result_must_account_for_every_generated_hypothesis_before_prior(
     second.phase = "searching"
     second.initialization_sample = dict(monotonic_ns=1, segment=0)
     second.search_started_ns = 1
+    second.last_pose_ns = second.last_cloud_ns = 1
     initialization["expected_attempts"] = 3
     rejected = second.offer_prior(
         dict(
@@ -236,13 +243,12 @@ def test_global_ambiguity_has_a_specific_operator_message():
     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_route_search_separates_numerical_progress_from_current_receipt_freshness():
+    assert "deadline_s" not in ROUTE_RELOCALIZATION_POLICY
+    assert "maximum_search_wall_s" not in ROUTE_RELOCALIZATION_POLICY
+    assert ROUTE_RELOCALIZATION_POLICY["worker_stall_s"] > 0
+    assert ROUTE_RELOCALIZATION_POLICY["hypothesis_freshness"] == (
+        "stationary-receipts-and-disjoint-confirmation/v1"
     )
 
 
@@ -263,7 +269,7 @@ def test_precise_search_reaches_the_last_ranked_anchor(monkeypatch):
     def register(self, query, initial, *, policy):
         calls.append(policy)
         result = candidate_result(np.eye(4))
-        if len(calls) < (len(ranked) - 1) * 3 + 1:
+        if len(calls) < (len(ranked) - 1) * 6 + 1:
             result.update(status="rejected", reasons=["fixture mismatch"])
         return result
 
@@ -273,14 +279,14 @@ def test_precise_search_reaches_the_last_ranked_anchor(monkeypatch):
     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(calls) == info["expected_attempts"] == len(ranked) * 6
     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):
+def test_elapsed_time_does_not_discard_the_rest_of_the_finite_queue(monkeypatch):
     import k1link.missions.route_relocalization as module
 
     reference, path = scene()
@@ -294,11 +300,39 @@ def test_budget_exhaustion_is_not_a_completed_negative_or_provisional_fit(monkey
     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"] == []
+    assert result["status"] == "candidate"
+    assert elapsed[0] > 120
+    assert info["complete"] and info["candidate_queue_exhausted"]
+    assert not info["remaining_candidate_indices"]
+    assert info["expected_attempts"] == len(info["attempts"])
+    assert info["candidate_queue"]
+
+
+def test_anchor_seeds_map_the_sensor_origin_not_the_cloud_median(monkeypatch):
+    import k1link.missions.route_relocalization as module
+    reference, path = scene()
+    entry = np.array([1.0, 2.0, 0.2])
+    monkeypatch.setattr(module.PreparedReference, "register",
+                        lambda self, query, initial, **kwargs: candidate_result(initial))
+    result = relocalize_route(reference, path, reference[:700], entry)
+    for item in result["initialization"]["attempts"]:
+        if item["seed_mode"] != "pose-anchor":
+            continue
+        matrix = np.asarray(item["result"]["initial_T_reference_query"])
+        assert np.allclose(matrix[:3, :3] @ entry + matrix[:3, 3], item["candidate"]["position"])
+
+
+def test_dense_start_cannot_hide_an_equally_good_distant_place(monkeypatch):
+    import k1link.missions.route_relocalization as module
+    reference, path = scene()
+    dense = attempt(-1, 0, np.eye(4))
+    other = np.eye(4)
+    other[0, 3] = 80
+    monkeypatch.setattr(module.PreparedReference, "register",
+                        lambda *args, **kwargs: candidate_result(other))
+    result = relocalize_route(reference, path, reference[:700], [0, 0, 0],
+                              additional_hypotheses=[dense])
+    assert result["reasons"] == ["ambiguous-route-location"]
 
 
 def test_ambiguity_uses_fitted_places_even_when_retrieval_anchor_is_same():
@@ -319,3 +353,14 @@ def test_fresh_confirmation_fallback_skips_dense_search(monkeypatch):
     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}
+
+
+def test_sparse_start_patch_does_not_prevent_searching_the_rest_of_the_map(monkeypatch):
+    import k1link.missions.route_relocalization as module
+    reference, path = scene()
+    def no_start(*args, **kwargs):
+        raise module.ReferenceCoverageError("sparse start")
+    monkeypatch.setattr(module, "_route_start_context", no_start)
+    monkeypatch.setattr(module, "relocalize_route", lambda *a, **k: {"initialization": {}})
+    result = relocalize_start_then_route(reference, path, reference[:700], [0, 0, 0])
+    assert result["initialization"]["dense_start_unavailable"] == "sparse start"