[codex] Bound non-converged GICP fitness fallback

[FieldDiTian]

Summary

• Bound the non-converged low-fitness GICP fallback with correction-size gates.
• Add gicp/nonConvergedFitnessOkMaxTransM and gicp/nonConvergedFitnessOkMaxRotDeg defaults of 3.0m and 5.0deg.
• Publish the same bounded effective-convergence decision on the debug converged topic.
• Fix an Eigen ternary expression around initial_guess that broke package builds.

Root Cause

Run5-on-Run3 accepted some wrong-basin GICP results where raw GICP reported converged=false, but absolute fitness was below gicp/fitnessRejectThreshold. Those accepted frames reset the consecutive-failure counter, so GT recovery could not engage until after the pose had already drifted far from INS.

The observed bad chain included low-fitness non-converged corrections such as about 3.27m / 11.2deg and 9.21m / 10.7deg. This PR keeps the intended max-iteration fallback only for small corrections and rejects larger non-converged candidates as failed convergence.

Validation

• git diff --check
• source /opt/ros/jazzy/setup.bash && colcon build --packages-select gicp_localization --cmake-args -DCMAKE_BUILD_TYPE=Release
• Run5-on-new-Run3 smoke replay covering the known bad window.
• Run5-on-new-Run3 full replay with manual debug-bag audit.

Full replay result after the patch:

• Overall INS position p50/p90/p95: 5.564 / 12.702 / 17.969 m versus baseline run20 6.447 / 21.634 / 39.159 m.
• Non-converged accepted frames: 1145 -> 397.
• Accepted/partial frames with INS error >=20m: 434 -> 78.
• Accepted/partial frames with INS error >=50m: 99 -> 3.

[Copilot]

Pull request overview

This PR tightens gicp_localization’s acceptance logic by bounding the existing “non-converged but low-fitness” fallback with correction-magnitude gates, reducing wrong-basin accepts that were preventing GT recovery from engaging. It also fixes a C++ initialization pattern (Eigen ternary) that was breaking builds.

Changes:

• Add parameters to cap acceptable non-converged corrections (gicp/nonConvergedFitnessOkMaxTransM, gicp/nonConvergedFitnessOkMaxRotDeg) and incorporate them into the effective-convergence decision.
• Publish the bounded effective-convergence decision to the debug converged topic for consistent observability.
• Replace an Eigen ternary initialization of initial_guess with a simpler/safer if assignment.

Reviewed changes

Copilot reviewed 3 out of 3 changed files in this pull request and generated no comments.

File	Description
gicp_localization/src/localization.cc	Implements bounded non-converged fallback, updates debug publishing, and fixes initial_guess initialization.
gicp_localization/include/gicp_localization/localization.h	Adds member fields for the new non-converged fallback bounds.
gicp_localization/cfg/localization.yaml	Documents and sets defaults for the new parameters.

---

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.