Summary of LIDAR scan matching algorithms

1 Introduction

These days I’m working on LIDAR odometry and mapping. That is, for a mobile robot equipped with a LIDAR sensor, we wish to find out what the robot’s pose is, and generate a map of the world. This is known as simultaneous localization and mapping (SLAM).

Algorithms that do this can be largely divided into broad categories:

Just odometry by itself is prone to drifting away from the true trajectory over time, so combining the de-warped point clouds in the mapping part can mitigate this. Even then there may be some drift, but some such open-loop algorithms (e.g. 4 ) are quite successful.

Odometry vs mapping
FIGURE 1 Picture showing point sets obtained from odometry being registered to a bigger point set during the mapping step. Image credit 4.

This blog post will focus on the scan matching process, since I was given some de-warped data that need to be aligned with minimal drift.

2 Point to point registration

A LIDAR sensor measures distance to the nearest obstacle in many directions. So its output is a point cloud with one point for each of those measurements.

Point set registration
FIGURE 2 Registration of two scans (blue, and grey), with the red lines showing correspondences between features. Image credit Martin Holzkothen, Michael Korn.

Point set registration is the problem of aligning two point sets, often denoted M and S (for map and scene). Here we will talk about registering S to M but in many papers they talk about registering M (model) to S instead.

Point set registration algorithms often use the following iterative algorithm:

  1. Estimate correspondences between points.
  2. Solve for the optimal transform assuming those correspondences.

Since step 2 might change the correspondences, these two steps are repeated until convergence. There are multiple ways to estimate correspondences.

ICP and its variants are so ubiquitous that almost all point set registration and scan matching programs use it. However, it sometimes get stuck in local minima when the initial positions are not well-aligned. By relaxing the correspondence assumptions, the latter two algorithms are more robust against poor initial alignment, noisy outliers and removed points. In all cases, the cost function is some (possibly weighted) sum of distances of each point in S to its corresponding point in M.

There are various ways to estimate the transformation that minimizes the cost. It depends on how you parametrize the transformation (e.g. rigid transformation, vs non-rigid using thin plate splines, etc). Then, you can use least squares, the Levenberg-Marquadt algorithm, or something else.

There are also other point set registration algorithms that don’t adhere to the idea of iteratively estimating correspondence and then solving for a transformation. For example, the kernel correlation method 3 assigns a kernel (e.g. a Gaussian) centered at each of the points and then uses gradient descent to maximize the correlation between those kernels.

Note that in addition to spatial distance, point set registration algorithms can also make use of normals information and colour information where available.

3 Higher level features

In laser scan matching, especially using data produced by LIDAR sensors that have a small number of lasers, data sets are often sparse and may appear to have spurious patterns (i.e. “rows” of points along each scan line) caused by the way the scanner collects data. Thus, assuming that each point in S corresponds to points in M may not always work. One way to get around this is to use a model specifically tailored to the pattern of points produced by a LIDAR, e.g. polar matching 7. This does not work in the general case, for example when points have been de-warped using an external source. A higher-level representation of the point cloud can be used for robust scan matching.

some NDTs
FIGURE 3 Some two-dimensional NDTs

In most cases, the cost function may be minimized using any nonlinear regression method such as the Levenberg-Marquadt. As with point set registration, for additional robustness against outliers, we can pass the cost through a robust function such as the Cauchy distribution — this is known as an M-estimator.

4 Other constraints

In the specific context of a mobile robot, there are other constraints useful for laser scan matching, which arise from physical assumptions about the robot. These introduce additional terms in the cost function to penalize unfeasible solutions caused by numerical stability issues due to noise. The following constraints have been used in 6 for a mine exploration robot.

5 Ideas for improvement

6 References