lidar_localization_ros2

Map-based 3D LiDAR localization for ROS 2 and Nav2.

Handheld Livox MID-360, ~390 m outdoors on a prior map, including stretches where the scans do not match the map and a section outside it. Each scan is matched to the map with NDT; RKO-LIO odometry predicts the motion between scans and carries the pose where matching fails. The error is 0.26 m RMSE for the whole run, in an offline replay started from the ground-truth pose (Koide Hard Point Cloud Localization Dataset outdoor_hard_02b, CC BY 4.0). How this was made · more runs

Give it a point cloud map (.pcd or .ply) and a LiDAR stream; it publishes the robot pose in the map. It finds the starting pose by itself, uses odometry and IMU when you have them, and works standalone or as the localization source for Nav2.

Where to start

Install

ROS 2 Jazzy is the main target; Humble is supported.

mkdir -p ~/lidarloc_ws/src
cd ~/lidarloc_ws/src
git clone https://github.com/rsasaki0109/lidar_localization_ros2.git
cd lidar_localization_ros2
scripts/bootstrap_colcon_workspace.sh --build
source ~/lidarloc_ws/install/setup.bash

This builds ndt_omp_ros2 and this package, about 30 minutes on an 8-core machine. small_gicp is optional. For a manual or no-sudo build, see local build.

No ROS install? Build the Docker image and check it in one command:

docker build -f docker/Dockerfile -t lidar_localization_ros2 .
docker run --rm lidar_localization_ros2 /ws/src/lidar_localization_ros2/docker/smoke_status.sh

Quick start

1. Start the LiDAR driver, and odometry if you have it.
2. Run quickstart with your map:

   ros2 run lidar_localization_ros2 quickstart.py --map /absolute/path/to/map.pcd
   

1. Check that it is tracking:

   ros2 run lidar_localization_ros2 check_lidar_localization_bringup.py --profile standalone
   

Quickstart finds the cloud and IMU topics, /clock, an odom -> base TF and the LiDAR frame, and prints what it picked on its Discovery: line. Then it starts the localizer and RViz. On the first run it searches the whole map for the robot; next time it restores the pose saved for the same map. If the search finds no safe match, it asks you for 2D Pose Estimate in RViz (press P, click the robot, drag along its heading) instead of guessing.

While it runs, the terminal prints one line whenever tracking starts, weakens, is lost or comes back, with what to do about it, and RViz shows the same status above the robot:

[localization_status]: Tracking.
[localization_status]: Tracking lost: no scan has matched the map for 4 s. If it does not recover, set 2D Pose Estimate in RViz.

What each line means: reading the status line.

Useful options:

• --dry-run shows what it would start, without starting it.
• --profile mid360 uses the Livox MID-360 preset.
• --initial-pose X Y Z QX QY QZ QW skips the search when you know the pose.
• --no-rviz for a headless robot; --no-status turns the status line off.

File truncated at 100 lines see the full file

Changelog

Unreleased

• Add /map_switch (std_msgs/String holding a .pcd or .ply path): replaces the map at runtime and keeps the current pose. A map that fails to load or has no points leaves the current one. If the new map has a different origin, publish /initialpose right after the switch; the global localization node keeps the map it was started with.
• Add publish_tf (default true). false turns off every TF the localizer broadcasts, for setups where another node (for example robot_localization fusing /pcl_pose) owns map -> odom; /pcl_pose and the other topics are still published. See Mode C in the frame contract.
• Quickstart reports tracking health after startup. localization_status_node.py prints one line when tracking starts, weakens (few or no scan points, poor match, CPU overload), is lost, stops receiving scans, or comes back, each with what to do, and RViz shows the same status as a colored label above the robot (Localization Status display). A change is reported once it has held for 2 s. No accepted scan for 5 s reads as lost, or as “the pose follows odometry only” (orange) while an external odom -> TF still arrives: on the Koide outdoor_hard_02b replay RKO-LIO kept the pose within 0.31 m through 70 s of rejected scans, while on outdoor_kidnap_a, with the odometry stopped too, the pose was 55-72 m off. Odometry only cannot tell a good bridged pose from a wrong reset. Diagnostics only; --no-status (launch arg show_status:=false) turns it off. Topics: /localization_status/status (JSON: phase, cause, label) and /localization_status/marker.
• Quickstart discovery and errors:
  • map errors list the .pcd/.ply next to a directory or missing file as --map options, and name wrong extensions and empty files;
  • only published topics count, the localizer’s own /map and /initial_map are not scan candidates, and a running localizer (/alignment_status published) is reported;
  • when a nav_msgs/Odometry topic is published, discovery reads the topic list again and waits up to 10 s for its odom -> TF, and names the topic in a hint if the TF never arrives. RKO-LIO publishes the TF only after its first scans (7 s after a cold Koide bag started), and DDS can discover it after the bag player; Koide replays started with odometry=none and failed global initialization in both cases.
• Add a Docker image (docker/Dockerfile, Jazzy by default, ROS_DISTRO=humble also builds) with ndt_omp_ros2, and docker/smoke_status.sh, which runs quickstart on a synthetic map and scan stream and checks the status lines; CI runs both. See docs/docker.md.
• Add a sparse-band global search: sparse_band_min_points (G2 node, default 0 = off; launch arg g2_sparse_band_min_points; quickstart --global-sparse-band-min-points, default 512). A scan with fewer 2D points than this in the height band (counted before the scan-point cap) is searched again from the sensor plane up, with 1024 points, 32 candidates and NDT-refined poses, and the answer reports sparse_band_search. With quickstart defaults the JEPLO Go2 started correctly in all 12 capture-room runs (before: 3-5 wrong) and now also on the Long_Stairs and Outdoor2 self maps; Outdoor1 stays ambiguous and asks for a manual pose. On handheld Koide replays it fired on no query (experiments/go2_wide_map_global_init).
• run_benchmark.py lists runs whose RKO-LIO odometry stalled (registration loop over 10 s), and score.json keeps the longest loop. Such runs looked like localizer failures in the post_init_jump replays.
• Add seed_correction_guard_exact_start_translation_m (node default 0, off; quickstart configs 0.1): when the first accepted update after an initial pose corrects it by at most this much, the seed correction guard’s warmup ends at once. It stops the EIL_Box jump, where NDT accepted 1.7-2.0 m aliased corrections right after a correct initial pose and the track slid 4.5 m: on a controlled ground-truth start both runs held, where the previous quickstart settings lost both. On the EIL, wide-map and Koide regression replays it fired once, on a correct start (experiments/post_init_jump).
• Record why the Go2 global search settings stay opt-in (experiments/go2_wide_map_global_init). On handheld Koide replays, a 256-point version initialized later in all 12 runs and wrongly in 2; the defaults were right in all 12. On the Go2 data, consecutive answers of the defaults’ saturated 2D search mostly agree 1-3 m off the robot, which the two-view startup consensus then confirms; starting each query’s yaw sweep at another sample did not change that (not promoted).
• Add opt-in quickstart options for global search with a sensor low above the ground: --global-z-min / --global-z-max (the G2 scan height band, launch args g2_z_min_m / g2_z_max_m, defaults 0.5 / 5.0 m as before) and --occupancy-inflate-radius-m (inflation of the generated grid, default 0.6 m as before, cached separately). On the JEPLO Go2 self maps (Long_Stairs, Outdoor1, Outdoor2), which never initialized, the recipe in go2_mid360_quickstart.md initialized all six runs without a wrong start (later: 5 of 6, one Outdoor2 run stayed ambiguous and asked for a manual pose), and in the capture room all 12 EIL runs, where the defaults started wrongly in 5 of 16 on the same (rebuilt) map. The 2D search saturated: about 100 scan points above an upside-down MID-360 matched a 0.6 m inflated grid perfectly at many places, so the truth rarely reached NDT (comparison). Defaults are unchanged: only Go2 data was compared. The G2 node logs its height band at startup.
• Add tools/benchmark/jeplo_to_pc2_bag.py and build_map_from_odometry.py: the public JEPLO dataset now ships only raw Livox bags, so the Go2 suite’s PointCloud2 bags and self maps are rebuilt from them (docs/benchmark.md).
• Add an experimental opt-in G3 confirmation mode for odometry dropouts (odometry_confirmation_mode:=segment_defer, quickstart --supervisor-odometry-confirmation-mode). Answers confirm each other only across gapless odometry, and queries wait while odometry is out. The default (window_waiver) is unchanged; see the synthetic comparison. A Koide replay (three runs per mode) did not support promotion: the mode regresses outdoor_kidnap_b recovery, where RKO-LIO odometry drops out throughout.

1.3.0 - 2026-10-09

• Add an opt-in 75 m local-map preset for online Koide replay with RELIABLE points

File truncated at 100 lines see the full file