From Blind Startup to Global Relocalization: Building a ROS 2 3D LiDAR Navigation System

Open-sourced on GitHub. Stars are welcome:
https://github.com/Ikunio/Lidar_nav2_ws

Recently, I built a ROS 2 Humble-based 3D LiDAR autonomous navigation system named Lidar_nav2_ws.

The focus of this system is not simply “I managed to run another Nav2 demo.” Instead, I wanted to solve two more practical problems.

First, can a robot perform global relocalization using 3D point clouds when its initial pose is unknown?

Second, can the system decouple LiDAR odometry, point cloud relocalization, Nav2 navigation, and the robot body system, so that I can switch algorithms later without rewriting the entire project?

In simple terms, what I wanted was not a demo that only works when the robot starts from a fixed position, but a navigation framework that can be extended, modified, tested on real hardware, and repeatedly experimented with in the future.

---

1. Why I Built This System

In mobile robot navigation, Nav2 already provides a relatively complete navigation framework, including global planning, local planning, costmaps, behavior trees, and controllers.

However, Nav2 itself does not tell the robot, “Where am I now?”

For a typical 2D LiDAR-based system, AMCL is a common localization solution. But once the sensor is replaced with a 3D LiDAR such as Livox MID-360, the situation becomes more complicated.

A 3D LiDAR provides richer spatial structure, but it also introduces a practical problem:

How can the current 3D point cloud scan be robustly matched against an existing global PCD map?

This becomes especially important when the robot starts up from an arbitrary position. If the robot is not placed at a fixed starting point, the system does not know its initial pose in the map. In this case, relying only on a local registration algorithm is often unreliable.

Therefore, the core goals of this system are:

• Use 3D LiDAR and IMU data to run LIO;
• Build a 2D map for Nav2;
• Save a 3D PCD map for point cloud relocalization;
• Perform global relocalization through point cloud matching when the initial pose is uncertain;
• Continuously publish map -> odom to provide stable global localization input for Nav2;
• Decouple LIO, relocalization, Nav2, and robot description modules, making it easier to replace algorithms later.

The most important part is:

Using KISS-Matcher + small_gicp to achieve global relocalization.

---

2. Overall System Architecture

The overall data flow of the system is roughly as follows:

LiDAR / IMU
    ↓
LIO odometry
    ↓
TF bridge
    ↓
/registered_scan
    ↓
3D point cloud relocalization
    ↓
publish map -> odom
    ↓
Nav2 navigation

I split the whole system into several relatively independent modules:

Module	Function
LIO module	Outputs odometry results based on LiDAR and IMU data
TF bridge module	Unifies TF relationships from different LIO backends
Point cloud generation module	Publishes /registered_scan as the input for relocalization
Relocalization module	Aligns the current local point cloud with the prior PCD map
Nav2 module	Handles path planning, local obstacle avoidance, and navigation execution
Robot description and simulation module	Handles URDF, Gazebo, and sensor simulation

With this design, LIO is not tightly coupled with Nav2, and the relocalization module is not tied to a specific LiDAR odometry algorithm.

That means the following components can be switched more conveniently later:

FAST-LIO  ↔  Point-LIO

small_gicp  ↔  ICP  ↔  KISS-Matcher + small_gicp

simulation URDF  ↔  real robot URDF

This is exactly the effect I wanted:

Each module can be replaced independently, instead of mixing everything into one big pot of chaos.

In a robotic system, the scariest thing is often not that one algorithm is weak, but that all modules are tightly coupled together.

Once something goes wrong, it becomes difficult to tell whether the issue comes from LIO drift, wrong TF, point cloud mismatch, or Nav2 going crazy.

After debugging for long enough, you may even start wondering whether Ubuntu is personally targeting you.

---

3. Mapping Mode: Generating the 2D Map and 3D PCD Map First

The system first supports a mapping mode.

During mapping, the robot moves in a simulation environment. It runs LIO using LiDAR and IMU data, while generating both a 2D map and a 3D point cloud map.

The process can be understood as:

robot motion
    ↓
LiDAR scans the environment
    ↓
LIO estimates motion
    ↓
generate local point cloud and 2D LaserScan
    ↓
SLAM Toolbox builds the 2D map
    ↓
save 2D map + 3D PCD map

The 2D map is mainly used by Nav2, while the 3D PCD map is mainly used for later point cloud relocalization.

I did not force Nav2 to directly process the full 3D point cloud. Instead, I slice the 3D point cloud by height and convert it into data similar to a 2D laser scan, then feed it into the Nav2 costmap module.

This may not sound particularly fancy, but it is very practical.

In engineering, what matters is often not how advanced the architecture diagram looks, but whether the robot can run stably.

---

4. Relocalization Mode: KISS-Matcher + small_gicp

The core part of this system is relocalization.

The problem is as follows:

The robot already has a prior PCD map. Now it starts from an unknown position in the map. The system needs to use the current local point cloud to automatically match against the global map and estimate the robot’s current pose.

If only small_gicp is used, the limitation is quite obvious.

small_gicp is more suitable for local fine registration.

If the initial pose is already close to the true pose, it can converge quickly and provide good accuracy.

However, if the initial pose is far away from the real pose, GICP can easily fall into a local optimum or fail to match at all.

It is like dropping a person into an unfamiliar city and only giving them a few photos of nearby buildings, then asking them to immediately figure out where they are.

If you already tell them that they are probably near a certain street, they may be able to locate themselves.

But if you give them no clue at all, then the whole thing starts to feel like fortune-telling-based navigation.

So I designed the relocalization process as two stages:

Stage 1: KISS-Matcher global coarse registration
Stage 2: small_gicp local fine registration and continuous tracking

---

5. KISS-Matcher Handles Global Coarse Registration

In this system, KISS-Matcher is responsible for global initialization.

It does not rely on a highly accurate initial pose. Instead, it uses the accumulated local point cloud and the prior PCD map to perform global matching, then estimates a rough transformation.

The key point here is “coarse registration.”

It does not need to give a highly accurate pose in one step. As long as it can pull the robot from:

I have absolutely no idea where I am

back to:

I am probably somewhere around this region in the map

then small_gicp can take over.

Therefore, KISS-Matcher is not the final answer in this system. It provides a reasonably reliable initial guess for small_gicp.

In other words:

KISS-Matcher reduces the problem from hell-level difficulty to normal difficulty.

---

6. small_gicp Handles Fine Registration and Continuous Tracking

After KISS-Matcher completes global initialization, the system switches to small_gicp.

small_gicp uses the current /registered_scan and the prior PCD map for fine registration, and continuously maintains the robot’s pose in the global map.

Eventually, the system publishes:

map -> odom

This TF is critical.

LIO usually provides something like:

odom -> base_footprint

This describes the relative motion of the robot in the odometry coordinate frame.

However, Nav2 needs to know the robot’s position in the global map frame.

Therefore, the relocalization module needs to continuously maintain:

map -> odom

Then the complete TF chain becomes:

map -> odom -> base_footprint -> chassis -> livox_frame

As long as this TF chain is stable, Nav2 can normally perform path planning, update costmaps, and display the robot’s position in the global map in RViz.

---

7. Why Not Use Only small_gicp?

If the robot always starts from a fixed position, or if you can manually provide a fairly accurate 2D Pose Estimate in RViz every time, then using only small_gicp can work.

But this approach is not ideal to me.

Because in essence, it becomes:

Step 1: A human guesses an approximate pose
Step 2: The robot proves that the human guessed reasonably well

This is not intelligence. This is human-assisted intelligence.

What I want is for the system to first complete global coarse localization by itself when there is no accurate initial pose, and then enter continuous fine registration.

Therefore, this system uses:

KISS-Matcher coarse registration
    ↓
small_gicp fine registration
    ↓
continuously publish map -> odom

In this way, the robot can attempt relocalization from an arbitrary startup position.

Of course, this statement should not be exaggerated too much.

If the robot starts in an open area with too little geometric structure, or if the current environment differs too much from the prior map, then no algorithm can save it.

Algorithms are not magic. They cannot localize the robot from thin air.

More accurately, this system supports:

Global relocalization without an accurate initial pose when the current point cloud and prior map have sufficient structure and overlap.

This is much more practical than relying on a human to manually provide the initial pose every time.

---

8. The Value of Decoupled Design

Besides relocalization itself, another key point of this system is decoupling.

I do not want it to be a system that only works under one fixed configuration. Instead, I want it to serve as an experimental framework that can continue to evolve.

Currently, LIO, relocalization, and Nav2 mainly communicate through standard ROS 2 interfaces:

/registered_scan

/Odometry

TF: map -> odom -> base_footprint

PCD map

2D map

This means that later, when replacing algorithms, the whole system does not need to be rebuilt from scratch.

For example:

Replaceable Part	Description
FAST-LIO / Point-LIO	Switch between different LiDAR odometry backends
small_gicp / ICP	Switch between different local point cloud registration methods
KISS-Matcher + small_gicp	Use global coarse registration plus local fine registration
Simulation robot / real robot	Switch between different URDF and sensor configurations

This structure is important for robot debugging.

In real-world debugging, the common problem is not always that a certain algorithm is completely unusable. More often, you simply do not know which layer is causing the problem.

If the module boundaries are clear, the system can be checked layer by layer:

• Whether LIO outputs odometry correctly;
• Whether /registered_scan is published correctly;
• Whether the PCD map coordinate frame is correct;
• Whether only one node is publishing map -> odom;
• Whether the Nav2 costmap is updating correctly;
• Whether the extrinsic transform from base_footprint to livox_frame is correct.

The system may not be elegant, but at least problems can still be located.

For robotics development, this is already very important.

---

9. Current Implementation Results

The system currently supports two main modes.

9.1 Mapping Mode

In mapping mode, the robot moves in the Gazebo simulation environment. It runs LIO with LiDAR and IMU data, builds a 2D occupancy grid map, and saves a 3D PCD map.

This mode is mainly used to generate the prior maps required for later navigation and relocalization.

9.2 Relocalization Navigation Mode

In relocalization mode, the system loads an existing PCD map and matches the current /registered_scan against the prior map.

When using the KISS-Matcher + small_gicp solution, the workflow is:

accumulate current local point cloud
    ↓
KISS-Matcher global coarse registration
    ↓
initialization succeeds
    ↓
small_gicp continuous fine registration
    ↓
continuously publish map -> odom
    ↓
Nav2 navigation works normally

In RViz, the robot can be seen relocating itself back into the global map coordinate frame through point cloud matching.

Once map -> odom is published stably, Nav2 can perform path planning and navigation control in the global map.

---

10. Some Debugging Experience

10.1 Only One Node Should Publish map -> odom at the Same Time

This is a very important point.

If small_gicp and another relocalization node publish map -> odom at the same time, the robot pose may jump, and Nav2 may also behave abnormally.

This kind of issue looks like algorithm instability, but it is actually TF conflict.

Both nodes think they are correct, and in the end, the wrong one is you.

10.2 The Current Point Cloud and Prior Map Must Have Enough Overlap

Although KISS-Matcher can perform global coarse registration, it is not magic.

If the robot sees too little structure when it starts, or if the current local scan has insufficient overlap with the prior map, initialization may fail.

A practical method is to let the robot rotate in place or move slowly for a short distance during initialization, so that more local point cloud data can be accumulated.

The richer the point cloud structure is, the easier global initialization becomes.

10.3 Coordinate Frames Must Be Consistent

A common TF chain is:

map -> odom -> base_footprint -> chassis -> livox_frame

If the extrinsic transform from base_footprint to livox_frame is incorrect, or if the frame names are inconsistent, the relocalization result will be biased.

Many times, what looks like GICP failing to converge is actually a TF error from the very beginning.

This kind of problem is painful because it may not produce an obvious error. It just remains consistently wrong.

10.4 Downsampling Parameters Must Be Adjusted According to Map Scale

Both KISS-Matcher and small_gicp involve point cloud downsampling.

Common parameters include:

voxel_resolution
global_leaf_size
registered_leaf_size

If the voxel size is too small, computation and memory pressure increase.

If the voxel size is too large, geometric details are lost and matching quality decreases.

Therefore, these parameters should not be copied blindly. They need to be adjusted based on map size, point cloud density, and hardware performance.

There is no silver bullet for parameter tuning. There is only repeated testing.

This is also one of the simplest truths in robotics development.

---

11. Summary

The core of this ROS 2 3D LiDAR autonomous navigation system is not simply making Nav2 run. Instead, it focuses on two goals.

First, it uses KISS-Matcher + small_gicp to achieve global relocalization.

KISS-Matcher performs global coarse registration when there is no accurate initial pose. small_gicp performs local fine registration and continuous tracking. The system eventually continuously publishes:

map -> odom

This provides stable global localization input for Nav2.

Second, the overall system adopts a decoupled design.

LIO, relocalization, Nav2, robot description, and simulation modules communicate through standard ROS 2 interfaces as much as possible, making it easier to switch between:

FAST-LIO / Point-LIO

small_gicp / ICP / KISS-Matcher + small_gicp

simulation environment / real robot environment

The system is not perfect yet, but it has already solved the problem I cared about most:

The robot does not have to start from a fixed position every time, nor does it need to fully rely on a manually provided initial pose. Instead, it can attempt global relocalization through 3D point clouds.

In one sentence:

This is a ROS 2-based 3D LiDAR autonomous navigation system centered on global relocalization and designed around modular decoupling.

It is still not perfect, but at least it has evolved from “it runs, so do not touch anything” to “I know where it can move, and I also know which part may explode if it moves.”

For a ROS 2 robot navigation project, that already counts as a meaningful stage victory.