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.
159 lines
6.0 KiB
Python
159 lines
6.0 KiB
Python
"""Deterministic entry hypotheses, ambiguity and bounded numeric qualification."""
|
|
|
|
import numpy as np
|
|
import pytest
|
|
|
|
from k1link.missions.entry_acquisition import acquire_entry, choose_entry, entry_seeds
|
|
from k1link.missions.registration import angle_deg, transform
|
|
|
|
|
|
def attempts(matrix=None):
|
|
matrix = np.eye(4) if matrix is None else matrix
|
|
return [
|
|
dict(
|
|
index=i,
|
|
along_m=(i // 9 - 1) * 3,
|
|
across_m=(i // 3 % 3 - 1) * 3,
|
|
yaw_deg=(i % 3 - 1) * 15,
|
|
result=dict(
|
|
status="candidate",
|
|
T_reference_query=matrix.tolist(),
|
|
overlap=0.95,
|
|
inlier_rmse_m=0.1,
|
|
matched_query_indices=[0],
|
|
registration_seconds=0.01,
|
|
),
|
|
)
|
|
for i in range(27)
|
|
]
|
|
|
|
|
|
def test_seeds_rotate_about_query_entry_and_span_route_basis():
|
|
initial = np.eye(4)
|
|
initial[:3, 3] = [10, 20, 1]
|
|
anchor = np.array([100, 200, 3.0])
|
|
seeds = list(entry_seeds(initial, anchor, [0, 4, 0]))
|
|
assert len(seeds) == 27
|
|
for seed in seeds:
|
|
expected = transform(anchor[None], initial)[0] + [-seed["across_m"], seed["along_m"], 0]
|
|
assert np.allclose(transform(anchor[None], seed["matrix"])[0], expected)
|
|
assert np.linalg.det(seed["matrix"][:3, :3]) == pytest.approx(1)
|
|
with pytest.raises(ValueError):
|
|
list(entry_seeds(initial, anchor, [0, 0, 0]))
|
|
|
|
|
|
def test_single_supported_solution_does_not_relax_local_rejection():
|
|
data = attempts()
|
|
data[0]["result"]["status"] = "rejected"
|
|
result = choose_entry(data, np.eye(4), [0, 0, 0])
|
|
assert result["status"] == "candidate"
|
|
assert result["initialization"]["clusters"][0]["support"] == 26
|
|
assert not result["initialization"]["attempts"][0]["entry_admitted"]
|
|
|
|
|
|
def test_two_near_equal_place_solutions_are_ambiguous_even_with_unequal_support():
|
|
data = attempts()
|
|
alternative = np.eye(4)
|
|
alternative[0, 3] = 2
|
|
data[-1]["result"].update(
|
|
T_reference_query=alternative.tolist(), overlap=0.94, inlier_rmse_m=0.11
|
|
)
|
|
result = choose_entry(data, np.eye(4), [0, 0, 0])
|
|
assert result["status"] == "rejected" and result["reasons"] == ["ambiguous-entry"]
|
|
assert result["matched_query_indices"] == []
|
|
|
|
|
|
def test_cluster_is_pairwise_not_a_chain_between_distant_places():
|
|
data = attempts()
|
|
for i, item in enumerate(data):
|
|
matrix = np.eye(4)
|
|
matrix[0, 3] = (i % 3) * 0.4
|
|
item["result"]["T_reference_query"] = matrix.tolist()
|
|
result = choose_entry(data, np.eye(4), [0, 0, 0])
|
|
assert len(result["initialization"]["clusters"]) == 2
|
|
assert result["reasons"] == ["ambiguous-entry"]
|
|
|
|
|
|
@pytest.mark.parametrize("offset", [[6, 0, 0], [0, 0, 1.1]])
|
|
def test_solution_outside_entry_region_is_rejected(offset):
|
|
matrix = np.eye(4)
|
|
matrix[:3, 3] = offset
|
|
result = choose_entry(attempts(matrix), np.eye(4), [0, 0, 0])
|
|
assert result["reasons"] == ["no-admissible-entry"]
|
|
|
|
|
|
def test_angles_and_multiple_translation_starts_required():
|
|
data = attempts()
|
|
for item in data[3:]:
|
|
item["result"]["status"] = "rejected"
|
|
assert choose_entry(data, np.eye(4), [0, 0, 0])["reasons"] == [
|
|
"insufficient-multistart-support"
|
|
]
|
|
matrix = np.eye(4)
|
|
a = np.radians(31)
|
|
matrix[:2, :2] = [[np.cos(a), -np.sin(a)], [np.sin(a), np.cos(a)]]
|
|
assert choose_entry(attempts(matrix), np.eye(4), [0, 0, 0])["status"] == "rejected"
|
|
|
|
|
|
def test_partial_search_and_deadline_cannot_claim_unique_solution():
|
|
assert choose_entry(attempts(), np.eye(4), [0, 0, 0], complete=False)["reasons"] == [
|
|
"incomplete-search"
|
|
]
|
|
times = iter([0, 1, 26, 26])
|
|
points = np.random.default_rng(1).normal(size=(400, 3))
|
|
output = acquire_entry(
|
|
points,
|
|
points,
|
|
np.eye(4),
|
|
[0, 0, 0],
|
|
[1, 0, 0],
|
|
fitter=lambda *args: attempts()[0]["result"],
|
|
clock=lambda: next(times),
|
|
)
|
|
assert len(output["initialization"]["attempts"]) == 1
|
|
assert output["reasons"] == ["incomplete-search"]
|
|
|
|
|
|
def test_recovers_known_transform_from_several_starts():
|
|
pytest.importorskip("small_gicp")
|
|
from test_mission_registration import geometry
|
|
|
|
ref = geometry()
|
|
truth = np.eye(4)
|
|
a = np.radians(10)
|
|
truth[:2, :2] = [[np.cos(a), -np.sin(a)], [np.sin(a), np.cos(a)]]
|
|
truth[:3, 3] = [0.5, 2.6, 0.1]
|
|
query = transform(ref, np.linalg.inv(truth))
|
|
result = acquire_entry(ref, query, np.eye(4), [0, 0, 0], [1, 0, 0])
|
|
assert result["status"] == "candidate", result["reasons"]
|
|
found = np.asarray(result["T_reference_query"])
|
|
assert np.linalg.norm(found[:3, 3] - truth[:3, 3]) < 0.03
|
|
assert angle_deg(found[:3, :3] @ truth[:3, :3].T) < 0.3
|
|
assert result["policy"]["maximum_correction_m"] == 3
|
|
assert not result["localization_confirmed"] and not result["vehicle_control"]
|
|
|
|
|
|
def test_centre_first_search_preserves_all_seed_identities():
|
|
from k1link.missions.stationary_entry import STATIONARY_POLICY
|
|
seeds = list(entry_seeds(np.eye(4), [0, 0, 0], [1, 0, 0], policy=STATIONARY_POLICY))
|
|
assert (seeds[0]["along_m"], seeds[0]["across_m"], seeds[0]["yaw_deg"]) == (0, 0, 0)
|
|
assert {s["index"] for s in seeds} == set(range(108))
|
|
assert len({(s["along_m"], s["across_m"], s["yaw_deg"]) for s in seeds}) == 108
|
|
|
|
|
|
def test_prepared_target_is_identical_across_rejected_and_accepted_fits():
|
|
from k1link.missions.registration import PreparedReference, register
|
|
from test_mission_registration import geometry
|
|
ref = geometry()
|
|
prepared = PreparedReference(ref)
|
|
query = ref + [.7, -.5, .2]
|
|
first = prepared.register(query, np.eye(4))
|
|
assert prepared.register(query + [100, 0, 0], np.eye(4))["status"] == "rejected"
|
|
second = prepared.register(query, np.eye(4))
|
|
standalone = register(ref, query, np.eye(4))
|
|
for result in (first, second, standalone):
|
|
result.pop("registration_seconds")
|
|
assert first == second == standalone
|
|
assert first["status"] == "candidate"
|
|
assert np.allclose(np.asarray(first["T_reference_query"])[:3, 3], [-.7, .5, -.2], atol=.02)
|