730 lines
29 KiB
Python
730 lines
29 KiB
Python
"""Staged stationary localisation before the normal fresh-data tracking gate.
|
|
|
|
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.
|
|
"""
|
|
|
|
from __future__ import annotations
|
|
|
|
import math
|
|
import time
|
|
from dataclasses import dataclass
|
|
from itertools import product
|
|
|
|
import numpy as np
|
|
|
|
from .entry_acquisition import acquire_entry
|
|
from .observation_profiles import TRACKING_INPUT
|
|
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/v7",
|
|
scope="selected-route",
|
|
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,
|
|
spatial_cell_m=10.0,
|
|
# Candidate retrieval stays local and distinctive. The chosen candidate is
|
|
# then matched against the high-resolution local tracking footprint.
|
|
descriptor_context_m=28.0,
|
|
descriptor_radial_bins=7,
|
|
descriptor_height_bins=6,
|
|
descriptor_height_low_m=-4.0,
|
|
descriptor_height_high_m=8.0,
|
|
polar_angle_bins=24,
|
|
# A batch controls scheduling, never eligibility. Ranking must not discard
|
|
# the real place merely because a coarse descriptor prefers an endpoint.
|
|
candidate_batch_size=6,
|
|
yaw_candidates_per_place=3,
|
|
yaw_step_deg=30.0,
|
|
target_context_margin_m=12.0,
|
|
target_maximum_points=None,
|
|
descriptor_voxel_m=0.5,
|
|
cluster_position_m=0.75,
|
|
cluster_rotation_deg=8.0,
|
|
ambiguity_overlap_margin=0.05,
|
|
ambiguity_rmse_margin_m=0.03,
|
|
# 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",
|
|
"maximum_correction_m": 25.0,
|
|
"maximum_correction_deg": 180.0,
|
|
},
|
|
)
|
|
|
|
|
|
def _valid_path(path):
|
|
path = np.asarray(path, dtype=float)
|
|
if path.ndim != 2 or path.shape[1] != 3 or len(path) < 2 or not np.isfinite(path).all():
|
|
raise ValueError("Для поиска по маршруту нужен конечный маршрут минимум из двух точек.")
|
|
lengths = np.linalg.norm(np.diff(path, axis=0), axis=1)
|
|
if not np.isfinite(lengths).all() or float(lengths.sum()) <= 0:
|
|
raise ValueError("Маршрут не содержит достаточной геометрии для поиска.")
|
|
return path, lengths
|
|
|
|
|
|
def route_reference_cloud(value):
|
|
"""Validate the complete route atlas without applying GICP's target cap.
|
|
|
|
A selected kilometre route is not one target: it is indexed here and only a
|
|
local, separately checked target is handed to GICP later.
|
|
"""
|
|
points = np.ascontiguousarray(value, dtype=np.float64)
|
|
if (
|
|
points.ndim != 2
|
|
or points.shape[1] != 3
|
|
or len(points) < 300
|
|
or not np.isfinite(points).all()
|
|
or np.abs(points).max() > 100_000
|
|
):
|
|
raise ValueError("Полная карта маршрута содержит недостаточно конечных точек в метрах.")
|
|
return points
|
|
|
|
|
|
def route_anchors(path, *, spacing_m=ROUTE_RELOCALIZATION_POLICY["anchor_spacing_m"]):
|
|
"""Resample the complete path; no endpoint or intermediate segment is skipped."""
|
|
if not 0 < spacing_m <= 25:
|
|
raise ValueError("Некорректный шаг индекса маршрута.")
|
|
path, lengths = _valid_path(path)
|
|
cumulative = np.r_[0.0, np.cumsum(lengths)]
|
|
distances = np.r_[np.arange(0.0, cumulative[-1], spacing_m), cumulative[-1]]
|
|
positions = []
|
|
for distance in distances:
|
|
segment = min(
|
|
int(np.searchsorted(cumulative, distance, side="right") - 1), len(lengths) - 1
|
|
)
|
|
fraction = (distance - cumulative[segment]) / lengths[segment]
|
|
positions.append(path[segment] + fraction * (path[segment + 1] - path[segment]))
|
|
return np.asarray(positions), distances
|
|
|
|
|
|
def _voxel(points, *, voxel_m):
|
|
if len(points) == 0:
|
|
return points
|
|
_, index = np.unique(np.floor(points / voxel_m).astype(np.int64), axis=0, return_index=True)
|
|
return points[np.sort(index)]
|
|
|
|
|
|
class ReferenceGrid:
|
|
"""Read-only spatial index for a full route map.
|
|
|
|
It prevents every atlas anchor from scanning every point in a kilometre
|
|
route. The grid is local to one isolated search process and is never
|
|
reused as a mutable tracking map.
|
|
"""
|
|
|
|
def __init__(self, reference, *, cell_m=ROUTE_RELOCALIZATION_POLICY["spatial_cell_m"]):
|
|
if not 1.0 <= cell_m <= 25.0:
|
|
raise ValueError("Некорректный размер ячейки карты маршрута.")
|
|
self.reference = route_reference_cloud(reference)
|
|
self.cell_m = float(cell_m)
|
|
cells = np.floor(self.reference / self.cell_m).astype(np.int64)
|
|
keys, inverse = np.unique(cells, axis=0, return_inverse=True)
|
|
order = np.argsort(inverse, kind="stable")
|
|
counts = np.bincount(inverse, minlength=len(keys))
|
|
boundaries = np.r_[0, np.cumsum(counts)]
|
|
self.ordered = self.reference[order]
|
|
self.slices = {
|
|
tuple(key): (int(boundaries[index]), int(boundaries[index + 1]))
|
|
for index, key in enumerate(keys)
|
|
}
|
|
|
|
def crop(self, center, radius_m):
|
|
center = np.asarray(center, dtype=float).reshape(3)
|
|
if not np.isfinite(center).all() or not 0 < radius_m <= 100:
|
|
raise ValueError("Некорректная локальная область маршрута.")
|
|
lower = np.floor((center - radius_m) / self.cell_m).astype(int)
|
|
upper = np.floor((center + radius_m) / self.cell_m).astype(int)
|
|
pieces = []
|
|
ranges = tuple(range(first, last + 1) for first, last in zip(lower, upper, strict=True))
|
|
for key in product(*ranges):
|
|
bounds = self.slices.get(key)
|
|
if bounds is not None:
|
|
pieces.append(self.ordered[slice(*bounds)])
|
|
if not pieces:
|
|
return np.empty((0, 3), dtype=float)
|
|
points = np.concatenate(pieces)
|
|
return points[np.linalg.norm(points - center, axis=1) <= radius_m]
|
|
|
|
|
|
def local_submap(reference, center, radius_m, *, maximum_points):
|
|
"""Radial crop. Production verification preserves the source resolution.
|
|
|
|
An explicit point budget is available only to descriptor/test callers.
|
|
It is never a density threshold for declaring tracking lost.
|
|
"""
|
|
if isinstance(reference, ReferenceGrid):
|
|
points = reference.crop(center, radius_m)
|
|
else:
|
|
full = route_reference_cloud(reference)
|
|
points = full[np.linalg.norm(full - center, axis=1) <= radius_m]
|
|
if maximum_points is not None and len(points) > maximum_points:
|
|
original = points
|
|
voxel_m = ROUTE_RELOCALIZATION_POLICY["descriptor_voxel_m"]
|
|
while len(points) > maximum_points:
|
|
reduced = _voxel(original, voxel_m=voxel_m)
|
|
if voxel_m > radius_m * 2.0:
|
|
return np.empty((0, 3), dtype=float)
|
|
points = reduced
|
|
voxel_m *= 2.0
|
|
if len(points) < 300:
|
|
return np.empty((0, 3), dtype=float)
|
|
return points
|
|
|
|
|
|
def radial_height_descriptor(points, center, *, policy=ROUTE_RELOCALIZATION_POLICY):
|
|
relative = np.asarray(points, dtype=float) - np.asarray(center, dtype=float)
|
|
radial = np.linalg.norm(relative[:, :2], axis=1)
|
|
histogram, _ = np.histogramdd(
|
|
np.column_stack([radial, relative[:, 2]]),
|
|
bins=(
|
|
policy["descriptor_radial_bins"],
|
|
policy["descriptor_height_bins"],
|
|
),
|
|
range=(
|
|
(0.0, policy["descriptor_context_m"]),
|
|
(policy["descriptor_height_low_m"], policy["descriptor_height_high_m"]),
|
|
),
|
|
)
|
|
flat = histogram.reshape(-1)
|
|
norm = float(np.linalg.norm(flat))
|
|
return flat / norm if norm else flat
|
|
|
|
|
|
def polar_descriptor(
|
|
points,
|
|
center,
|
|
*,
|
|
bins=ROUTE_RELOCALIZATION_POLICY["polar_angle_bins"],
|
|
context_m=ROUTE_RELOCALIZATION_POLICY["descriptor_context_m"],
|
|
):
|
|
relative = np.asarray(points, dtype=float) - np.asarray(center, dtype=float)
|
|
angle = np.mod(np.arctan2(relative[:, 1], relative[:, 0]), 2 * math.pi)
|
|
radial = np.linalg.norm(relative[:, :2], axis=1)
|
|
# Four equally sized radial rings prevent one distant, unrelated wall
|
|
# from deciding yaw while retaining the full declared context.
|
|
rings = np.minimum((radial / (context_m / 4.0)).astype(int), 3)
|
|
output = np.zeros((4, bins), dtype=float)
|
|
angles = np.minimum((angle / (2 * math.pi) * bins).astype(int), bins - 1)
|
|
np.add.at(output, (rings, angles), 1.0)
|
|
norm = float(np.linalg.norm(output))
|
|
return output / norm if norm else output
|
|
|
|
|
|
def _yaw_candidates(query, target, query_center, target_center, *, policy):
|
|
q = polar_descriptor(
|
|
query,
|
|
query_center,
|
|
bins=policy["polar_angle_bins"],
|
|
context_m=policy["descriptor_context_m"],
|
|
)
|
|
t = polar_descriptor(
|
|
target,
|
|
target_center,
|
|
bins=policy["polar_angle_bins"],
|
|
context_m=policy["descriptor_context_m"],
|
|
)
|
|
candidates = []
|
|
for yaw in np.arange(0.0, 360.0, policy["yaw_step_deg"]):
|
|
shift = int(round(yaw / 360.0 * policy["polar_angle_bins"]))
|
|
candidates.append((float(np.linalg.norm(t - np.roll(q, shift, axis=1))), float(yaw)))
|
|
return [yaw for _, yaw in sorted(candidates)[: policy["yaw_candidates_per_place"]]]
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class RouteCandidate:
|
|
index: int
|
|
position: np.ndarray
|
|
progress_m: float
|
|
descriptor_distance: float
|
|
|
|
|
|
def rank_route_candidates(
|
|
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)
|
|
grid = grid or ReferenceGrid(reference, cell_m=policy["spatial_cell_m"])
|
|
anchors, progress = route_anchors(reference_path, spacing_m=policy["anchor_spacing_m"])
|
|
query_center = np.median(query, axis=0)
|
|
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,
|
|
policy["descriptor_context_m"],
|
|
maximum_points=policy["target_maximum_points"],
|
|
)
|
|
if len(target) < 300:
|
|
continue
|
|
descriptor = radial_height_descriptor(target, np.median(target, axis=0), policy=policy)
|
|
ranked.append(
|
|
RouteCandidate(
|
|
index=index,
|
|
position=position,
|
|
progress_m=float(distance),
|
|
descriptor_distance=float(np.linalg.norm(query_descriptor - descriptor)),
|
|
)
|
|
)
|
|
ranked.sort(key=lambda candidate: (candidate.descriptor_distance, candidate.index))
|
|
return ranked, dict(
|
|
route_anchor_count=len(anchors),
|
|
descriptor_covered_anchor_count=len(ranked),
|
|
descriptor_candidate_count=len(ranked),
|
|
descriptor_scope="entire-selected-route",
|
|
)
|
|
|
|
|
|
def _seed(query_center, target_center, yaw_deg):
|
|
angle = math.radians(yaw_deg)
|
|
rotation = np.array(
|
|
[
|
|
[math.cos(angle), -math.sin(angle), 0.0],
|
|
[math.sin(angle), math.cos(angle), 0.0],
|
|
[0, 0, 1],
|
|
],
|
|
dtype=float,
|
|
)
|
|
matrix = np.eye(4)
|
|
matrix[:3, :3] = rotation
|
|
matrix[:3, 3] = np.asarray(target_center) - rotation @ np.asarray(query_center)
|
|
return matrix
|
|
|
|
|
|
def _rejected_attempt(message, initial):
|
|
return dict(
|
|
status="rejected",
|
|
reasons=[message],
|
|
T_reference_query=rigid(initial).tolist(),
|
|
initial_T_reference_query=rigid(initial).tolist(),
|
|
overlap=0.0,
|
|
inlier_rmse_m=None,
|
|
matched_query_indices=[],
|
|
localization_confirmed=False,
|
|
vehicle_control=False,
|
|
registration_seconds=0.0,
|
|
)
|
|
|
|
|
|
def _distance(first, second, query_entry):
|
|
a, b = np.asarray(first), np.asarray(second)
|
|
position = float(
|
|
np.linalg.norm(
|
|
transform(np.asarray(query_entry).reshape(1, 3), a)
|
|
- transform(np.asarray(query_entry).reshape(1, 3), b)
|
|
)
|
|
)
|
|
return position, angle_deg(a[:3, :3] @ b[:3, :3].T)
|
|
|
|
|
|
def choose_route_location(attempts, query_entry, *, complete, policy=ROUTE_RELOCALIZATION_POLICY):
|
|
"""Accept one well-separated route location, or expose why we did not."""
|
|
candidates, diagnostics = [], []
|
|
for attempt in attempts:
|
|
result = attempt["result"]
|
|
diagnostic = {k: v for k, v in attempt.items() if k != "result"}
|
|
diagnostic["result"] = {k: v for k, v in result.items() if k != "matched_query_indices"}
|
|
diagnostics.append(diagnostic)
|
|
if result["status"] == "candidate":
|
|
candidates.append(attempt)
|
|
candidates.sort(
|
|
key=lambda attempt: (
|
|
-attempt["result"]["overlap"],
|
|
attempt["result"]["inlier_rmse_m"],
|
|
attempt["candidate"]["index"],
|
|
attempt["yaw_deg"],
|
|
)
|
|
)
|
|
clusters = []
|
|
for attempt in candidates:
|
|
for cluster in clusters:
|
|
if all(
|
|
_distance(
|
|
attempt["result"]["T_reference_query"],
|
|
other["result"]["T_reference_query"],
|
|
query_entry,
|
|
)[0]
|
|
<= policy["cluster_position_m"]
|
|
and _distance(
|
|
attempt["result"]["T_reference_query"],
|
|
other["result"]["T_reference_query"],
|
|
query_entry,
|
|
)[1]
|
|
<= policy["cluster_rotation_deg"]
|
|
for other in cluster
|
|
):
|
|
cluster.append(attempt)
|
|
break
|
|
else:
|
|
clusters.append([attempt])
|
|
# Keep the remaining distinct hypotheses for disjoint fresh confirmation.
|
|
# Their ambiguity is evaluated again relative to the remaining queue, not
|
|
# inherited from the best hypothesis after it has been rejected.
|
|
queue = []
|
|
for index, cluster in enumerate(clusters):
|
|
best = cluster[0]
|
|
ambiguous = any(
|
|
alternative[0]["result"]["overlap"]
|
|
>= best["result"]["overlap"] - policy["ambiguity_overlap_margin"]
|
|
and alternative[0]["result"]["inlier_rmse_m"]
|
|
<= best["result"]["inlier_rmse_m"] + policy["ambiguity_rmse_margin_m"]
|
|
for alternative in clusters[index + 1 :]
|
|
)
|
|
queue.append(dict(
|
|
candidate_index=best["candidate"]["index"],
|
|
route_progress_m=best["candidate"]["progress_m"],
|
|
T_reference_query=best["result"]["T_reference_query"],
|
|
overlap=best["result"]["overlap"],
|
|
inlier_rmse_m=best["result"]["inlier_rmse_m"],
|
|
ambiguous=ambiguous,
|
|
))
|
|
reason = None
|
|
if not complete:
|
|
reason = "incomplete-route-search"
|
|
elif not clusters:
|
|
reason = "no-route-location"
|
|
elif queue[0]["ambiguous"]:
|
|
# Distinctness comes from fitted SE(3), not the retrieval anchor: two
|
|
# seeds at one anchor can converge to different places or directions.
|
|
reason = "ambiguous-route-location"
|
|
selected = (
|
|
dict(clusters[0][0]["result"])
|
|
if clusters
|
|
else _rejected_attempt(
|
|
"Ни один кандидат маршрута не прошёл геометрическую проверку.", np.eye(4)
|
|
)
|
|
)
|
|
selected.update(
|
|
status="rejected" if reason else "candidate",
|
|
reasons=[reason] if reason else [],
|
|
matched_query_indices=[] if reason else selected.get("matched_query_indices", []),
|
|
localization_confirmed=False,
|
|
vehicle_control=False,
|
|
)
|
|
selected["initialization"] = dict(
|
|
policy=policy,
|
|
scope=policy["scope"],
|
|
complete=complete,
|
|
reason=reason,
|
|
expected_attempts=len(attempts),
|
|
attempts=diagnostics,
|
|
candidate_queue=queue if complete else [],
|
|
selected_candidate_index=clusters[0][0]["candidate"]["index"] if clusters else None,
|
|
selected_route_progress_m=(clusters[0][0]["candidate"]["progress_m"] if clusters else None),
|
|
clusters=[
|
|
dict(
|
|
candidate_indices=sorted({item["candidate"]["index"] for item in cluster}),
|
|
route_progress_m=cluster[0]["candidate"]["progress_m"],
|
|
support=len(cluster),
|
|
overlap=cluster[0]["result"]["overlap"],
|
|
rmse_m=cluster[0]["result"]["inlier_rmse_m"],
|
|
)
|
|
for cluster in clusters
|
|
],
|
|
)
|
|
selected["registration_seconds"] = sum(
|
|
item["result"].get("registration_seconds", 0.0) for item in attempts
|
|
)
|
|
return selected
|
|
|
|
|
|
def relocalize_route(
|
|
reference,
|
|
reference_path,
|
|
query,
|
|
query_entry,
|
|
*,
|
|
clock=time.monotonic,
|
|
policy=ROUTE_RELOCALIZATION_POLICY,
|
|
on_progress=None,
|
|
additional_hypotheses=(),
|
|
):
|
|
"""Run complete candidate retrieval and qualification against a selected route."""
|
|
started = clock()
|
|
reference, query = route_reference_cloud(reference), cloud(query)
|
|
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, on_progress=on_progress
|
|
)
|
|
attempts, evaluated, batches = [], [], []
|
|
query_center = np.median(query, axis=0)
|
|
radius = max(
|
|
policy["descriptor_context_m"],
|
|
float(np.linalg.norm(query - query_center, axis=1).max())
|
|
+ policy["target_context_margin_m"],
|
|
)
|
|
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 len(evaluated) % batch_size == 0:
|
|
batches.append([])
|
|
target = local_submap(
|
|
grid, candidate.position, radius, maximum_points=policy["target_maximum_points"]
|
|
)
|
|
if len(target) < 300:
|
|
# Descriptor-admitted geometry unexpectedly disappeared. Do not
|
|
# call this a complete negative search or silently skip the place.
|
|
break
|
|
target_center = np.median(target, axis=0)
|
|
count_before = len(attempts)
|
|
prepared = None
|
|
for yaw_deg, seed_mode in product(
|
|
_yaw_candidates(query, target, query_center, target_center, policy=policy),
|
|
policy["seed_modes"],
|
|
):
|
|
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.
|
|
if prepared is None:
|
|
prepared = PreparedReference(target)
|
|
result = prepared.register(
|
|
query, initial, policy=policy["registration_policy"]
|
|
)
|
|
except ValueError as exc:
|
|
result = _rejected_attempt(str(exc), initial)
|
|
attempts.append(
|
|
dict(
|
|
candidate=dict(
|
|
index=candidate.index,
|
|
position=candidate.position.tolist(),
|
|
progress_m=candidate.progress_m,
|
|
descriptor_distance=candidate.descriptor_distance,
|
|
),
|
|
yaw_deg=yaw_deg,
|
|
seed_mode=seed_mode,
|
|
result=result,
|
|
)
|
|
)
|
|
if len(attempts) - count_before != fits_per_place:
|
|
break
|
|
evaluated.append(candidate.index)
|
|
batches[-1].append(candidate.index)
|
|
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,
|
|
expected_attempts=expected,
|
|
evaluated_candidate_indices=evaluated,
|
|
remaining_candidate_indices=[c.index for c in ranked if c.index not in evaluated],
|
|
candidate_batches=batches,
|
|
candidate_queue_exhausted=complete,
|
|
)
|
|
return result
|
|
|
|
|
|
def _route_start_context(reference, reference_path, query, query_entry, reference_position=None):
|
|
"""Prepare the established dense start target without shrinking the scene.
|
|
|
|
The selected route's first point is still a valuable, explicitly chosen
|
|
laboratory datum. After loss, the last confirmed place takes its role.
|
|
This target preserves the precise local map representation used
|
|
by the successful start-area runs instead of voxelising a broad whole-route
|
|
crop before GICP has a chance to converge.
|
|
"""
|
|
reference, query = route_reference_cloud(reference), cloud(query)
|
|
path, _lengths = _valid_path(reference_path)
|
|
entry = np.asarray(query_entry, dtype=float).reshape(3)
|
|
initial = np.eye(4)
|
|
anchor = path[0] if reference_position is None else np.asarray(reference_position, dtype=float)
|
|
if anchor.shape != (3,) or not np.isfinite(anchor).all():
|
|
raise ValueError("Некорректная область восстановления привязки.")
|
|
initial[:3, 3] = anchor - entry
|
|
forward = next(
|
|
(point - path[0] for point in path[1:] if np.linalg.norm((point - path[0])[:2]) >= 3),
|
|
None,
|
|
)
|
|
if forward is None:
|
|
raise ValueError("Reference lacks a usable route basis.")
|
|
target, window = reference_window(
|
|
reference,
|
|
dict(points=query, path=np.asarray([entry])),
|
|
initial,
|
|
initializing=True,
|
|
)
|
|
return target, query, initial, entry, forward, window
|
|
|
|
|
|
def _stage_attempts(stage, initialization):
|
|
"""Keep every fit auditable while retaining its stage of the hybrid search."""
|
|
return [dict(stage=stage, **attempt) for attempt in initialization.get("attempts", [])]
|
|
|
|
|
|
def _hybrid_initialization(policy, start_result, route_result=None):
|
|
"""Normalize two numerical stages for StationaryBootstrap's strict gate."""
|
|
start = start_result["initialization"]
|
|
attempts = _stage_attempts("dense-start", start)
|
|
expected = start.get("expected_attempts", len(attempts))
|
|
stages = [
|
|
dict(
|
|
name="dense-start",
|
|
status=start_result["status"],
|
|
reason=start.get("reason"),
|
|
complete=start.get("complete", False),
|
|
elapsed_s=start.get("elapsed_s"),
|
|
expected_attempts=expected,
|
|
target_window=start.get("target_window"),
|
|
reference_position=start.get("reference_position"),
|
|
)
|
|
]
|
|
selected = dict(
|
|
selected_candidate_index=0 if start_result["status"] == "candidate" else None,
|
|
selected_route_progress_m=start.get("route_progress_m", 0.0)
|
|
if start_result["status"] == "candidate"
|
|
else None,
|
|
)
|
|
reason = start.get("reason")
|
|
complete = bool(start.get("complete"))
|
|
if route_result is not None:
|
|
route = route_result["initialization"]
|
|
attempts.extend(_stage_attempts("route-recovery", route))
|
|
expected += route.get("expected_attempts", len(route.get("attempts", [])))
|
|
stages.append(
|
|
dict(
|
|
name="route-recovery",
|
|
status=route_result["status"],
|
|
reason=route.get("reason"),
|
|
complete=route.get("complete", False),
|
|
elapsed_s=route.get("elapsed_s"),
|
|
expected_attempts=len(route.get("attempts", [])),
|
|
descriptor_scope=route.get("descriptor_scope"),
|
|
expected_attempts_total=route.get("expected_attempts"),
|
|
evaluated_candidate_indices=route.get("evaluated_candidate_indices"),
|
|
remaining_candidate_indices=route.get("remaining_candidate_indices"),
|
|
candidate_batches=route.get("candidate_batches"),
|
|
)
|
|
)
|
|
selected = dict(
|
|
selected_candidate_index=route.get("selected_candidate_index"),
|
|
selected_route_progress_m=route.get("selected_route_progress_m"),
|
|
candidate_queue=route.get("candidate_queue", []),
|
|
)
|
|
reason = route.get("reason")
|
|
complete = bool(route.get("complete"))
|
|
return dict(
|
|
policy=policy,
|
|
scope=policy["scope"],
|
|
strategy=policy["strategy"],
|
|
complete=complete,
|
|
reason=reason,
|
|
expected_attempts=expected,
|
|
attempts=attempts,
|
|
stages=stages,
|
|
**selected,
|
|
)
|
|
|
|
|
|
def relocalize_start_then_route(
|
|
reference,
|
|
reference_path,
|
|
query,
|
|
query_entry,
|
|
*,
|
|
clock=time.monotonic,
|
|
policy=ROUTE_RELOCALIZATION_POLICY,
|
|
reference_position=None,
|
|
route_only=False,
|
|
on_progress=None,
|
|
):
|
|
"""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, 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,
|
|
initial,
|
|
entry,
|
|
forward,
|
|
clock=clock,
|
|
policy={**STATIONARY_POLICY, "deadline_s": None},
|
|
progress=on_progress,
|
|
)
|
|
start_result["initialization"].update(
|
|
scope=policy["scope"],
|
|
target_window=window,
|
|
query_radius_m=policy["query_radius_m"],
|
|
reference_position=(np.asarray(query_entry) + initial[:3, 3]).tolist(),
|
|
route_progress_m=float(
|
|
np.r_[0.0, np.cumsum(np.linalg.norm(np.diff(reference_path, axis=0), axis=1))][
|
|
np.argmin(
|
|
np.linalg.norm(
|
|
np.asarray(reference_path) - (np.asarray(query_entry) + initial[:3, 3]),
|
|
axis=1,
|
|
)
|
|
)
|
|
]
|
|
),
|
|
)
|
|
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
|
|
|
|
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)
|
|
return route_result
|