Improve localization

[openminddev]

This pull request makes significant performance and maintainability improvements to the go2_lidar_localization.py node by replacing the tf_transformations dependency with scipy.spatial.transform.Rotation, introducing Numba-accelerated functions for scan matching, and refactoring related code paths. The changes enhance scan matching speed, simplify dependencies, and modernize pose and transformation handling.

Performance optimizations (scan matching and pose evaluation):

• Added Numba-accelerated functions: evaluate_pose_numba, evaluate_multiple_poses, and create_gradient_mask_numba for fast, parallelized scan matching and mask creation, and integrated them into the main localization flow. [1] [2] [3] [4] [5] [6] [7]
• Removed the old loop-based and non-Numba vectorized pose evaluation code in favor of the new Numba implementations. [1] [2]

Dependency and transformation handling refactor:

• Replaced all uses of tf_transformations with scipy.spatial.transform.Rotation for quaternion and rotation matrix operations, and removed the tf_transformations dependency from both package.xml and pyproject.toml. [1] [2] [3] [4] [5] [6] [7] [8]
• Updated transformation matrix math to use NumPy and SciPy directly, further reducing reliance on external libraries.

Dependency updates:

• Updated pyproject.toml to require newer versions of scipy and numpy, and added numba as a dependency for JIT acceleration.
• Removed the strict numpy version requirement and the tf_transformations dependency. [1] [2]

General code modernization and cleanup:

• Ensured all scan point arrays are explicitly typed as np.float64 for consistency and Numba compatibility. [1] [2]
• Improved code clarity and maintainability by consolidating scan point conversion and evaluation logic. [1] [2]

These changes collectively improve the speed, reliability, and maintainability of the lidar localization node.

[Copilot]

Pull request overview

This pull request modernizes the localization codebase by replacing tf_transformations with scipy.spatial.transform.Rotation and introducing Numba-accelerated scan matching functions to significantly improve performance. The changes also include adaptive scan point downsampling and updated dependency management.

Key Changes:

• Replaced tf_transformations dependency with scipy.spatial.transform.Rotation for all quaternion and matrix operations
• Added Numba-optimized functions (evaluate_pose_numba, evaluate_multiple_poses, create_gradient_mask_numba) with JIT compilation warmup at startup
• Removed legacy loop-based evaluate_pose function in favor of Numba-accelerated pathways

Reviewed changes

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

File	Description
pyproject.toml	Updated dependency versions: relaxed numpy constraint, upgraded scipy, added numba and coverage
go2_sdk/package.xml	Removed tf_transformations dependency
go2_sdk/go2_sdk/go2_lidar_localization.py	Implemented Numba-accelerated scan matching, replaced tf_transformations with scipy Rotation, and modernized transformation matrix operations

---

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