SILVER/NODEDC_MISSION_CORE
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

feat: finalize corrected-route planning and Rerun recording review

Browse Source
This commit is contained in:
DCCONSTRUCTIONS
2026-09-22 10:10:03 +03:00
parent c804d89b18
commit 2e5d52521f
132 changed files with 14141 additions and 898 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/k1link/missions/route_relocalization.py
+71 -63
View File
@@ -1,9 +1,8 @@
"""Staged stationary localisation before the normal fresh-data tracking gate.
A known start is the reliable laboratory path, so it first receives a dense
multi-start fit. Only its honest rejection permits retrieval over the entire
selected route. That preserves a repeatable start while retaining an auditable
recovery path for a restarted rover that must look for *where it is*.
A known start first receives a dense multi-start fit, but it cannot shortcut
comparison with the entire selected route. A finite queue, not elapsed wall
time, defines completeness. The process owner handles cancellation and stalls.
Neither stage grants tracking or vehicle authority: both only produce a
provisional hypothesis for the separate, disjoint fresh-data gate.
@@ -13,7 +12,6 @@ from __future__ import annotations
import math
import time
from copy import deepcopy
from dataclasses import dataclass
from itertools import product
@@ -21,15 +19,17 @@ import numpy as np
from .entry_acquisition import acquire_entry
from .observation_profiles import TRACKING_INPUT
from .reference_window import reference_window
from .reference_window import ReferenceCoverageError, reference_window
from .registration import POLICY as TRACKING_POLICY
from .registration import PreparedReference, angle_deg, cloud, rigid, transform
from .stationary_entry import STATIONARY_POLICY
ROUTE_RELOCALIZATION_POLICY = dict(
version="route-relocalization/v6",
version="route-relocalization/v7",
scope="selected-route",
strategy="dense-start-first-then-route-recovery/v1",
strategy="dense-start-and-complete-route-comparison/v2",
hypothesis_freshness="stationary-receipts-and-disjoint-confirmation/v1",
seed_modes=["pose-anchor", "cloud-median"],
# Local geometry is independent of the 80-m presentation envelope.
query_radius_m=TRACKING_INPUT["radius_m"],
anchor_spacing_m=5.0,
@@ -54,13 +54,9 @@ ROUTE_RELOCALIZATION_POLICY = dict(
cluster_rotation_deg=8.0,
ambiguity_overlap_margin=0.05,
ambiguity_rmse_margin_m=0.03,
# Keep the stationary prefix younger than the bootstrap's 40-s source-age
# fence. A late exhaustive calculation is an explicit incomplete search,
# never a stale provisional position.
deadline_s=30.0,
maximum_search_wall_s=35.0,
# This is a numerical convergence envelope, not an operator start-radius
# admission rule. Reaching its wall deadline is reported as incomplete.
# No overall search timer: every admitted place must be compared. A child
# that makes NO progress is separately stopped, never called a map mismatch.
worker_stall_s=60.0,
registration_policy={
**TRACKING_POLICY,
"version": "route-relocalization-gicp/v1",
@@ -257,7 +253,8 @@ class RouteCandidate:
def rank_route_candidates(
reference, reference_path, query, *, policy=ROUTE_RELOCALIZATION_POLICY, grid=None
reference, reference_path, query, *, policy=ROUTE_RELOCALIZATION_POLICY, grid=None,
on_progress=None,
):
"""Rank every resampled route position against the stationary query cloud."""
reference, query = route_reference_cloud(reference), cloud(query)
@@ -267,6 +264,9 @@ def rank_route_candidates(
query_descriptor = radial_height_descriptor(query, query_center, policy=policy)
ranked = []
for index, (position, distance) in enumerate(zip(anchors, progress, strict=True)):
if on_progress is not None:
on_progress(dict(stage="route-index", completed_anchors=index,
total_anchors=len(anchors)))
target = local_submap(
grid,
position,
@@ -454,6 +454,8 @@ def relocalize_route(
*,
clock=time.monotonic,
policy=ROUTE_RELOCALIZATION_POLICY,
on_progress=None,
additional_hypotheses=(),
):
"""Run complete candidate retrieval and qualification against a selected route."""
started = clock()
@@ -461,7 +463,7 @@ def relocalize_route(
query_entry = np.asarray(query_entry, dtype=float).reshape(3)
grid = ReferenceGrid(reference, cell_m=policy["spatial_cell_m"])
ranked, coverage = rank_route_candidates(
reference, reference_path, query, policy=policy, grid=grid
reference, reference_path, query, policy=policy, grid=grid, on_progress=on_progress
)
attempts, evaluated, batches = [], [], []
query_center = np.median(query, axis=0)
@@ -470,11 +472,10 @@ def relocalize_route(
float(np.linalg.norm(query - query_center, axis=1).max())
+ policy["target_context_margin_m"],
)
expected = len(ranked) * policy["yaw_candidates_per_place"]
fits_per_place = policy["yaw_candidates_per_place"] * len(policy["seed_modes"])
expected = len(ranked) * fits_per_place
batch_size = policy["candidate_batch_size"]
for candidate in ranked:
if clock() - started > policy["deadline_s"]:
break
if len(evaluated) % batch_size == 0:
batches.append([])
target = local_submap(
@@ -487,12 +488,18 @@ def relocalize_route(
target_center = np.median(target, axis=0)
count_before = len(attempts)
prepared = None
for yaw_deg in _yaw_candidates(
query, target, query_center, target_center, policy=policy
for yaw_deg, seed_mode in product(
_yaw_candidates(query, target, query_center, target_center, policy=policy),
policy["seed_modes"],
):
if clock() - started > policy["deadline_s"]:
break
initial = _seed(query_center, target_center, yaw_deg)
if on_progress is not None:
on_progress(dict(stage="route-search", completed_fits=len(attempts),
total_fits=expected, candidate_index=candidate.index))
# The sensor pose is the spatial origin of this hypothesis. Cloud
# medians shift with occlusion/vegetation and are not scanner poses.
initial = (_seed(query_entry, candidate.position, yaw_deg)
if seed_mode == "pose-anchor"
else _seed(query_center, target_center, yaw_deg))
try:
# Target preprocessing is independent of yaw. Keep one tree
# per place; all seeds and all eligibility checks stay intact.
@@ -512,15 +519,21 @@ def relocalize_route(
descriptor_distance=candidate.descriptor_distance,
),
yaw_deg=yaw_deg,
seed_mode=seed_mode,
result=result,
)
)
if len(attempts) - count_before != policy["yaw_candidates_per_place"]:
if len(attempts) - count_before != fits_per_place:
break
evaluated.append(candidate.index)
batches[-1].append(candidate.index)
complete = len(evaluated) == len(ranked) and clock() - started <= policy["deadline_s"]
result = choose_route_location(attempts, query_entry, complete=complete, policy=policy)
complete = len(evaluated) == len(ranked)
result = choose_route_location(
[*attempts, *additional_hypotheses], query_entry, complete=complete, policy=policy
)
# Dense-start evidence is accounted for by the caller, not counted twice as
# one extra route seed. It nevertheless participates in spatial ambiguity.
result["initialization"]["attempts"] = result["initialization"]["attempts"][:len(attempts)]
result["initialization"].update(
coverage,
elapsed_s=clock() - started,
@@ -644,23 +657,26 @@ def relocalize_start_then_route(
policy=ROUTE_RELOCALIZATION_POLICY,
reference_position=None,
route_only=False,
on_progress=None,
):
"""Use the proven start-area fit first, then a bounded route fallback.
This is deliberately not a looser acceptance rule. The dense start fit
runs every stationary multi-start seed against its high-resolution local
target. Only an honest rejection enters whole-route retrieval, whose
result remains provisional until the existing fresh-data gate confirms it.
"""
"""Compare the proven dense start with every route place before deciding."""
started = clock()
if route_only:
# A dense-start prior failed fresh confirmation. Recollect first, then
# search the route without repeatedly retrying that unconfirmed start.
return relocalize_route(reference, reference_path, query, query_entry,
clock=clock, policy=policy)
target, query, initial, entry, forward, window = _route_start_context(
reference, reference_path, query, query_entry, reference_position
)
clock=clock, policy=policy, on_progress=on_progress)
try:
target, query, initial, entry, forward, window = _route_start_context(
reference, reference_path, query, query_entry, reference_position
)
except ReferenceCoverageError as exc:
# A sparse start patch is not proof that the whole known route is
# unusable. Keep the ordinary global proof and fresh confirmation.
result = relocalize_route(reference, reference_path, query, query_entry,
clock=clock, policy=policy, on_progress=on_progress)
result["initialization"]["dense_start_unavailable"] = str(exc)
return result
start_result = acquire_entry(
target,
query,
@@ -668,7 +684,8 @@ def relocalize_start_then_route(
entry,
forward,
clock=clock,
policy=STATIONARY_POLICY,
policy={**STATIONARY_POLICY, "deadline_s": None},
progress=on_progress,
)
start_result["initialization"].update(
scope=policy["scope"],
@@ -686,35 +703,26 @@ def relocalize_start_then_route(
]
),
)
if start_result["status"] == "candidate":
start_result["initialization"] = _hybrid_initialization(policy, start_result)
return start_result
if not start_result["initialization"].get("complete"):
# Compute exhaustion is not evidence that this place did not match.
start_result["initialization"] = _hybrid_initialization(policy, start_result)
return start_result
# A failed standard start may still be a valid mid-route or recovery
# position. Give retrieval only the fresh-prefix time remaining: it must
# never turn a late calculation into an apparently usable prior.
remaining = policy["maximum_search_wall_s"] - (clock() - started)
if remaining <= 0:
route_result = choose_route_location([], entry, complete=False, policy=policy)
route_result["initialization"].update(
elapsed_s=0.0, worker_timeout_reason="start-stage-timeout"
)
else:
recovery_policy = deepcopy(policy)
recovery_policy["deadline_s"] = min(policy["deadline_s"], remaining)
route_result = relocalize_route(
reference,
reference_path,
query,
entry,
clock=clock,
policy=recovery_policy,
)
additional = []
if start_result["status"] == "candidate":
additional.append(dict(
candidate=dict(index=-1, position=start_result["initialization"]["reference_position"],
progress_m=start_result["initialization"]["route_progress_m"],
descriptor_distance=0.0),
yaw_deg=0.0,
result={key: value for key, value in start_result.items() if key != "initialization"},
))
route_result = relocalize_route(
reference, reference_path, query, entry, clock=clock, policy=policy,
on_progress=on_progress, additional_hypotheses=additional,
)
route_result["initialization"] = _hybrid_initialization(policy, start_result, route_result)
route_result["initialization"]["elapsed_s"] = clock() - started
route_result["registration_seconds"] = start_result.get(
"registration_seconds", 0.0
) + route_result.get("registration_seconds", 0.0)