Global localization G3 + Boreas crop-cliff root-cause (#91)

* Resolve issue #54/#55 as non-bugs; document executor invariant

#55 (odom_frame_id_ unused) is stale: the field is wired into the
map->odom TF broadcast in publishMapToOdomTransform.

#54 (corrent_pose_with_cov_stamped_ptr_ unlocked) is not a race: the
node is spun by a SingleThreadedExecutor with the default
mutually-exclusive callback group, so the shared pose pointer is
serialized without a lock. Documented that invariant at the executor
site and the member declaration so a future MultiThreadedExecutor /
Reentrant-group change cannot regress it silently.

Also records a Phase 1 correction in development_plan.md: the
SMALL_GICP and SMALL_VGICP backends are already fully wired, so the
Koide backend comparison is a build-and-benchmark task, not new
implementation.

* Add G3 guarded automatic reinitialization (policy + supervisor node)

Close the global-localization recovery loop. The localization component
already raises /reinitialization_requested from its C++ recovery
supervisor; this adds the consumer that queries the G2 service and
republishes /initialpose, behind explicit safety guards.

- scripts/reinitialization_supervisor_policy.py: ROS-free deterministic
  state machine owning every safety decision (candidate-score floor,
  minimum reset spacing, post-reset recovery evidence, non-self-resetting
  attempt ceiling, edge-triggered re-arming). Built around the Phase 3
  lesson that a closed loop must not reset the bounds that protect it.
- scripts/reinitialization_supervisor_node.py: thin opt-in ROS shell
  wiring /reinitialization_requested + /alignment_status -> G2 ~/query
  service -> /initialpose. Not in the default launch.
- test/test_reinitialization_supervisor_policy.py: the roadmap-required
  regression test that fails on unsafe publication or false acceptance;
  10 adversarial sequences, all green.

Docs: CHANGELOG, global_localization_roadmap.md G3 entry, and the
development_plan execution log. Runtime smoke on the Koide
kidnapped-start window is still pending an idle machine.

* Phase 2: root-cause the Boreas local_map_crop_too_small cliff (code-level)

Read the crop path end to end and pin down the failure chain without an
idle-machine run:

- the local-map crop is centered on the *predicted* seed, not the last
  accepted pose, so a runaway prediction starves its own target cloud
  (a positive-feedback divergence);
- with local_map_radius=300 and local_map_min_points=100,
  local_map_crop_too_small is a lagging symptom (center must be ~300 m
  off-map), not an independent map fault at the cliff instant;
- the crop-failure guard then drops every scan until a new initial pose
  arrives, freezing the estimate into the flat ~45 m plateau.

Two candidate triggers remain with opposite fixes: prediction
divergence (estimator-side) vs map coverage / map-split (map-side). Add
scripts/diagnose_local_map_crop_coverage.py to decide between them
offline -- it counts map points within local_map_radius of every
ground-truth pose and flags the first under-covered time -- with the
counting core unit-tested in test/test_local_map_crop_coverage.py.
Running it is blocked only on the Boreas map/reference data mount.

Docs: development_plan.md Phase 2 root-cause analysis, CHANGELOG.

* Add global localization operations guide (G2 service + G3 supervisor)

docs/global_localization.md is the how-to-run companion to the
design-focused roadmap: launch commands for the opt-in G2 service and
G3 supervisor nodes, their parameters, the wiring between them, and a
table of the supervisor's safety guards. Cross-linked from the README
(demo section + docs index) and the roadmap G3 section.

* Fix coverage diagnostic: subsample undercount and brute-force memory

Self-review of the diagnostic found two bugs:

- Subsampling the map by stride S divided the in-radius counts by ~S,
  which would trip a *false* local_map_crop_too_small cliff against the
  absolute min_points (e.g. a real >1000-point neighbourhood read as
  ~100). Default to no subsample (a KD-tree handles multi-million-point
  maps fine); when subsampling is explicitly requested, scale the counts
  back by the stride so min_points stays comparable, and print a note.
- The brute-force fallback built a (chunk x map) array, which could OOM
  on a real map; bound the effective chunk to ~20M elements.

Adds test_count_scale_compensates_for_subsampling, which fails on the
unscaled path and passes on the scaled one.

* G3 runtime smoke: validate the supervisor node end-to-end; fix shutdown

Ran the supervisor node under ROS for the first time. Two outcomes:

- Found and fixed a shutdown bug: main() caught only KeyboardInterrupt,
  so an external SIGTERM / context shutdown raised an uncaught
  ExternalShutdownException and dumped a traceback. Now catches both and
  guards rclpy.shutdown() with rclpy.ok().
- Added test/test_reinitialization_supervisor_node_ros.py, an rclpy
  integration smoke (skipped without a sourced ROS env) that fakes the
  localizer + G2 service and asserts the full glue: the node receives
  /reinitialization_requested + /alignment_status, debounces, calls the
  G2 ~/query service, parses the candidate JSON, and publishes
  /initialpose (x/y/yaw + 0.5 m std covariance) before moving to the
  settling state. Validates the ROS glue the pure-policy unit tests
  cannot.

* Record G3 runtime-glue validation in the roadmap

* G3: add 3-node recovery bringup launch; install supervisor scripts

Author the launch the post-reset recovery-evidence gate needs:
global_localization_recovery.launch.py brings up the core localizer (via
lidar_localization.launch.py), the G2 global_localization_node, and the G3
reinitialization_supervisor_node on a shared cloud_topic / global_frame_id,
with the supervisor wired to the G2 ~/query service and the localizer's
/alignment_status and /reinitialization_requested. The min_candidate_score /
max_attempts guards are launch args, coerced with ParameterValue so the node's
float/int parameter types are respected.

Install reinitialization_supervisor_{policy,node}.py via CMakeLists so they
resolve as ros2 run / launch executables (the node imports the policy as a
sibling module, mirroring global_localization_node.py + global_localization_query.py).

Validated: colcon install resolves both executables and the launch file;
ros2 launch --show-args resolves the full argument graph (including the
included localizer pass-through args). Live 3-node replay on the Koide
kidnapped-start window remains the pending recovery-evidence gate.

* G3: live closed-loop result on Koide outdoor_hard_01a + supervisor timeout launch args

Run the three-node bringup against the real bag + map with a kidnap injected at a
healthy location. Two results, documented in docs/g3_live_closed_loop.md:

- The G3 supervisor mechanism is validated end-to-end against a real localizer +
  G2: detect -> debounce -> query -> score-guard -> guarded /initialpose publish ->
  recovery-wait -> retry -> max_attempts ceiling -> safe give-up with operator
  alert. The ceiling contained three confidently-wrong-or-stale candidates without
  an unbounded reset loop (the Phase 3 lesson, validated live).
- Recovery did not complete because of candidate quality, not the supervisor: G2's
  BBS_2D returned high-confidence but wrong candidates (107 m and 190 m off at BBS
  score ~0.99) on 2 of 3 attempts; the correct pose (5.5 m, score 0.998) arrived too
  late. The BBS occupancy score does not separate right from wrong.

Surfaces the evidence-backed next requirement: validate a candidate by registration
fitness before publishing (G2 per-candidate NDT/GICP fitness, and/or walk the ranked
candidate list) rather than trusting the BBS score.

Expose supervisor query_timeout_sec / settle_timeout_sec as launch arguments
(ParameterValue float coercion) so the loop can be made patient enough for the
~10-25 s BBS query latency; this run used query_timeout_sec:=30 settle_timeout_sec:=20.

* G3: ranked-candidate walk so the localizer fitness adjudicates aliased candidates

The live closed-loop run showed G2's BBS occupancy score does not separate a
190 m-wrong candidate (0.99) from the 5.5 m-correct one (0.998), so a score floor
alone cannot pick the right reset. The localizer's NDT fitness can. So the
supervisor now walks the ranked candidate list from a single query: publish the
best, and if fitness does not recover within settle_timeout_sec, publish the
next-best from the same query, letting the localizer's fitness reject aliased poses.

- G2 (global_localization_node) returns the full ranked 'candidates' list in the
  ~/query reply; 'top' kept for back-compat.
- Policy carries the ranked scores + a candidate index; on a failed settle it walks
  to the next candidate above min_candidate_score instead of cooling down. Walking
  does NOT spend a max_attempts slot -- only re-querying does -- so the ceiling stays
  a true bound on queries while one query can try every pose it found. A wrong reset
  is rejected by the localizer (high fitness, no accept), so walking cannot cause the
  Phase 3 acceptance blowup.
- Supervisor node delivers the ranked scores to the policy and publishes the
  candidate at the policy's requested index.

Regression tests: policy walk recovers on a lower-ranked candidate without spending
attempts; list-exhaust still respects max_attempts and gives up; the score floor
truncates the walk. ROS integration test: the node walks 0->1 within one query and
recovers (query_calls==1). 21 related tests green. Live validation of the walk on
the kidnap window is pending (does the real BBS top-K contain the true pose).

* G3: live walk run pins the blocker to BBS query latency; expose G2 speed args

Second live kidnap run with the ranked-candidate walk enabled corrects the earlier
candidate-quality diagnosis (docs/g3_live_closed_loop.md):

- The walk is live-validated: the supervisor walked all 16 candidates from one query
  (each a next_candidate transition, published by index), kept every guard, then
  re-queried -- exactly as designed.
- Candidate generation was fine: BBS put the TRUE pose at rank 1 (-107.1, 18.44,
  score 1.0; ground truth -107.2, 17.7 at query-issue time, ~0.8 m off).
- It still did not recover because the BBS query took ~23 s (runtime_sec 23.376). The
  candidate is computed on the query-issue scan but published ~23 s later; at the
  replay rate the moving vehicle was ~12-17 m away by then, outside the registration
  basin, so the localizer rejected the now-stale correct pose. Every candidate shares
  that stale scan, so the walk cannot rescue it.

So the primary lever for live recovery is G2 query speed, not candidate quality or
ranking. Expose G2 search-cost parameters as launch args (g2_angular_resolution_deg,
g2_max_scan_points, g2_pyramid_depth, g2_max_candidates) so a faster/coarser config can
be selected for the runtime loop and trade a little accuracy for a fresher seed.

* G3: fast-G2 run shows query speedup is necessary but not sufficient

Exposing G2 search cost as launch args and running coarse (yaw 10 deg, 256 pts) cut
the BBS query from ~23 s to ~4-8 s. Recovery still did not complete: the walk spends
settle_timeout_sec on each earlier candidate, so a near-correct candidate that is not
rank 1 (here rank 3, -107.9/12.0) is published ~20 s after the query and is stale
again. The latency budget that matters is query + rank*settle, not just the query.

Caveat for this run: the one-shot seed publisher lost a transient-local discovery race
at the slower replay rate, so the healthy baseline did not establish; the query-timing
and walk-latency observations stand regardless. Documented in g3_live_closed_loop.md
along with the real levers (cut query hard AND keep truth at rank 1, or motion-compensate
the seed, or scope to low-speed kidnaps) and a harness note to republish the seed.

* G3: clean run isolates the blocker to the localizer post-reset re-lock path

Fourth live run, with the seed harness fixed, had a healthy baseline, a fast query
(~5 s), and a correct fresh rank-1 candidate all at once: query 1's top was
(-106.10, 9.04, 150 deg) vs ground truth (-106.8, 8.6, 156 deg) -- 0.8 m and 6 deg
off in full 3-DOF. It still did not re-lock: the localizer went tracking -> recovering
(reject_measurement), streamed fitness > 6, and sat at the seed with fitness ~27 -- at
a pose where the same area scored 0.23 in healthy tracking.

This eliminates everything upstream of the localizer (candidate generation, ranking,
the walk, query latency, yaw). The decisive blocker is the localizer's own post-reset
re-lock path not re-converging from a correct externally-published /initialpose during
the recovering state -- likely the same init_guess / local-map-crop centering the
Boreas root-cause flagged. Confirming needs code-level C++ investigation, not more
replays; the five runs isolated it by elimination. Documented in g3_live_closed_loop.md.

* G3: carry z/roll/pitch from localizer pose onto 2D candidates (the re-lock blocker)

Code investigation of the failed re-lock found the root cause, and it is upstream of
the localizer after all: G2 candidates are 2D (x, y, yaw; z=seed_z_m=0), and the
supervisor published the reset with z=0. On the Koide outdoor map the true z is ~-11 m,
so every reset seeded the pose ~11 m above ground -- outside the NDT z-basin -- and the
lock never took even when x/y/yaw were correct (the fourth live run's rank-1 candidate
was 0.8 m / 6 deg off in x/y/yaw yet held fitness ~27). It worked on the flat HDL demo
only because z=0 was right there.

Fix: the supervisor subscribes to the localizer pose (/pcl_pose) and carries its
z / roll / pitch onto the candidate, overriding only x / y / yaw -- the vehicle's height
and attitude drift slowly even while xy tracking is lost, so the last pose is a good
carry-over. Falls back to reset_default_z_m if no pose seen yet. Adds _quat_from_rpy /
_roll_pitch_from_quat helpers and pose_topic / reset_default_z_m parameters.

Regression test: a faked /pcl_pose at z=-11.05 makes the published reset use z=-11.05,
not 0. 16 related tests green (13 policy + 3 ROS).

* G3: recovery-evidence gate met live -- z-carry fix closes the closed loop

With the z/roll/pitch carry-over fix, the clean kidnap run recovers end-to-end on the
real Koide window: the reset now publishes z~-9.4/-7.3 (carried from /pcl_pose), a
candidate locks, fitness falls from ~30 to 0.10, and the supervisor logs
recovery_confirmed -> standdown -> idle (re-armed). The last G3 Decision Gate --
post-reset recovery evidence with a real localizer -- is satisfied.

Document the root cause (2D candidate z=0 vs ~-11 m true ground, outside the NDT
z-basin) and the success in g3_live_closed_loop.md, and mark the gate met in the
roadmap. Remaining work is quality (faster/first-try recovery via lower query latency
and per-candidate registration scoring), not correctness.

* G3: cap candidate walk depth so a stale query re-queries on a fresher scan

The live kidnap run spent ~95 s walking all 16 candidates of a stale first
query (none of whose poses were the true location) before re-querying -- and
it was the second query, on a newer more distinctive scan, that returned the
true pose at rank 1 and locked. Past the top few BBS occupancy maxima a fresh
query beats a deeper walk, so add max_walk_candidates (default 4): abandon the
query after the top few fail to lock and re-query instead of grinding ranks
5-16. Pure policy change with two unit tests; wired through the node param and
the supervisor_max_walk_candidates launch arg (set high to restore full walk).

Author: rsasaki0109

CHANGELOG.md

@@ -16,6 +16,21 @@
   ROS-free query logic in `scripts/global_localization_query.py`
 - `scripts/render_global_localization_demo_gif.py` renders the
   kidnapped-start -> global localization -> tracking-resume demo GIF
+- `scripts/diagnose_local_map_crop_coverage.py` decides offline whether a Boreas
+  `local_map_crop_too_small` cliff is a map-coverage problem or prediction-driven
+  divergence, by counting map points within `local_map_radius` of every
+  ground-truth pose (no ROS / no localizer); core unit-tested in
+  `test/test_local_map_crop_coverage.py`
+- G3 guarded automatic reinitialization: `scripts/reinitialization_supervisor_node.py`
+  connects `/reinitialization_requested` to the G2 query service and republishes
+  `/initialpose` only when explicit safety guards pass (opt-in node; default
+  launch unchanged). The decision logic is the ROS-free state machine in
+  `scripts/reinitialization_supervisor_policy.py` -- candidate-score floor, minimum
+  reset spacing, post-reset recovery evidence, and a non-self-resetting attempt
+  ceiling -- regression-tested against unsafe-publication and false-acceptance
+  sequences in `test/test_reinitialization_supervisor_policy.py`
+- `docs/global_localization.md`: operations guide for running the G2 service and
+  the G3 supervisor (commands, parameters, and the supervisor's safety guards)
 
 ### Changed
 
@@ -27,6 +42,9 @@
   coverage ≥ 96 % on the calibration runs); the v1.1 heuristic remains
   available as `pose_covariance_mode: fitness_scaled` (docs:
   `pose_covariance.md`, issue #72)
+- Documented the single-threaded-executor invariant that serializes the node's
+  shared pose state without locks, so a future executor change cannot silently
+  introduce a data race (issue #54)
 
 ## 1.1.0 - 2026-06-11
 

CMakeLists.txt

@@ -140,6 +140,8 @@ install(PROGRAMS
   scripts/make_bbs_relocalization_attempts.py
   scripts/global_localization_query.py
   scripts/global_localization_node.py
+  scripts/reinitialization_supervisor_policy.py
+  scripts/reinitialization_supervisor_node.py
   scripts/make_registration_relocalization_jobs.py
   scripts/scaffold_mapless_public_dataset_bundle.py
   scripts/fetch_official_autoware_istanbul_dataset.sh

README.md

@@ -91,9 +91,14 @@ of the run.
 
 <img src="./images/global_localization_demo.gif" alt="Kidnapped start, BBS_2D global localization candidates, NDT tracking resumes" width="480">
 
-The service node is opt-in and never part of the default launch. Status and
-limits (query latency, validated windows) are tracked in
-[docs/global_localization_roadmap.md](docs/global_localization_roadmap.md).
+The service node is opt-in and never part of the default launch. An optional
+supervisor (`scripts/reinitialization_supervisor_node.py`) can close the loop
+automatically — on the `/reinitialization_requested` signal it queries the service
+and re-seeds `/initialpose` behind explicit safety guards. See
+[docs/global_localization.md](docs/global_localization.md) for how to run the
+service and the supervisor, and
+[docs/global_localization_roadmap.md](docs/global_localization_roadmap.md) for
+status and limits (query latency, validated windows).
 
 ## Quick Start
 
@@ -283,6 +288,7 @@ git branch -D <branch>
 | Experiment interfaces and decisions | [docs/interfaces.md](docs/interfaces.md), [docs/experiments.md](docs/experiments.md), [docs/decisions.md](docs/decisions.md) |
 | Roadmap | [docs/competitive_roadmap.md](docs/competitive_roadmap.md) |
 | Development plan | [docs/development_plan.md](docs/development_plan.md) |
+| Global localization (run it) | [docs/global_localization.md](docs/global_localization.md) |
 | Global localization phases | [docs/global_localization_roadmap.md](docs/global_localization_roadmap.md) |
 | Reliability / open issues | [docs/reliability_roadmap.md](docs/reliability_roadmap.md) |
 | Bringup troubleshooting | [docs/troubleshooting.md](docs/troubleshooting.md) |

docs/development_plan.md

@@ -77,16 +77,18 @@ by the maintainer; this table is the triage record.
 | 37 | CPU占用率爆炸 | Answered — `failure_category: overload` diagnostic + throughput tuning (`voxel`, `ndt threads`, `cloud_queue_depth`) in `troubleshooting.md`; close with pointer |
 | 49 | ERROR run with Rslidar | Answered — point-type / remap guidance; close with pointer (ask for log if it recurs) |
 | 50 | enhance stability (try/catch) | Partially shipped — Phase 0 crash-survival fixes (#76/#56/#47) + Phase 3 diagnostics; reply with what shipped, keep open for broader hardening |
-| 55 | `odom_frame_id_` defined but not used | Actionable code cleanup — verify and remove or wire it; small win |
-| 54 | `corrent_pose_with_cov_stamped_ptr_` not locked | Actionable — confirm the single-threaded executor assumption or add a guard; small win |
+| 55 | `odom_frame_id_` defined but not used | Resolved (2026-06-14) — stale report; the field is wired into the `map -> odom` TF path (`publishMapToOdomTransform`), no code change needed |
+| 54 | `corrent_pose_with_cov_stamped_ptr_` not locked | Resolved (2026-06-14) — the node runs on a `SingleThreadedExecutor` with the default mutually-exclusive callback group, so the shared pose pointer is serialized without a lock; documented the invariant in the node + header + CHANGELOG so a future executor change can't regress it silently |
 | 52 | different results (degrades each restart, worse after reboot) | Needs investigation — restart-to-restart degradation suggests state/seed leak, not documented run variance; keep open |
 | 68 | MGRS map not displayed in Rviz | Needs investigation — large-coordinate PCD likely hits float32 precision in the map path; reporter offered to implement, give a hint and keep open |
 | 77 | IMU angular velocity + estimator | Future enhancement track (IMU); keep open |
 | 36 | imu preintegration | Future enhancement track (IMU), overlaps #77; keep open |
 
-Two cheap code wins (#55, #54) can land in the next `Unreleased` batch; the doc-answered
-set (#25/#33/#34/#37/#49) is ready to close once the maintainer posts the pointer
-comments; #52 and #68 want a reproduction before any claim.
+The two cheap code wins (#55, #54) were investigated on 2026-06-14: both turned out
+to be non-bugs under the shipped configuration (#55 already wired; #54 protected by the
+single-threaded executor) and were closed with documenting changes rather than fixes.
+The doc-answered set (#25/#33/#34/#37/#49) is ready to close once the maintainer posts
+the pointer comments; #52 and #68 want a reproduction before any claim.
 
 Branch hygiene: `origin` carries ten non-`main` branches. Five are stale merged feature
 branches fully contained in `main` (`codex/mid360-policy-split`,
@@ -166,6 +168,54 @@ Done when:
 - RMSE is in a range where backend and IMU comparisons are meaningful
 - Boreas earns a defined role in the regression or benchmark suite
 
+### Root-cause analysis (2026-06-14, code-level; data run still pending)
+
+Read the crop path end to end to narrow step 1 before spending an idle-machine run.
+Findings, all from the shipped code and `param/boreas_ndt_velodyne.yaml`:
+
+- **The crop is centered on the *predicted* seed, not the last accepted pose.** The
+  primary attempt is `runAlignmentAttempt(init_guess, init_guess, ...)`
+  (`lidar_localization_component.cpp:1969`), and `init_guess` is the twist/IMU
+  prediction (`:1710-1753`). `setInputTargetForPose` crops `full_map_cloud_ptr_`
+  around that center (`:1810`). Only the *recovery retry* re-centers on
+  `last_accepted_pose_matrix_` (`:1980`). So a runaway prediction starves its own
+  target cloud — a positive-feedback divergence, exactly the closed-loop shape
+  Phase 3 warned about.
+- **`local_map_crop_too_small` is a lagging symptom here, not a primary cause.** With
+  `local_map_radius: 300` and `local_map_min_points: 100`, a 300 m disk around any
+  on-map center holds far more than 100 points. For the count to fall under 100 the
+  center must be roughly the map radius (~300 m) off the mapped area — i.e. the pose
+  has *already* diverged. So the reported `local_map_crop_too_small` is downstream of
+  whatever starts the divergence, not an independent map problem at the cliff instant.
+- **The crop-failure guard is what freezes the run into a flat 45 m plateau.** After a
+  streak of crop failures the component activates `crop_failure_guard`, resets the
+  prediction to the last accepted pose, and then *drops every subsequent scan until a
+  new initial pose arrives* (`:1503-1521`). With no auto-reinitialization wired in the
+  Boreas config, the estimate is frozen at the last good pose while the vehicle keeps
+  driving — RMSE then grows linearly to ~45 m. (G3, just landed, is the mechanism that
+  could break this freeze automatically; Boreas is its natural second test scenario.)
+
+This leaves two candidate triggers for the *initial* divergence, with opposite fixes:
+
+  (A) **prediction divergence** — a twist-prediction glitch or a single bad accepted
+      match throws the seed, and the predicted-center crop then guarantees the next
+      reject; fix is estimator-side (crop around last-accepted instead of the raw
+      seed, and/or harden the twist/reject path).
+  (C) **map coverage** — the Boreas map (`*_rebuild_loc_frame.pcd`, a GT-aligned
+      rebuild) does not cover the full 60 s route, so once the vehicle passes the
+      mapped region the crop around the *true* pose legitimately falls below
+      `min_points`; fix is map-side (extend / retile the map).
+
+`scripts/diagnose_local_map_crop_coverage.py` decides (A) vs (C) offline, with no ROS
+and no localizer: it counts map points within `local_map_radius` of every ground-truth
+pose and reports the first time that count drops below `local_map_min_points`. If the
+true trajectory stays covered, a real `local_map_crop_too_small` is prediction-driven
+(A); if coverage collapses at the cliff time, it is a map-split (C). The counting core
+is unit-tested in `test/test_local_map_crop_coverage.py`. **Blocked only on data**: the
+Boreas map PCD and reference CSV live on the `/media/autoware/aa/...` mount, which is
+not currently attached; run the diagnostic the moment it is back to fix the root cause
+before any runtime change (no unreplayed estimator edits — the Phase 3 rule).
+
 ## Phase 3: Measurement Acceptance, Diagnostics, Covariance — DONE (2026-06-12)
 
 Goal was to move acceptance and failure detection beyond the scalar fitness score, so
@@ -496,6 +546,69 @@ branches are stale and fully in `main` (`codex/mid360-policy-split`,
 stale 2024 README commit (not the backend integration its name implies). The distro
 branches (`dashing`/`foxy`/`humble`/`jazzy`) are kept as user checkout points.
 
+### 2026-06-14: #55 / #54 closed as non-bugs with documentation
+
+Followed up on the two "small code wins" from the triage. Both turned out to be stale
+or already-safe under the shipped configuration, so neither warranted a behavioral
+change:
+
+- **#55 (`odom_frame_id_` unused)** — stale. The field is read in
+  `publishMapToOdomTransform` to look up `odom -> base` and broadcast the
+  `map -> odom` TF whenever `enable_map_odom_tf_` is set. Nothing to remove.
+- **#54 (`corrent_pose_with_cov_stamped_ptr_` unlocked)** — not a race. `main` spins the
+  node on a `SingleThreadedExecutor` (`lidar_localization_node.cpp`) and no callback
+  opts into a Reentrant group, so every subscription/timer/service callback runs in one
+  mutually-exclusive group on one thread; the shared pose pointer is serialized by the
+  executor, not a lock. The real risk is that this invariant was implicit, so a later
+  switch to `MultiThreadedExecutor` (or a Reentrant group) would silently introduce a
+  data race. Made the invariant explicit instead of adding a now-redundant mutex:
+  comments at the executor construction site and the member declaration, plus a
+  CHANGELOG note.
+
+Comment/doc-only changes, no rebuild required.
+
+**Phase 1 correction (found while reading this code):** the SMALL_GICP *and* SMALL_VGICP
+backends are already fully wired, not a "start fresh" item as previously noted.
+`registration_backend_policy.hpp` maps `SMALL_GICP`/`SMALL_VGICP`, and
+`lidar_localization_component.cpp:998-1013` constructs `small_gicp::RegistrationPCL`,
+selects GICP vs VGICP via `smallGicpRegistrationType`, and sets epsilon / correspondence
+randomness / max-correspondence-distance / `vgicp_voxel_resolution_` / thread count
+before assigning `registration_`. `CMakeLists.txt` already does
+`find_package(small_gicp QUIET CONFIG)` and defines `LIDAR_LOCALIZATION_HAVE_SMALL_GICP`
++ links `small_gicp::small_gicp` when found; without the dependency the backend is a
+clean `RCLCPP_ERROR` + exit. So Phase 1 is a **build-and-benchmark** task, not an
+implementation one: install `small_gicp`, rebuild, then run NDT_OMP vs SMALL_GICP vs
+SMALL_VGICP on an idle machine (still gated on `load < ~5`).
+
+### 2026-06-14: G3 guarded automatic reinitialization (logic complete)
+
+Built the consumer side of the global-localization recovery loop, which was the
+named next item after G1/G2. The producer (`/reinitialization_requested` from the C++
+recovery supervisor) already existed; G3 adds the part that closes the loop while
+respecting the Phase 3 lesson that a closed loop must not be able to reset its own
+bounds. Two new files:
+
+- `scripts/reinitialization_supervisor_policy.py` — a ROS-free, deterministic state
+  machine (`decide(params, state, obs)`) that owns every safety decision: a candidate
+  must clear `min_candidate_score` to be published, resets are spaced by
+  `min_seconds_between_attempts`, after a reset it requires alignment fitness back under
+  `recovery_fitness_threshold` as recovery evidence, and the attempt counter is a true
+  ceiling (`max_attempts`) that only clears on confirmed recovery or request release —
+  never as a side effect of attempting. After a terminal outcome it waits for the request
+  to de-assert (edge-triggered), so a stuck-high request line cannot re-fire it.
+- `scripts/reinitialization_supervisor_node.py` — the thin ROS shell (opt-in, not in the
+  default launch): subscribes `/reinitialization_requested` + `/alignment_status`
+  (reads the `fitness_score` diagnostic), calls the G2 `~/query` Trigger service, and
+  publishes `/initialpose` from the top candidate only when the policy says so.
+
+`test/test_reinitialization_supervisor_policy.py` is the roadmap-required regression
+test that fails on unsafe publication or false acceptance: 10 adversarial sequences
+(transient blip, pure tracking, happy-path single reset + recovery, weak-candidate
+never published, bounded queries, false-acceptance no-loop, reset spacing, budget reset
+on recovery between episodes, exhausted latch, purity). All green. **Pending** (machine-
+or runtime-bound, not logic): a live/replay smoke on the Koide kidnapped-start window
+and capture of the post-reset recovery evidence for the roadmap's evidence gate.
+
 ## Suggested Order Of Work
 
 Phase 0, Phase 3 (all three steps), G1, the BBS speedup, and G2 are complete. Remaining,