"""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