Realtime Scanmatching and Localization Using a Spinning Laser Rangefinder

Tudor Achim
For a robot to be useful in the home or office, it must be able to construct a detailed map of
its surroundings to use for navigation planning. Until recently, most approaches involved a 2D laser
rangefinder that took range readings in a fixed plane and used those to estimate either sensor motion
between two scans, or a global position in a precomputed map.
Unfortunately, this method is not very robust to outliers. Though the robot may be able to remove
a person’s legs from the scan as they walk in front of the laser, a more dynamic environment poses
greater challenges. For example, if the robot sees a chair’s legs in one part of the room, and builds it
into its map, and then the chair is moved, any position estimate with the new chair in view may be
significantly off. Another plausible scenario for a personal robot is navigation in a cluttered room; this
one poses much the same problems as the chair example.
The simplest solution to this problem is to mount a 2D rangefinder high up on the robot, where it can
scan above peoples’ heads and objects that might conceivably be dynamic, like cabinets. Another simple
approach would be to have a camera point upwards and reconcile images with a precomputed global
map, like the MINERVA robot. However, the robot I had access to is not tall enough to accommodate
the first option. Furthermore, it was already fitted with a spinning 2D rangefinder to help with arm
planning, so it was decided to also use it for localization.

1. First Steps
The apparatus with which 3D range scans are collected consists of a small hokuyo laser rangefinder,
attached to a motor with an encoder. The axis of rotation forms about a 20◦ angle of declination with
the horizontal plane. By symmetry, each half of a revolution of the scanner gives one full scan about the
robot, composed of 60 scanlines in about 1.4 seconds. Because each scanline consists of 1081 individual
range measurements over 270◦ , applying ICP to the raw point data would take more than 1.4 seconds
per scan, which is too slow.
Therefore, it was clear that the solution would depend on taking advantage of the enormous redun-
dancy of most of the data. For example, the plane representing the ceiling might contain 20,000 points,
although it could be described completely by its normal and extents. Thus, scanmatching the planes
from two scans seemed like a time and space-efficient way to determine the sensor’s displacement over
time. Initial experiments used an existing point cloud processing library to segment planes from the
scans. The library implemented the random sample consensus (RANSAC) algorithm, which segmented
planes by choosing three points in the scan at random, and seeing how well the rest of the scan fit the
plane defined by those points.
Though robust results were obtained, segmenting the planes took about 1.2 seconds, which left
too little time for the scanmatching portion. This was caused by the obviously suboptimal algorithm:
although it is guaranteed to yield correct results, incorporating even a little bit of prior knowledge about
the structure of the cloud can yield dramatic speedups.
Around this time, I was pointed to the paper “Continuous Scanmatching with a Spinning 2D Laser

1
Rangefinder”, by Bosse and Zlot. It described a way to match scans that took advantage of the local
structure of the point clouds, but upon contacting the authors it was discovered that around 5 seconds
of processing were required for every one second of real time, which precluded using it for the current
project.

2. Local Point Cloud Structure

To deal with the redundancy of the point cloud, the workspace was discretized into voxels with
0.3m per side, PCA (principal component analysis) was performed on the points falling in each voxel.
Intuitively, this operation calculates the axis along which most of the variation in the set of points lies,
the orthogonal axis along which the second highest variation lies, and finally the third orthogonal axis,
corresponding to the least variation. If {p1 , . . . , pn } are the points in the voxel and their mean is µ, we
define
N
1 X T
S= pi pi − µµT
N
i=1
where S has eigenvalues λ1 ≤ λ2 ≤ λ3 corresponding to eigenvectors v1 , v2 , v3 . Then, a measure of the
plane and ellipsoid similarity of the points in the voxel is given by
λ3 − λ2
c=
λ1 + λ2 + λ3
λ2 − λ1
p=2
λ1 + λ2 + λ3
p > c is interpreted as meaning that the voxel’s points resemble a plane more than an ellipsoid. In
practice, it is also useful to enforce the conditition that p ≥ 0.6. If the latter is satisfied, then the normal
to the voxel is given by v1 .
The procedure to segment planes from the cloud has two parts. First, the local shape parameters are
computed for each voxel. Then, a probabilistic region growing algorithm, inspired by RANSAC, is used.
Whereas RANSAC would choose three arbitrary voxels as the seed for a plane, the current algorithm
chooses a random planar voxel, along with two distances and two angles. Then, it projects the segments
defined by the two distances and angular directions onto the plane defined by the points in the voxel,
and checks if the voxels containing the endpoints are planar, with approximately the same normal. If
this test passes, one takes the initial voxel and the averaged normal and applies a simple region growing
algorithm.
In practice, this operation takes less than 0.1 seconds, which is acceptable.

3. Scanmatching
Upon testing the plane growing algorithm on the real robot, I discovered that he had vastly overesti-
mated the accuracy of the wheels’ encoders during rotations. Any points further than one meter from
the robot were noticeably distorted, and rendered any efforts to segment planes ineffectual. Because of
this, I revisited the paper, hoping to write a realtime implementation.
The idea is to discretize the trajectory spanning two scans and apply small corrections to all the
guesses, each of which were provided by odometry.

2
 7

4
3

1 Boxes represent positions of scanlines at each trajectory

sample.

Global Frame

The curved line represents the actual trajectory of the robot, and each small frame is a sampled pose.
We note that because the laser is spinning, only samples 1 and 5, 2 and 6, 3 and 7, etc will have voxels
that match up with each other.
Once voxel correspondences have been calculated, it remains to compute the corrections δti and δri
for each τi corresponding to the times when poses were sampled. Let µa and µb denote the centroids of
a matched pair. After deriving their “average” normal from the eigenvector of Sa + Sb corresponding to
the smallest eigenvalue, if their poses are exact, it needs to be the case that →
−
n T (µa −µb ) = 0. Intuitively,
any displacement between the two centroids must lie in the plane perpendicular to the normal.
Since the rotational correction causes the system to be nonlinear, the rotation is linearized, and several
iterations of trajectory updates are computed for each matching of voxels. To do this, the approximation
sin δr = δr and cos δr = 1 is used. As the mean time for points added to each voxel most often does
not fall on a pose timestamp, linear interpolation between adjacent poses is used.

4. Global Scan Registration

To complete the localization package, I will implement some variant of a global registration algorithm.
Current plans are to sample uniformly from the probability distribution over states, and to evaluate each
sample quicklybased on how well the planes in sight match up with the planes that one would expect
to see given that one is in the position dictated by the sample.
Using planes instead of voxels should allow for testing many more samples per unit time, because
there are relatively few planes in a scan compared to the number of voxels.

3
5. Results
In conclusion, the method given above is successful; on average, 1.2 seconds are needed to match
two scans, even with large translations and rotations. Combined with fast plane-based Monte Carlo
localization, it seems reasonable to expect that the whole package can be made to run in fewer than
1.4 seconds, which is enough to be considered realtime.
An example of the scanmatcher is given below. The first image represents a robot moving backwards
at a constant speed from a cabinet, with the first sweep in black and the second in green. The second
image is the match without smoothness constraints applied, and the third image shows a match with
smoothness constraints. In all three, the blue lines represent the estimated correspondences between
voxels.

4
 Although the implementation still has some bugs related to properly specifying the constraints, fixing
them will not affect the time required to solve the system because the same number of constraints will
apply. Specifically, in the previous image it is clear that a few voxels were left unmatched, which implies
that the real pose corrections were not recovered, and a few voxels were matched properly, but both
were shifted away from their real positions (for example, some ceiling voxels).

6. References
M. Bosse and R. Zlot. Continuous 3D Scan-Matching with a Spinning 2D Laser. ICRA 2009.