Add SMALL_VGICP Koide manifest for Phase 1 backend comparison

docs/phase1_koide_backend_comparison.md

@@ -0,0 +1,110 @@
+# Phase 1: Koide backend comparison (NDT_OMP vs SMALL_GICP vs SMALL_VGICP)
+
+First result on the locally-available Koide data, `outdoor_hard_01a`, 60 s smoke
+window. Run with the GT-aligned diagnostic-map manifests' data absent; this uses
+the runnable `01a` sequence + `map_outdoor_hard.ply`. Numbers are indicative
+(single 60 s smoke, machine just-idle ~4.5), not a final ranking.
+
+## Headline: RMSE alone is a trap; look at throughput + ok-rate first
+
+A naive translation-RMSE table makes the GICP backends look best, but that is a
+mirage: with full-density Livox scans the GICP backends are throughput-bound and
+produce only a handful of poses, so their RMSE is computed over ~10-15 early
+poses near the seed, not over the window.
+
+Full-density scans (`enable_scan_voxel_filter: false`):
+
+| backend | max align time | poses | ok-rate | trans RMSE | note |
+| --- | --- | --- | --- | --- | --- |
+| NDT_OMP | 0.154 s | 195 | 89.4% | 0.266 m | healthy, tracks the window |
+| SMALL_GICP | 3.425 s | 16 | 51.7% | 0.058 m | throughput-bound (11 non-converged); RMSE is a mirage |
+| SMALL_VGICP | 1.531 s | 11 | 29.4% | 0.060 m | throughput-bound (13 non-converged); RMSE is a mirage |
+
+The GICP backends take 1.5-3.4 s per alignment on full-density scans (10-22x
+NDT), so the node drops most scans and never tracks the window.
+
+## Fair comparison: identical downsampling (voxel 0.5, as NDT)
+
+Re-run with `enable_scan_voxel_filter: true`, `voxel_leaf_size: 0.5` -- identical
+preprocessing to the NDT baseline, isolating the registration backend:
+
+| backend | max align time | poses | ok-rate | trans RMSE | rot RMSE |
+| --- | --- | --- | --- | --- | --- |
+| NDT_OMP | 0.154 s | 195 | 89.4% | 0.266 m | 2.46 deg |
+| **SMALL_GICP** | 0.396 s | 126 | **100%** | **0.095 m** | **2.18 deg** |
+| SMALL_VGICP | 0.630 s | 29 | 38.9% | 0.209 m | 2.56 deg |
+
+### Findings (60 s smoke only -- see the full-window section below, which reverses this)
+
+- **SMALL_GICP is the accuracy/stability winner on this easy window.** With downsampling it is fast
+  enough (0.40 s max align, ~2.6x NDT but real-time-viable at the processed
+  ~2 Hz), 100% ok-rate, and 2.8x better translation than NDT (0.095 vs
+  0.266 m). Notably it had **zero** rejects, while NDT had a 6.2 s
+  reject streak (23 rows, fitness up to 33.7) on the hard part of the window.
+- **NDT_OMP is the fast baseline.** Lowest per-scan cost (0.15 s), processes the
+  most scans (195), but less accurate and rejects ~10% on the hard segment.
+- **SMALL_VGICP is not viable as configured.** Even downsampled, only 38.9%
+  ok-rate (31 threshold rejects + 10 non-converged) at
+  `vgicp_voxel_resolution: 0.5`. The voxelized model needs a finer resolution
+  (e.g. 0.2-0.3) to be competitive on this Livox data -- a tuning follow-up.
+
+### Caveats / next steps
+
+- 60 s smoke only, single run (no repeat for variance), first window. The hard
+  part of `outdoor_hard_01a` is later; a full 380 s run will stress all three.
+- VGICP needs a `vgicp_voxel_resolution` sweep before any verdict.
+- Full-density GICP being throughput-bound is data/sensor-specific (dense Livox
+  returns); on a sparser lidar the trade-off differs.
+
+## Full 380 s window (definitive): the ranking REVERSES
+
+Re-run on the full `outdoor_hard_01a` window (380 s), same identical-downsampling
+config. The 60 s smoke ranking is overturned:
+
+| backend | poses | ok-rate | trans RMSE | rot RMSE | longest lost window |
+| --- | --- | --- | --- | --- | --- |
+| **NDT_OMP** | 476 | 57.3% | **0.668 m** | 11.2 deg | 75.4 s |
+| SMALL_GICP | 87 | 11.2% | 11.70 m | 47.4 deg | 274.5 s |
+| SMALL_VGICP | 35 | 6.7% | 22.68 m | 32.6 deg | 298.4 s |
+
+- **NDT_OMP is the robust winner on the full hard window** (0.668 m). It survives the
+  hard section -- 57% ok, `local_map_crop_too_small: 101`, `fitness_rejected: 247`, and
+  one `recovery_retry_from_last_pose_recovered` -- and ends well-tracked.
+- **SMALL_GICP collapses** (11.70 m): it is lost for 274.5 s (~72% of the run). Once it
+  loses lock on the hard section it never recovers -- fitness explodes (max 1.3M),
+  rejections pile up (523), and the crop starves (`local_map_crop_too_small: 106`).
+- **SMALL_VGICP** is worse still (22.68 m, lost 298 s).
+
+**The lesson:** SMALL_GICP looked like the winner on the easy first 60 s (0.095 m,
+100% ok) but is *fragile* -- more accurate while locked, yet unable to hold lock
+through the hard section, where it diverges catastrophically. NDT_OMP is less
+accurate when locked but far more robust, which is what matters end-to-end. This is
+a textbook case of why a single easy window is necessary-but-not-sufficient (the
+Phase 3 lesson): the smoke ranking was exactly reversed by the full replay.
+
+### Verdict
+
+- **NDT_OMP: recommended default** -- robust across the full hard window.
+- **SMALL_GICP: higher locked-in accuracy, but not a safe default** without
+  robustness work (lock retention / recovery on the hard section).
+- **SMALL_VGICP: not competitive** as configured.
+
+Single run per backend; the divergence magnitude (11.7 m vs 0.67 m) is far beyond
+run-to-run variance, but a repeat would firm up the numbers. A `vgicp_voxel_resolution`
+sweep is still open for VGICP.
+
+## Reproduce
+
+```bash
+source scripts/setup_local_env.sh
+python3 scripts/benchmark_from_manifest \
+  --manifest param/benchmark/koide_hard_localization_outdoor_hard_01a_smoke60.yaml            # NDT_OMP
+python3 scripts/benchmark_from_manifest \
+  --manifest param/benchmark/koide_hard_localization_outdoor_hard_01a_smoke60_small_gicp_ds.yaml
+python3 scripts/benchmark_from_manifest \
+  --manifest param/benchmark/koide_hard_localization_outdoor_hard_01a_smoke60_small_vgicp_ds.yaml
+```
+
+Run only on an idle machine (`load < ~5`) for valid alignment-time numbers. Read
+`trajectory_eval.json` (RMSE) and `health_summary.json` (`max_alignment_time_sec`,
+`ok_rate_percent`, `message_counts`) from each output dir.

param/benchmark/koide_hard_localization_outdoor_hard_01_full_gt_diag_map_small_vgicp.yaml

@@ -0,0 +1,43 @@
+# Koide Hard Point Cloud Localization -- full Outdoor01 on the GT-aligned diagnostic map
+# using SMALL_VGICP (voxelized GICP), the third backend for the Phase 1 comparison.
+
+mode: run
+
+dataset:
+  bag_path: repo://data/public/koide_hard_localization/sequences/outdoor_hard_01
+  map_path: repo://data/public/koide_hard_localization/map_outdoor_hard_full_gt_stride10_voxel05.ply
+  reference_csv: repo://data/public/koide_hard_localization/benchmark/outdoor_hard_01/reference.csv
+  initial_pose_yaml: repo://data/public/koide_hard_localization/benchmark/outdoor_hard_01/initial_pose.yaml
+  cloud_topic: /livox/points
+  imu_topic: /livox/imu
+
+localizer:
+  base_param_yaml: repo://param/nav2_ndt_urban.yaml
+  param_overrides:
+    registration_method: SMALL_VGICP
+    use_imu: false
+    use_imu_preintegration: false
+    enable_scan_voxel_filter: false
+    voxel_leaf_size: 0.2
+    base_frame_id: livox_frame
+    scan_min_range: 1.0
+    scan_max_range: 60.0
+    score_threshold: 15.0
+    local_map_radius: 80.0
+    gicp_max_correspondence_distance: 3.0
+    vgicp_voxel_resolution: 0.5
+  launch_args:
+    - use_sim_time:=true
+    - lidar_frame_id:=livox_frame
+
+benchmark:
+  output_dir: /tmp/lidarloc_koide_outdoor_hard_01_full_gt_diag_map_small_vgicp
+  ros_domain_id: 159
+  bag_duration: 0
+  bag_start_offset: 0
+  settle_seconds: 5
+  post_roll_seconds: 1
+  max_time_diff: 0.05
+  record_qos_reliability: reliable
+  record_qos_durability: volatile
+  record_use_sim_time: true

param/benchmark/koide_hard_localization_outdoor_hard_01a_full380_ndt_omp.yaml

@@ -0,0 +1,39 @@
+# Koide outdoor_hard_01a FULL 380s -- NDT_OMP backend (Phase 1 definitive).
+# Identical downsampling (voxel 0.5) across backends; GICP-scale gate for GICP.
+mode: run
+dataset:
+  bag_path: repo://data/public/koide_hard_localization/sequences/outdoor_hard_01a
+  map_path: repo://data/public/koide_hard_localization/map_outdoor_hard.ply
+  reference_csv: repo://data/public/koide_hard_localization/benchmark/outdoor_hard_01a/reference.csv
+  initial_pose_yaml: repo://data/public/koide_hard_localization/benchmark/outdoor_hard_01a/initial_pose.yaml
+  cloud_topic: /livox/points
+  imu_topic: /livox/imu
+localizer:
+  base_param_yaml: repo://param/nav2_ndt_urban.yaml
+  param_overrides:
+    use_imu: false
+    use_imu_preintegration: false
+    enable_scan_voxel_filter: true
+    voxel_leaf_size: 0.5
+    base_frame_id: livox_frame
+    scan_min_range: 1.0
+    scan_max_range: 100.0
+    score_threshold: 6.0
+    enable_recovery_retry_from_last_pose: true
+    recovery_retry_from_last_pose_min_rejections: 3
+    recovery_retry_from_last_pose_max_accepted_gap_sec: 1.0
+    recovery_retry_from_last_pose_max_seed_translation_m: 15.0
+  launch_args:
+  - use_sim_time:=true
+  - lidar_frame_id:=livox_frame
+benchmark:
+  output_dir: /tmp/lidarloc_koide_01a_full380_ndt_omp
+  ros_domain_id: 135
+  bag_duration: 380
+  bag_start_offset: 0
+  settle_seconds: 5
+  post_roll_seconds: 1
+  max_time_diff: 0.05
+  record_qos_reliability: reliable
+  record_qos_durability: volatile
+  record_use_sim_time: true

param/benchmark/koide_hard_localization_outdoor_hard_01a_full380_small_gicp.yaml

@@ -0,0 +1,42 @@
+# Koide outdoor_hard_01a FULL 380s -- SMALL_GICP backend (Phase 1 definitive).
+# Identical downsampling (voxel 0.5) across backends; GICP-scale gate for GICP.
+mode: run
+dataset:
+  bag_path: repo://data/public/koide_hard_localization/sequences/outdoor_hard_01a
+  map_path: repo://data/public/koide_hard_localization/map_outdoor_hard.ply
+  reference_csv: repo://data/public/koide_hard_localization/benchmark/outdoor_hard_01a/reference.csv
+  initial_pose_yaml: repo://data/public/koide_hard_localization/benchmark/outdoor_hard_01a/initial_pose.yaml
+  cloud_topic: /livox/points
+  imu_topic: /livox/imu
+localizer:
+  base_param_yaml: repo://param/nav2_ndt_urban.yaml
+  param_overrides:
+    use_imu: false
+    use_imu_preintegration: false
+    enable_scan_voxel_filter: true
+    voxel_leaf_size: 0.5
+    base_frame_id: livox_frame
+    scan_min_range: 1.0
+    scan_max_range: 100.0
+    score_threshold: 15.0
+    enable_recovery_retry_from_last_pose: true
+    recovery_retry_from_last_pose_min_rejections: 3
+    recovery_retry_from_last_pose_max_accepted_gap_sec: 1.0
+    recovery_retry_from_last_pose_max_seed_translation_m: 15.0
+    registration_method: SMALL_GICP
+    gicp_max_correspondence_distance: 3.0
+    vgicp_voxel_resolution: 0.5
+  launch_args:
+  - use_sim_time:=true
+  - lidar_frame_id:=livox_frame
+benchmark:
+  output_dir: /tmp/lidarloc_koide_01a_full380_small_gicp
+  ros_domain_id: 185
+  bag_duration: 380
+  bag_start_offset: 0
+  settle_seconds: 5
+  post_roll_seconds: 1
+  max_time_diff: 0.05
+  record_qos_reliability: reliable
+  record_qos_durability: volatile
+  record_use_sim_time: true

param/benchmark/koide_hard_localization_outdoor_hard_01a_full380_small_vgicp.yaml

@@ -0,0 +1,42 @@
+# Koide outdoor_hard_01a FULL 380s -- SMALL_VGICP backend (Phase 1 definitive).
+# Identical downsampling (voxel 0.5) across backends; GICP-scale gate for GICP.
+mode: run
+dataset:
+  bag_path: repo://data/public/koide_hard_localization/sequences/outdoor_hard_01a
+  map_path: repo://data/public/koide_hard_localization/map_outdoor_hard.ply
+  reference_csv: repo://data/public/koide_hard_localization/benchmark/outdoor_hard_01a/reference.csv
+  initial_pose_yaml: repo://data/public/koide_hard_localization/benchmark/outdoor_hard_01a/initial_pose.yaml
+  cloud_topic: /livox/points
+  imu_topic: /livox/imu
+localizer:
+  base_param_yaml: repo://param/nav2_ndt_urban.yaml
+  param_overrides:
+    use_imu: false
+    use_imu_preintegration: false
+    enable_scan_voxel_filter: true
+    voxel_leaf_size: 0.5
+    base_frame_id: livox_frame
+    scan_min_range: 1.0
+    scan_max_range: 100.0
+    score_threshold: 15.0
+    enable_recovery_retry_from_last_pose: true
+    recovery_retry_from_last_pose_min_rejections: 3
+    recovery_retry_from_last_pose_max_accepted_gap_sec: 1.0
+    recovery_retry_from_last_pose_max_seed_translation_m: 15.0
+    registration_method: SMALL_VGICP
+    gicp_max_correspondence_distance: 3.0
+    vgicp_voxel_resolution: 0.5
+  launch_args:
+  - use_sim_time:=true
+  - lidar_frame_id:=livox_frame
+benchmark:
+  output_dir: /tmp/lidarloc_koide_01a_full380_small_vgicp
+  ros_domain_id: 186
+  bag_duration: 380
+  bag_start_offset: 0
+  settle_seconds: 5
+  post_roll_seconds: 1
+  max_time_diff: 0.05
+  record_qos_reliability: reliable
+  record_qos_durability: volatile
+  record_use_sim_time: true

param/benchmark/koide_hard_localization_outdoor_hard_01a_smoke60_small_gicp.yaml

@@ -0,0 +1,42 @@
+# Koide outdoor_hard_01a 60s smoke -- SMALL_GICP backend (Phase 1 comparison).
+# Backend-appropriate config: full-density scan + GICP-scale gate.
+mode: run
+dataset:
+  bag_path: repo://data/public/koide_hard_localization/sequences/outdoor_hard_01a
+  map_path: repo://data/public/koide_hard_localization/map_outdoor_hard.ply
+  reference_csv: repo://data/public/koide_hard_localization/benchmark/outdoor_hard_01a/reference.csv
+  initial_pose_yaml: repo://data/public/koide_hard_localization/benchmark/outdoor_hard_01a/initial_pose.yaml
+  cloud_topic: /livox/points
+  imu_topic: /livox/imu
+localizer:
+  base_param_yaml: repo://param/nav2_ndt_urban.yaml
+  param_overrides:
+    use_imu: false
+    use_imu_preintegration: false
+    enable_scan_voxel_filter: false
+    voxel_leaf_size: 0.2
+    base_frame_id: livox_frame
+    scan_min_range: 1.0
+    scan_max_range: 100.0
+    score_threshold: 15.0
+    enable_recovery_retry_from_last_pose: true
+    recovery_retry_from_last_pose_min_rejections: 3
+    recovery_retry_from_last_pose_max_accepted_gap_sec: 1.0
+    recovery_retry_from_last_pose_max_seed_translation_m: 15.0
+    registration_method: SMALL_GICP
+    gicp_max_correspondence_distance: 3.0
+    vgicp_voxel_resolution: 0.5
+  launch_args:
+  - use_sim_time:=true
+  - lidar_frame_id:=livox_frame
+benchmark:
+  output_dir: /tmp/lidarloc_koide_01a_smoke60_small_gicp
+  ros_domain_id: 181
+  bag_duration: 60
+  bag_start_offset: 0
+  settle_seconds: 5
+  post_roll_seconds: 1
+  max_time_diff: 0.05
+  record_qos_reliability: reliable
+  record_qos_durability: volatile
+  record_use_sim_time: true

param/benchmark/koide_hard_localization_outdoor_hard_01a_smoke60_small_gicp_ds.yaml

@@ -0,0 +1,42 @@
+# Koide outdoor_hard_01a 60s smoke -- SMALL_GICP, downsampled scans (voxel 0.5).
+# Identical preprocessing to the NDT_OMP baseline; isolates the backend.
+mode: run
+dataset:
+  bag_path: repo://data/public/koide_hard_localization/sequences/outdoor_hard_01a
+  map_path: repo://data/public/koide_hard_localization/map_outdoor_hard.ply
+  reference_csv: repo://data/public/koide_hard_localization/benchmark/outdoor_hard_01a/reference.csv
+  initial_pose_yaml: repo://data/public/koide_hard_localization/benchmark/outdoor_hard_01a/initial_pose.yaml
+  cloud_topic: /livox/points
+  imu_topic: /livox/imu
+localizer:
+  base_param_yaml: repo://param/nav2_ndt_urban.yaml
+  param_overrides:
+    use_imu: false
+    use_imu_preintegration: false
+    enable_scan_voxel_filter: true
+    voxel_leaf_size: 0.5
+    base_frame_id: livox_frame
+    scan_min_range: 1.0
+    scan_max_range: 100.0
+    score_threshold: 15.0
+    enable_recovery_retry_from_last_pose: true
+    recovery_retry_from_last_pose_min_rejections: 3
+    recovery_retry_from_last_pose_max_accepted_gap_sec: 1.0
+    recovery_retry_from_last_pose_max_seed_translation_m: 15.0
+    registration_method: SMALL_GICP
+    gicp_max_correspondence_distance: 3.0
+    vgicp_voxel_resolution: 0.5
+  launch_args:
+  - use_sim_time:=true
+  - lidar_frame_id:=livox_frame
+benchmark:
+  output_dir: /tmp/lidarloc_koide_01a_smoke60_small_gicp_ds
+  ros_domain_id: 183
+  bag_duration: 60
+  bag_start_offset: 0
+  settle_seconds: 5
+  post_roll_seconds: 1
+  max_time_diff: 0.05
+  record_qos_reliability: reliable
+  record_qos_durability: volatile
+  record_use_sim_time: true