lidar_localization_ros2: map-based 3D LiDAR localization for ROS 2 that finds its own start pose

What it is

lidar_localization_ros2 is a ROS 2 package: give it a previously built point-cloud map (.pcd or .ply) and a LiDAR stream, and it continuously publishes the robot pose in the map frame. It is not SLAM - it builds no map and optimises no trajectory; it answers one question, "where am I on this map". It runs standalone (publishing map -> base_link) or as the localization source for Nav2 (publishing map -> odom). The repository is rsasaki0109/lidar_localization_ros2 by ryohei sasaki (an individual developer), with 580 stars and 132 forks under BSD-2-Clause, created 2020-04-18 and last pushed 2026-10-17 - six years of uninterrupted maintenance. The current package version is 1.3.0, targeting ROS 2 Jazzy with Humble compatibility.

It is the companion piece to the author's lidar_slam_ros2: that package builds maps and odometry (RKO-LIO), this one localizes on the finished map. The README's standard usage is to start RKO-LIO odometry first, then quickstart.py --map map.pcd from this package. Reading this package therefore means reading the reference implementation of "what a robot does once it already has a map".

Fact sheet

Field	Value
Repository / author	rsasaki0109/lidar_localization_ros2 (owner type User, ryohei sasaki)
Stars / forks / watchers	580 / 132 / 6 (as of 2026-10-18)
Language / licence	Classified Python on GitHub; the localization core is a C++ lifecycle component plus Python tooling / BSD-2-Clause
Created / last push	2020-04-18 / 2026-10-17
Package version / ROS	1.3.0 / Jazzy primary, Humble compatible
Registration backends	NDT, GICP, NDT_OMP, GICP_OMP, SMALL_GICP, SMALL_VGICP
Map formats	.pcd / .ply (binary little-endian float32 PLY preferred at runtime)
Hard dependency	ndt_omp_ros2 (same author's OpenMP-accelerated NDT/GICP); small_gicp optional behind CMake guards
Tests	81 files under test/: C++ policy unit tests plus Python contract/CLI tests
Runtime shape	Lifecycle node on a multi-threaded executor (3 threads); standalone / Nav2 / MID-360 legged-robot launch files
Output topics	/pcl_pose, /path, /alignment_status, /reinitialization_requested

Which side of SLAM it stands on: localization on a prior map, not mapping

Mobile robot localization asks two different questions. One is "I start from nothing and know where I am while building the map" (SLAM). The other is "the map already exists - maybe built last week, maybe by another robot - and every time I boot I have to find myself on it" (map-based localization). The latter is what AMCL answers for 2D laser scans; lidar_localization_ros2 moves the same question to 3D point-cloud maps and 3D LiDAR, with the matching engine swapped for the NDT / GICP family of 3D registration.

This split creates a set of problems SLAM never owns, and the package's design revolves around them:

• Cold start has no initial pose. First boot, being carried to another room, or recovering from lost tracking all require re-answering "where am I". The package runs branch-and-bound (BBS_2D) over a 2D occupancy grid for map-wide search, scores candidates with 3D NDT, and has the localizer itself verify the answer.
• Self-similar maps have aliased solutions. Corridors, symmetric rooms and repetitive structures produce several candidates that fit "perfectly". The answer is an explicit chain of safety gates: score threshold, margin over runner-up, candidate age, multi-scan consensus, verification window - it would rather fail the search and ask the operator for a 2D Pose Estimate in RViz than guess, and there is no implicit fallback to the map origin.
• Localization must not jitter more than odometry does. Pure NDT tracking jumps when scans mismatch the map, so the package layers prediction: twist prediction, twist EKF, IMU preintegration smoothing, external odometry TF bridging - each with its own guard, and any disagreement falls back to the LiDAR measurement itself.

The full pipeline of one scan

The core is PCLLocalization, an rclcpp_lifecycle::LifecycleNode on a three-threaded multi-threaded executor (IMU preintegration and timer publishing get dedicated callback groups so long registration callbacks cannot starve them). The implementation is split by concern into src/component_*.cpp, while every decision lives in one of the 60-plus ROS-free policy headers under include/lidar_localization/ (measurement_gate_policy.hpp, registration_seed_policy.hpp, imu_preintegration_guard_policy.hpp ...), each with its own unit tests. This split is the engineering decision most worth copying: policies are pure functions, unit-testable and regression-testable outside the ROS graph.

One sensor_msgs/PointCloud2 travels this path from arrival to published pose:

1. Scan admission (scan_admission_policy): stamp, minimum interval, queue depth checks.
2. Preprocessing (scan_preprocessing_policy): range filtering plus optional voxel downsampling; per-point relative times are extracted from time / timestamp / offset_time / t fields (Livox FLOAT64 nanoseconds are auto-detected).
3. Continuous-time deskew (on by default): rewrites points into one reference time using the motion estimate; if timing or motion data is not ready the scan passes through unchanged rather than being warped anyway.
4. Seed selection (registration_seed_policy): current pose / previous delta / twist prediction / IMU preintegration / external odometry TF prediction, chosen by priority and availability.
5. Registration: NDT / GICP backends with a local map crop (150 m radius by default) to bound cost on large maps.
6. Measurement gate (measurement_gate_policy): fitness, correction magnitude and seed deviation limits; a rejected match is never interpolated into a guess.
7. Pose backend: raw registration pose / twist EKF / GTSAM sliding-window smoother / IMU preintegration smoother, published with covariance.
8. Publish: /pcl_pose (with covariance), /path (2000 most recent poses by default), TF, and a set of diagnostic key-values.

Global initialization and auto-recovery: the G1 / G2 / G3 trio

This is the most heavily evidenced part of the package; docs/global_localization.md is dedicated to it and splits it into three stages:

Stage	What it does	Entry point
G1	Map-wide candidate generation: branch-and-bound (BBS_2D) over a 2D occupancy grid, artifact-first and offline-capable	make_bbs_relocalization_attempts.py
G2	On-demand query service: hand it a scan, get ranked candidates back (JSON plus a PoseArray)	global_localization_node.py
G3	Guarded automatic re-seeding: watch the lost-tracking signal, query G2, publish /initialpose only when every safety gate passes	reinitialization_supervisor_node.py

G1 and G2 need an occupancy grid built from the point-cloud map with generate_occupancy_map_from_pcd (points between 0.4 and 2.0 m above the ground count as obstacles, so ceilings and tree canopy do not). G3's decision logic lives in reinitialization_supervisor_policy.py, a ROS-free state machine with regression coverage. Its guard list reads like a post-mortem notebook: minimum candidate score, minimum seconds between resets, request debounce, recovery fitness threshold, consecutive recovery evidence samples, settle timeout, hard attempt ceiling, query timeout, and a cross-check against the localizer pose - miss any one gate and nothing is published. The documentation is blunt about why: a closed recovery loop that lets one confidently-wrong candidate be republished repeatedly diverges catastrophically, and that is the Phase 3 lesson.

quickstart wraps the whole stack into one command: ros2 run lidar_localization_ros2 quickstart.py --map /path/map.pcd. It discovers the cloud and IMU topics, /clock, the odom -> base TF and the LiDAR frame, printing them on its Discovery: line; it searches the whole map on first run and afterwards restores the last verified pose keyed by the map content SHA-256; if no unambiguous match is found it asks for a 2D Pose Estimate in RViz - there is no (0,0,0) fallback. For the case of an upside-down MID-360 on a legged robot 0.45 m above the ground, where the default height band holds only about 100 points and the 2D search saturates, a sparse-band fallback search exists (fewer than 512 band points triggers a re-search from the sensor plane up, 1024 points, 32 candidates, NDT-refined poses); on Go2 capture-room replays it took wrong starts from 3-5 of 12 runs to none.

IMU preintegration and continuous-time deskew

IMU support is layered and off by default (pure NDT/GICP tracking), so users who do not want it are never affected:

Layer	Parameter	What it does
Twist prediction	use_twist_prediction	Constant-twist motion model between scans, rotating the seed with IMU/odom angular velocity
Twist EKF	use_twist_ekf	Small constant-velocity EKF on the twist, feeding both the prediction seed and the published covariance
IMU preintegration + smoother	use_imu + use_imu_preintegration	On-manifold preintegration (Forster et al. 2017) inside a sliding-window Gauss-Newton optimizer with 6-DoF NDT priors and online gyro/accel bias estimation
Twist GTSAM smoother	use_gtsam_smoother	Sliding-window odometry+NDT smoother without raw IMU

The preintegration math is pinned by a numerical regression test: pure rotation reproduces delta_R = Exp(omega*t), pure translation reproduces delta_v = a*t, the residual is zero for a consistent world trajectory, and all five bias Jacobians match finite differences - pure Eigen, buildable and runnable outside ROS. The safety side is a set of guards: a consistency gate (an IMU prediction may seed registration only after consecutive comparisons against accepted LiDAR poses pass), a correction guard (if IMU and NDT correction disagree by more than 2.0 m or 10 degrees, the smoother is reset to the LiDAR measurement), and a five-accept warmup after any reset.

Continuous-time deskew (on by default) has three motion sources: relative_motion, imu_pose_history and lidar_constant_velocity. The documentation's own negative result must be quoted here: on 2026-07-13, three runs each on the Koide outdoor_hard_01a 85-112 s corner window, none of the three modes passed the shared accuracy, coverage, reject-streak and latency gate - so the default keeps the conservative fallback of leaving scans unchanged until readiness checks pass. Publishing negative results like this is the norm in this repository, not the exception.

Nav2 integration and the topic/frame contract

The frame contract is fixed at map / odom / base_link, and every topic name is remappable. Standalone mode publishes map -> base_link; Nav2 mode publishes map -> odom and requires an external odom -> base_link (RKO-LIO, or wheel/leg odometry). use_odom: true only reads /odom; it does not publish odometry TF.

One AMCL-style design in Nav2 mode deserves attention: the frozen map -> odom bridge. map -> odom is updated only from accepted scan matches and re-stamped on every scan callback while matches are being rejected, so the composed map -> base_link stays continuously available through TF even while NDT is mismatching - downstream consumers never see the pose tear. The companion topic /odom_bridge_pose is published with transient_local QoS and the G3 supervisor subscribes to it instead of doing its own TF lookups (the documentation records that /tf's volatile durability was measured not to reach a third process reliably).

There are four launch files: nav2_lidar_localization.launch.py (localization only), nav2_navigation.launch.py (localization plus the Nav2 stack), mid360_legged_localization.launch.py (MID-360 on a legged robot) and lidar_localization.launch.py (base launch with every argument exposed). The parameter YAML (localization_param_dir) is the source of truth; launch arguments override it only when passed explicitly. Lifecycle activation goes through the GetState / ChangeState services without relying on transition events.

Benchmark: hard numbers on public datasets

The benchmark method itself matters: public datasets are replayed through the exact two commands a user runs (odometry plus quickstart.py --map), every run starts with no initial pose, and errors are measured in the map frame with no trajectory alignment - because alignment would hide the worst failure mode, a wrong initialization tracked consistently (the documentation's example: one Go2 run was 1.8 m off but looked like 0.2 m after alignment).

Dataset	Runs	Initialized 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 replay. The kidnap p95 comes from the stretch while the sensor is being carried, before recovery, not from stable tracking. The v1.3.0 tag verification (2026-10-09, Humble) adds one full replay: 0.2796 m map-frame position RMSE with 2,203 of 2,209 ground-truth-matched pose stamps (99.73%).

The failures are documented just as plainly: in one outdoor_hard_02b run, RKO-LIO dropped frames with too few ICP keypoints and the recovery supervisor reset the pose 128 m away on a single unconfirmed answer inside the odometry-dropout waiver window; /alignment_status reports OK on most poses after a wrong reset (a confident match at the wrong place) while raising false alarms on 20-41% of good poses; and on wide Go2 maps the 2026-10-06 defaults saturated into ambiguity (the later sparse-band fallback fixed that). There are no multi-hour runs and no physical-hardware validation.

Getting started: install, quickstart, robot-free reproduction

Installation goes through the repository's own script, about 30 minutes on 8 cores:

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

It builds the same author's ndt_omp_ros2 alongside this package; the no-sudo / manual path is in docs/local_build.md. With a robot, start from quickstart.py --map; Unitree Go2 plus MID-360 has a five-minute guide (docs/go2_mid360_quickstart.md); without a robot, replay a public dataset: ros2 run lidar_localization_ros2 run_koide_public_bag.py --download downloads the Koide data, runs odometry plus localization and reports the error against ground truth in about five minutes of playback. When bringup misbehaves, run check_lidar_localization_bringup.py --profile standalone first and follow docs/troubleshooting.md.

Boundaries: what it does not promise

docs/v1_status.md draws the line with two lists, "Safe claims" and "Do not claim", transcribed here. Fair to say: the package provides ROS 2 map-based 3D LiDAR localization with Nav2 launch helpers, and the recommended preset has public replay and smoke validation within the documented boundary. Not fair to say: production-grade long-horizon urban operation, production-ready global relocalization, automatic reset publication in the default benchmark path, SMALL_GICP as default-ready without current public comparisons, or Jetson + MID-360 hardware validation. Covariance semantics are not yet a headline downstream-fusion claim, and occupancy-map generation is route-crop oriented, not a general mapper.

Who it fits: teams that already hold a point-cloud map (self-built or from a SLAM package like lidar_slam_ros2) and need 3D-accurate localization - the legged robot with upside-down MID-360 is the best-evidenced form factor in the docs; anyone needing a Nav2 localization source to replace AMCL; and anyone who wants a reference localization pipeline whose policies are decoupled from ROS and unit-tested. Who it does not fit: scenes with no prior map (that is SLAM's job), or deployments needing production-grade long-horizon unattended recovery - exactly the open problem the Known Limits section names.