DCCONSTRUCTIONS
e515ab1b8c
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.
322 lines
12 KiB
Python
322 lines
12 KiB
Python
"""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}
|