Robotics: Science and Systems 2014
Berkeley, CA, USA, July 12-16, 2014
1
LOAM: Lidar Odometry and Mapping in Real-time
Ji Zhang and Sanjiv Singh
Abstract— We propose a real-time method for odometry and
mapping using range measurements from a 2-axis lidar moving
in 6-DOF. The problem is hard because the range measurements
are received at different times, and errors in motion estimation
can cause mis-registration of the resulting point cloud. To date,
coherent 3D maps can be built by off-line batch methods, often
using loop closure to correct for drift over time. Our method
achieves both low-drift and low-computational complexity with-
out the need for high accuracy ranging or inertial measurements.
The key idea in obtaining this level of performance is the
division of the complex problem of simultaneous localization and
mapping, which seeks to optimize a large number of variables
simultaneously, by two algorithms. One algorithm performs
odometry at a high frequency but low fidelity to estimate velocity
of the lidar. Another algorithm runs at a frequency of an order of
magnitude lower for fine matching and registration of the point
cloud. Combination of the two algorithms allows the method to
map in real-time. The method has been evaluated by a large set
of experiments as well as on the KITTI odometry benchmark.
The results indicate that the method can achieve accuracy at the
level of state of the art offline batch methods.
I. INTRODUCTION
3D mapping remains a popular technology [1]–[3]. Mapping
with lidars is common as lidars can provide high frequency
range measurements where errors are relatively constant irre-
spective of the distances measured. In the case that the only
motion of the lidar is to rotate a laser beam, registration of the
point cloud is simple. However, if the lidar itself is moving
as in many applications of interest, accurate mapping requires
knowledge of the lidar pose during continuous laser ranging.
One common way to solve the problem is using independent
position estimation (e.g. by a GPS/INS) to register the laser
points into a fixed coordinate system. Another set of methods
use odometry measurements such as from wheel encoders or
visual odometry systems [4], [5] to register the laser points.
Since odometry integrates small incremental motions over
time, it is bound to drift and much attention is devoted to
reduction of the drift (e.g. using loop closure).
Here we consider the case of creating maps with low-
drift odometry using a 2-axis lidar moving in 6-DOF. A key
advantage of using a lidar is its insensitivity to ambient lighting
and optical texture in the scene. Recent developments in lidars
have reduced their size and weight. The lidars can be held by
a person who traverses an environment [6], or even attached
to a micro aerial vehicle [7]. Since our method is intended to
push issues related to minimizing drift in odometry estimation,
it currently does not involve loop closure.
The method achieves both low-drift and low-computational
complexity without the need for high accuracy ranging or
J. Zhang and S. Singh are with the Robotics Institute at Carnegie Mellon
University. Emails: zhangji@cmu.edu and ssingh@cmu.edu.
The paper is based upon work supported by the National Science Founda-
tion under Grant No. IIS-1328930.
Lidar
Lidar
Mapping
Odometry
Fig. 1. The method aims at motion estimation and mapping using a moving
2-axis lidar. Since the laser points are received at different times, distortion is
present in the point cloud due to motion of the lidar (shown in the left lidar
cloud). Our proposed method decomposes the problem by two algorithms
running in parallel. An odometry algorithm estimates velocity of the lidar and
corrects distortion in the point cloud, then, a mapping algorithm matches and
registers the point cloud to create a map. Combination of the two algorithms
ensures feasibility of the problem to be solved in real-time.
inertial measurements. The key idea in obtaining this level of
performance is the division of the typically complex problem
of simultaneous localization and mapping (SLAM) [8], which
seeks to optimize a large number of variables simultaneously,
by two algorithms. One algorithm performs odometry at a high
frequency but low fidelity to estimate velocity of the lidar. An-
other algorithm runs at a frequency of an order of magnitude
lower for fine matching and registration of the point cloud.
Although unnecessary, if an IMU is available, a motion prior
can be provided to help account for high frequency motion.
Specifically, both algorithms extract feature points located on
sharp edges and planar surfaces, and match the feature points
to edge line segments and planar surface patches, respectively.
In the odometry algorithm, correspondences of the feature
points are found by ensuring fast computation. In the mapping
algorithm, the correspondences are determined by examining
geometric distributions of local point clusters, through the
associated eigenvalues and eigenvectors.
By decomposing the original problem, an easier problem is
solved first as online motion estimation. After which, mapping
is conducted as batch optimization (similar to iterative closest
point (ICP) methods [9]) to produce high-precision motion
estimates and maps. The parallel algorithm structure ensures
feasibility of the problem to be solved in real-time. Further,
since the motion estimation is conducted at a higher frequency,
the mapping is given plenty of time to enforce accuracy.
When running at a lower frequency, the mapping algorithm
is able to incorporate a large number of feature points and use
sufficiently many iterations for convergence.
II. RELATED WORK
Lidar has become a useful range sensor in robot navigation
[10]. For localization and mapping, most applications use 2D
lidars [11]. When the lidar scan rate is high compared to
its extrinsic motion, motion distortion within the scans can