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

| You want to | Go to | | --- | --- | | Localize your robot on a map you already have | Install, then Quick start | | Run it on a Unitree Go2 or another MID-360 robot | Go2 / MID-360 in five minutes | | Navigate with Nav2 | Nav2 | | Try it without a robot | Run the public dataset | | See how accurate it is | Benchmark | | Fix a bringup that does not work | Troubleshooting |

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.
• --help lists the bringup options; --help-all adds the tuning ones.

ros2 launch lidarslam rko_lio_odometry.launch.py
ros2 run lidar_localization_ros2 quickstart.py --map /path/to/output/my_map/map.pcd

More: quickstart and automatic initialization, repeat-route site setup, runtime details.

Nav2

ros2 launch lidar_localization_ros2 nav2_navigation.launch.py \
  map_yaml:=/absolute/path/to/map.yaml

Nav2 also needs a 2D occupancy map and an odom -> base_link source. In this mode the localizer publishes map -> odom.

Launch files

Quickstart covers most setups. To launch directly:

| Launch file | Use | | --- | --- | | nav2_lidar_localization.launch.py | Localization only | | nav2_navigation.launch.py | Localization plus the Nav2 stack | | mid360_legged_localization.launch.py | Livox MID-360 on a legged robot | | lidar_localization.launch.py | Base launch with every argument exposed |

ros2 launch lidar_localization_ros2 mid360_legged_localization.launch.py \
  map_path:=/absolute/path/to/map.pcd \
  cloud_topic:=/livox/points imu_topic:=/livox/imu

How it works

Startup finds a first pose; then every scan is matched against the map near the predicted pose. When matching keeps failing, recovery searches the map again.

flowchart LR
    start(["quickstart.py --map"]) --> explicit{"--initial-pose?"}
    explicit -- yes --> verify
    explicit -- no --> saved{"pose saved for<br/>this map?"}
    saved -- yes --> verify["verify against<br/>the next scans"]
    saved -- "no / fails" --> search["global search<br/>2D candidates + NDT score"]
    search -- "two views agree" --> verify
    search -- ambiguous --> operator["ask for<br/>2D Pose Estimate"]
    operator --> verify
    verify -- ok --> tracking(["tracking"])
    tracking -- "reinitialization request<br/>or odometry dropout" --> recovery["recovery: query the<br/>global search, guarded reset"]
    recovery --> verify

Each scan:

flowchart TD
    odom[/"optional: external odometry<br/>e.g. RKO-LIO from lidar_slam_ros2,<br/>wheel or leg odometry"/]
    scan["LiDAR scan"] --> prep["range crop, voxel filter,<br/>deskew"]
    prep --> seed["seed pose: last pose,<br/>moved by odometry or IMU if given"]
    seed --> reg["NDT or GICP against the map<br/>cropped around the seed"]
    reg --> gate{"converged, score under<br/>threshold, guards pass?"}
    gate -- yes --> pub["publish pose and TF,<br/>/alignment_status OK"]
    gate -- no --> bridge["keep the predicted pose<br/>(odometry bridge)"]
    bridge -- "rejected for long" --> reinit["/reinitialization_requested"]
    odom -. "odom -> base TF" .-> seed
    odom -. "carries the pose" .-> bridge

Map matching is always NDT (default) or GICP. External odometry is optional: it only predicts the motion between scans and carries the pose where scans do not match the map. It helps with fast or handheld motion; quickstart uses it when it sees an odom -> base TF.

/alignment_status reports each step, and quickstart's status line turns it into Tracking / Weak / Odometry only / Lost. Details: global localization, initialization order, runtime, reading the status line.

Topics and frames

Default frames are map, odom and base_link.

| | Topics | | --- | --- | | Inputs | /cloud, /initialpose (in map), /odom, /imu, /map_switch | | Outputs | /pcl_pose, /path, /alignment_status, /reinitialization_requested | | Status (quickstart) | /localization_status/status (JSON), /localization_status/marker (RViz label) |

• Standalone mode publishes map -> base_link.
• Nav2 mode publishes map -> odom and needs an external odom -> base_link.
• use_odom: true reads /odom; it does not publish odometry TF.
• publish_tf: false turns off every TF the localizer broadcasts; use it when another

• Each static LiDAR and IMU transform must have exactly one publisher.

/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 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.

Benchmark

Public datasets replayed through the same commands as above (odometry plus quickstart.py --map). Each run starts with no initial pose, so the localizer has to find the robot itself. Errors are measured in the map frame with no trajectory alignment, so a wrong start shows up as error.

| Dataset | Runs | Started correctly | Median error | p95 error | | --- | --- | --- | --- | --- | | Koide outdoor, handheld MID-360 (4 sequences) | 12 | 12 | 0.08-0.12 m | 0.21-0.91 m | | Koide outdoor kidnap (sensor carried elsewhere) | 6 | 6 | 0.04-0.12 m | 15.9-60.5 m | | JEPLO, Unitree Go2, lidar_slam_ros2 map (4 sequences) | 12 | 12 | 0.03 m | 0.09-0.13 m | | JEPLO, Go2 on wide maps, low-sensor settings (7 sequences) | 18 | 18 | 0.02-0.03 m | - |

Measured 2026-10-06 and 2026-10-11 on Jazzy at 1x. Ranges are over sequences.

What this does not cover:

• In the kidnap runs the error is large while the sensor is carried, until recovery.
• On wide Go2 maps, the default search can stay ambiguous and ask for a manual pose

• No long multi-hour runs and no physical-hardware validation.

Try it without a robot

Build this package and rko_lio from lidar_slam_ros2, then:

ros2 run lidar_localization_ros2 run_koide_public_bag.py --download

It downloads the Koide data, runs odometry and localization, and reports the error against the ground truth. Playback takes about five minutes. Data is cached in ./koide-data and results go to ./koide-run-*. See the public-bag guide.

Documentation

Using it:

• Quickstart and automatic initialization
• Docker: build, smoke test, and running on your data
• Go2 / MID-360 in five minutes and MID-360 bringup
• Repeat-route site setup
• Runtime details: parameter precedence, deskew, lifecycle
• Frames and troubleshooting
• Map alignment: map frame and pose offsets

• Global localization
• IMU and deskew and pose covariance

• Benchmark and validated scope
• Koide demo gallery
• Release notes
• Development notes: plans, roadmaps, experiment records

License

BSD 2-Clause.