repository·main·Indexed 19 days ago
https://github.com/rsasaki0109/lidar_localization_ros2A map-based 3D LiDAR localization system for ROS 2 and Nav2 supporting NDT/GICP algorithms against .pcd and .ply maps. It features a GLIL-style GLIM prior-map localizer with tightly coupled architecture, continuous-time deskewing, and guarded global initialization. The system supports sensors like the Livox MID-360 and includes advanced capabilities such as a scan-bounded dual IMU queue, an IMU seed consistency gate, and kidnap recovery using a deferred exact-coreset approach.
The global localization feature is being developed in phases. Current development focuses on map-wide candidate generation and runtime services, but it is not yet intended for production-grade kidnapped-robot recovery or as part of the default bringup path.
Current Non-Goals:
The v1.0.0 release of lidar_localization_ros2 includes the following core components:
NDT, GICP, NDT_OMP, GICP_OMP, SMALL_GICP, and SMALL_VGICP..pcd and .ply pointcloud-map loading with local-map cropping for large maps./alignment_status, and reinitialization requests are output on /reinitialization_requested./initialpose from the Nav2 wrapper.The project provides a ROS 2 3D LiDAR map-based localization package with the following core features:
param/nav2_ndt_urban.yaml.NDT, GICP, NDT_OMP, GICP_OMP, SMALL_GICP, and SMALL_VGICP./initialpose commands.Current Known Limits:
The package provides a layered set of IMU-aided estimation options. These are opt-in; the default configuration uses pure NDT/GICP tracking and does not use the IMU path. You can choose from the following layers based on your computational budget and accuracy requirements:
use_twist_prediction and use_twist_prediction_use_angular_velocity. It performs constant-twist motion prediction between scans using IMU or odometry angular velocity to rotate the seed.use_twist_ekf. A small constant-velocity Extended Kalman Filter (EKF) on the twist that feeds both the prediction seed and the published covariance.use_imu and use_imu_preintegration. This is the most advanced layer, implementing on-manifold IMU preintegration (Forster et al. 2017) within a sliding-window Gauss-Newton optimizer. It includes 6-DOF NDT priors and online gyro/accel bias estimation.use_gtsam_smoother. A sliding-window odometry+NDT smoother that does not use raw IMU data.When using the MID-360 legged robot configuration, the localization stack expects specific inputs and provides specific outputs. This stack acts as a localization boundary and does not manage walking control, motor estimation, or the MID-360 driver itself.
/livox/points./livox/imu.TwistWithCovarianceStamped. Default topic: /twist.odom -> base_link transform if enable_map_odom_tf:=true is set./initialpose is required, unless set_initial_pose:=true is used to provide a known starting pose via parameters..pcd or .ply pointcloud map./localization/pose_with_covariance/path/alignment_status/reinitialization_requestedmap -> odom TF (by default)/initial_mapNDT_OMP backend supports 6-DoF Hessian diagnostics to evaluate localizability. This diagnostic is implemented as a default-off feature to avoid latency overhead. It provides metrics like weak ratio and condition numbers to help diagnose registration failures. Use this for offline analysis or debugging rather than real-time deployment unless latency requirements are met.The IMU seed consistency gate is a safety mechanism that validates an IMU-predicted pose against the currently accepted LiDAR pose before allowing the IMU prediction to seed the registration process.
To pass the gate, the comparison must satisfy both translation and rotation thresholds for five consecutive accepted updates.
Failure Conditions: Permission to use the IMU seed is revoked immediately if any of the following occur:
If the gate is not passed, the system remains in an open-loop state with respect to registration, meaning it does not use the IMU prediction as a seed.
The startup manager follows a strict hierarchy to determine the initial pose. It will use the first valid method found in this order:
--initial-pose flag. This disables both saved-pose publication and global search for that session..pcd or .ply map.--occupancy-map is supplied.Note: There is no implicit (0, 0, 0) fallback. If all methods fail, the system will not publish a pose.
Saved Pose Details:
~/.local/state/lidar_localization_ros2/<map-name>.jsonThis mechanism allows the system to accept degraded-but-consistent registration results that a standard scalar fitness gate might reject. This prevents the system from entering long reject streaks when ground-truth poses have lower fitness scores than the hard threshold.
Key variants include:
fixed_threshold: The runtime baseline using a scalar fitness threshold.bounded_degraded: Uses a correction cross-check with a non-resetting degraded-accept budget.correction_conditioned: Combines a fitness threshold with a correction/staleness cross-check.score_ratio_budget: Uses a relative score cap with a gap/streak budget.Koide GLIL-style localization uses a fixed-lag graph (GLIM) to jointly optimize three components:
This approach follows the sliding-window estimator structure from Koide et al. (ICRA 2024) and uses deferred exact point-cloud downsampling (ICRA 2025). Unlike legacy split methods, scan-to-map results are not solved independently; instead, each map observation constrains the same active pose states as scan-to-scan and IMU factors. This ensures continuous range-inertial odometry even if map overlap is lost.
The global localization stack is designed to recover a usable pose when the initial pose is unknown or lost, and to automatically re-seed tracking when divergence occurs. It consists of three distinct stages:
scripts/make_bbs_relocalization_attempts.py.scripts/global_localization_node.py./initialpose only after passing several safety guards. Entry point: scripts/reinitialization_supervisor_node.py.Prerequisites:
scripts/generate_occupancy_map_from_pcd.py.To prevent catastrophic failures during global localization (kidnapping), the system uses an 'Odometry-aware guarded recovery' mechanism:
map -> odom from the candidate pose, but do not immediately reset the odometry state.