2017 IEEE Intelligent Vehicles Symposium (IV)

June 11-14, 2017, Redondo Beach, CA, USA

PCE-SLAM: A Real-time Simultaneous Localization and Mapping

using LiDAR data*
Pragya Agrawal, Asif Iqbal, Brittney Russell, Mehrnaz Kh. Hazrati, Member, IEEE, Vinay Kashyap,
and Farshad Akhbari, Member, IEEE

Recently the advent of high-speed computers and

Abstract This paper describes a real-time design,

implementation and validation of a LiDAR-based Simultaneous emergence of affordable low weight LiDAR sensors has
Localization and Mapping solution for intelligent vehicles. We enabled the new generation of LiDAR-based SLAMs. Higher
propose a two-step Sweep-to-Sweep Motion Estimation and resolution, greater depth of field and consistency in
Sweep-to-Map Registration framework that compensates for measurement errors over different environmental conditions
the distortion 


 
 

 




 

 
makes LiDAR a desirable source of data to estimate the
and generates a 3D map of the world. The rotating motion of motion of a moving vehicle. Rendering a 3D map in real-time
LiDAR and the longitudinal motion of the vehicle together from huge point cloud is a challenging problem and it is even
create an inherent distortion in relative motions observed in h%4()4
9,)1
6,)
-B5
291
426%6-1+
026-21
1))(5
62
&)

each scan per sweep. Hence applying the same translation and '215-()4)(
-1
%((-6-21
62
6,)
8),-'/)B5
)+2
026-21

rotation values to the entire sweep does not guarantee the
Calculating the accurate motion vector without relying on an
 

  

 

 

 






sweep. Therefore, the key idea in this paper is to obtain
accurate localization of the vehicle by processing LiDAR sweep
in a batch-wise fashion followed by 3D Map Registration using
Iterative Closest Point (ICP). To reduce drift in localization
ICP utilizes local map information in a radius of about 100m
from the position of vehicle. Our main contribution is to
introduce an innovative CPU-only pipeline for simultaneous
localization and mapping that runs real-time on Intel
external reference and source of localization data such as
GPS, fully relies on the accuracy of the SLAM algorithm.
Our literature survey reveals that there are a few research
articles that deal with a robust and real-time LiDAR SLAM
for autonomous driving [4, 9-10, 12-16]. There are several
open challenges that have yet to be tackled. Different
strategies were adopted to eliminate or neglect the distortion
architecture. We have tested our algorithm by processing every
sweep from Velodyne VLP-16 LiDAR at about 50ms on vehicle
moving at speeds up-to 25mph in urban roads and parking lot
structures. Our algorithm has been evaluated on KITTI
datasets for city and suburban roads with an average relative
position error of around 1%.
I. INTRODUCTION
S IMULTANOUS Localization and Mapping (SLAM) has
been a topic of research in robotics and artificial
intelligence for several years [1-15], only in the recent years
has it attracted attention from the autonomous driving
community [6, 7]. SLAM is a critical component for an
autonomous vehicle. It helps generate the surrounding map
and simultaneously perform ego localization within the map.
The core of SLAM algorithm is estimation of the motion of a
moving vehicle solely based on the data collected from its
perception sensors.
Majority of proposed algorithms up to date rely on vision
as the perception medium. Vision based approaches are
highly sensitive to ambient lighting, weather condition or
texture features [11]. When tested outdoors these factors are
major impediments for accurate localization, and hence not a
reliable solution for an intelligent vehicle [3, 4].
*
This work was fully supported by Intel Corporation.
P. Agrawal, B. Russell, M Kh. Hazrati, V. Kashyap and F. Akhbari are
with the Perceptual Computing Enabling, Internet of Things Group (IoTG),
Intel Corporation, Chandler, Az 85226.(e-mail: Pragya.Agrawal@intel.com,
Brittney.Russell@intel.com, Mehrnaz.Khodam.Hazrati@intel.com,
Vinay.Kashyap@intel.com, Farshad.Akhbari@intel.com).
A. Iqbal is an intern with the same group., (Asif_Iqbal@outlook.com)
978-1-5090-4804-5/17/$31.00 ©2017 IEEE 1752
(7)
62
026-21
1

7',6)4
)6
%/
75)(
%
?5623
%1(
5'%1A

strategy to prevent inserting any distortion due to the motion
into the point cloud [4]. Moosmann team in 2011 used a
single-axis 3D LiDAR to compensate distortion [12]. Later in
2013, Pomerleau et al. proposed an ICP-based approach to
match collected laser beams between different scans [13].
Dong et al. 2010 also showed that a two-step method can be
applied to remove the distortion [14]. They estimated the ego
velocity and then compensated the distortion according to the
approximate velocity. In all these methods the assumption is
that scanning motion is much higher than the ego motion,
otherwise the motion distortion cannot be compensated.
Several research teams applied constant velocity models in
their SLAM solutions [15, 16]. Among all [9] and [10]
reported outdoor real-time LiDAR-based SLAM tested on
moving vehicles. Authors in [10] reviewed near real time
LiDAR and visual SLAM algorithm in the context of
driverless cars. In [9] authors introduced a two-step LiDAR
based Odometry and Mapping, where mapping is done in
1Hz intervals.
Our goal is to develop an end-to-end system based on
LiDAR data suitable for urban environment to provide
localization in real-time on Intel architecture. Inspired from
[9] here we propose a two-step sequential process: Sweep-to-
Sweep Motion Estimation and Sweep-to-Map Registration.
In the first stage, LiDAR point cloud distortion is removed
and an initial estimate of motion is obtained. In the second
phase, the estimate for motion is further refined as distortion
free point clouds are matched and registered to incrementally
build maps. Though we have a two-step sequential process,
the capabilities we have put in place allow us to output a real-
time Simultaneous Localization and Map output for every
 Edge and Planar
Feature Extraction
Feature Points
Re-project sweep
VLP-16 LiDAR Sensor Interface
to Map Location
Raw LiDAR
Point Cloud
Motion Corrected Point Cloud
(Used to compare
against next sweep)
Figure 1. Flow diagram that demonstrates the PCE-SLAM algorithm. Each of the blocks mentioned is broken down and explained in Section III.
incoming sweep, approximately every 50ms, as opposed to
Odometry output every 100ms and Map update every 1s as
shown in [9]. The unique characteristic of our system
enables successful live testing in a moving vehicle up to the
speed of (25 miles/hour) in urban environment with limited
dynamic objects in the surroundings. The key idea that
enabled us to achieve this level of performance possible is the
batch-wise technique in Sweep-to-Sweep Motion Estimation
and threaded implementation of our algorithm. The proposed
framework is considerably robust against ambient lighting
and can be an appropriate solution for localization of
autonomous vehicles, especially when a GPS cannot be a
reliable source of information.
The paper is organized as follows. In Sect. II, the system
overview is presented, and the hardware and software
components of the system are introduced. In Sect. III. More
details on methods and algorithms are described. Sect IV,
specifically deals with real-time implementation. Sect. V
presents the results of several experiments that were carried
out in urban environment and within Intel campus in
Chandler, AZ to demonstrate the robustness and efficiency of
the proposed approach.
II. SYSTEM OVERVIEW
PCE-SLAM is a real-time solution processing data on a
quad core Intel CPU from a single Velodyne VLP-16 PUCK
LiDAR mounted on the roof of a vehicle. It estimates 6-
Degree of Freedom (DOF) pose of the moving vehicle while
following a trajectory and simultaneously creates a 3D point
cloud representation of the surrounding environment.
A. Sensor and Software Interface
PCE-SLAM uses a 16-channel Velodyne VLP-16 LiDAR
[20] module which provides a rich 3D point cloud in real
time. The VLP-16 has a horizontal field of view (HFOV) of
360
, Vertical field of view (VFOV) of 30
, maximum range
of up to 100m and produces 30,000 points per sweep at a
frequency of 20Hz. Point reflectance received in one 360

rotation of LiDAR is referred to one LiDAR sweep
A custom threaded interface was developed to collect data
from Velodyne VLP-16 in real time and send it to PCE-
SLAM software. The VLP-16 connects to systems through an
Ethernet interface and broadcasts data as UDP packets.
Motion corrected
Sweep to Sweep Sweep to Map
Mapping Feature Map Output
Motion Estimation Registration
Points
Batch
Residual Pose Transform Pose Transform Final Vehicle
ICP Residual
Integration Integration Pose Estimate
Previous Pose
Re-project to Transform Estimate
Refine Sweep
Estimate with Map
Residual
Advantage of VLP-16 over other LiDAR sensors is mainly
due to its compact size, lower cost, wider vertical FOV, and
lower point-cloud density compared to other sensors like the
HDL-32E/64E.
B. Algorithm Flow
A block diagram representing dataflow of the system can
be seen in Fig. 1. Each of the blocks mentioned is broken
down and explained in the following sections, Sect. III. To
begin with, every sweep produced by VLP-16 sensor is
received and re-projected to current vehicle location in the
world (map). Feature extraction is then run on the point cloud
and the indices of feature-point candidates are passed on and
used to calculate the edge and planar points for the sweep.
Once these features are found, their correspondences are
matched with the previous sweep. Each LiDAR sweep is
divided into four batches of scans for processing in Sweep-
to-Sweep Motion Estimation. The features and their
correspondences from each batch are used in Sweep-to-
Sweep Motion Estimation to obtain an estimate of relative
motion in each batch compared to previous sweep. The
mapping feature points selected previously are re-projected
using the batch estimates to correct the motion distortion and
are further processed in Sweep-to-Map Registration. The
residual value obtained from Iterative Closest Point algorithm
in Sweep-to-Map Registration is combined with the previous
pose estimate of vehicle to produce the final vehicle pose
estimate. LiDAR sweep represented by mapping feature
points is re-projected using the residual value to update the
map. The residual value from Sweep-to-Map Registration is
also combined with motion estimate from Sweep-to-Sweep
Motion Estimation. The updated pose transform matrix is
used to re-project the raw LiDAR point cloud (the motion
corrected point cloud obtained is compared against next
sweep).
III. ALGORITHMIC DEFINITION
PCE-SLAM is designed to work with two coordinate
frames of reference as described in the next page:
 The sensor frame, LiDAR-Coordinate System {S} -
always centered at LiDAR
 The world frame, World Coordinate System {W}
centered at initial position of vehicle (t=0)
1753
 It is assumed at t=0 the origin of the sensor coordinate
system coincides with the origin of the world coordinate
system.
While the sensor coordinate system observes a 360

rotating motion of LiDAR, the world coordinate system
observes the trajectory of vehicle defined with respect to its
-1-6-%/
325-6-21
!,)
8),-'/)B5
8)/2'-6:
'%75)5
(-56246-21
9,-/)

the LiDAR rotates to complete a 360
sweep, which needs to
be corrected before processing data for pose estimation.
PCE-SLAM estimates the motion in each coordinate
system and combines the two to give an accurate 6-DOF pose
estimate of the vehicle at every time frame. The 6-DOF pose
estimate represents the vehicles translation and
rotation angles in each of the x, y and z direction
of motion.
A. Feature Extraction
Features are distinctive 3D points found using spatial
information of a point cloud. Feature extraction is an
important pre-processing step of the algorithm that allows us
to filter out redundant points that may affect accuracy and/or
processing time. Our SLAM algorithm uses extracted
features that represent sharp edges and planar surfaces. These
are the prominent features in the point cloud that remain
consistent from sweep to sweep, and hence aid in motion
estimation. Each feature type edge point and planar point has
its own significance. Edge points help in finding strong
corner points which facilitate identification of lines. These
can be visualized as wall corners or the contour of an object
such as a car. On the other hand, planar points help maintain
the motion in z direction for trajectory estimation and map
alignment. These can be visualized as road surfaces, ceilings,
surface of a wall, etc.
Our feature extraction algorithm has been inspired from
previous works like [9] and [10]. To ensure features extracted
are evenly distributed throughout the point cloud, each of the
16 scan channels (laser beams) is split into four sub regions.
For Sweep-to-Sweep Motion Estimation (first stage of the
SLAM algorithm), a maximum of 8 edge points and 16
planar points are selected from each sub region of a scan. In
Sweep-to-Map Registration (second stage of SLAM
algorithm), five times the features in first stage are selected.
Hence, a point cloud with 300,000 points is reduced to 1536
points for processing in Sweep-to-Sweep Motion Estimation
and 7680 points for processing in Sweep-to-Map
Registration.
B. Sweep-to-Sweep Motion Estimation
After the LiDAR point cloud is reduced to a set of
features the first stage of PCE-SLAM algorithm uses these
features to estimate relative motion of the LiDAR observed
between two consecutive sweeps. The 6-DOF pose estimate
obtained at this stage is used to correct any distortions
observed in the point cloud in sensor coordinate system.
Each VLP-16 LiDAR sweep has 16 scans and a vertical
8-)9
2*

C
24
274
%33/-'%6-21
5'%15
%4)
&%6',)(
-162
execution. Other inputs for LM algorithm are edge and planar
scans close to the ground (and centered close to LiDAR) have
less motion distortion than the scans that are higher up due to
6,)
8),-'/)B5
026-21
%1(
*7rther away from LiDAR center).
Thus, instead of applying the same translation and rotation
values to all the scans, our algorithm corrects the motion
distortion for each batch of scans individually as follows:
 Transform the reduced point cloud of current sweep
(centered at LiDAR) using the final vehicle pose
estimate from previous time frame. This step aligns
the centers of current sweep and previous sweep for
accurate comparison.
 Followed by aligning the centers of two point clouds,
a correspondence is established between the features
of the two point clouds.
 Using the features from current sweep and
correspondences from previous sweep, an estimate of
motion in current sweep with respect to previous
sweep is obtained.
1) Feature Correspondence
Feature Correspondence or Feature Matching refers to the
process of finding points in previous sweep which correspond
to feature points extracted in current sweep. This one-to-one
correspondence helps to identify which features are
consistent from one sweep to next.
Our feature correspondence algorithm has been inspired
from previous works such as [9] [10]. Our approach for
feature correspondence selects two points forming a line for
every edge feature point and three points forming a plane for
every planar feature point.
Feature points from the current sweep which do not have
any strong correspondence in previous sweep, can be further
eliminated through the process of feature correspondence
rejection. Our algorithm rejects correspondences by
assigning weights wi to each of the feature point and
correspondence pair based l-2 norm distance metric.
2) Motion Estimation
Motion estimation solves a non-linear optimization
function by minimizing the distance between feature points
from current LiDAR sweep and their correspondences from
previous sweep. The output it generates is a pose transform
composed of translation and rotation angles
about x, y and z axis. In fact, instead of
calculating a single pose transform once for the entire sweep,
we proposed and implemented a batch-wise approach to
increase both accuracy and execution speed. In other words,
estimation of pose transform during the sweepp is split to
calculate for each batch (i
[1, 4]) called
d .
LiDAR motion estimation is solved using the Levenberg-
Marquardt (LM) method: where d is the distance
measure used to minimize the optimization function .
LM uses an initial estimate for equal to zero to start
+42735
6,%6
'28)4
<C
8)46-'%/
*-)/(
2*
8-)9
)1')
9)
,%8)
points, weights calculated for each of those points, and their
four batches in a sweep. Scans in the sweep correspondences for each batch. At the end of processing four
capture reflections from the ground to objects up high. Based batches we assign
g the residual from Sweep-to-Sweep Motion
on our study of raw LiDAR point-clouds, we observed the Estimation, = .

1754
 Within the LM algorithm, every iteration solves the
function to update the rotation angles and
translation values off . For every iteration of the LM
algorithm, each feature point (Xi) is reprojected to ( i) using
the equation:
i = R*Xi + (1)
using the updated rotation matrix R and translation values
from the previous iteration. The rotation matrix R
is computed from the rotation angles using the
Rodrigues formula [9]. Reprojected edge and planar points
along with their correspondences are then used to solve the
distance metric for optimization in next iteration. Each of the
pair-wise distances obtained are further weighed by the
individual weights wi of each feature point. The objective of
LM is to solve these distance functions by stacking them up
for all the feature points trying to minimize the norm of
distance vector and hence update the rotations and translation
values with every iteration. After the convergence criteria is
met, LM function returns the correct pose alignment between
the feature points and their correspondences.
C. Sweep-to-Map Registration
Map is a collection of sweeps, built by registering
consecutive sweeps by Sweep-to-Map Registration as the
vehicle travels. At time instant t the map is composed of
motion corrected and registered point clouds up-to time t-1.
Sweep-to-Map Registration uses the Iterative Closest Point
(ICP) Point-to-Plane algorithm which is responsible for
matching the mapping feature points from most current
sweep with the previously registered features on map.
ICP Point-to-Plane [20] algorithm aligns two point clouds
together, assuming there is certain overlap between the two.
The process is done by estimating a rigid transformation
between the two point clouds where the total distances
between the overlapping points are minimized. ICP algorithm
involves several stages, as can be seen in Fig. 2. The process
is iterated until the error is 1) smaller than a threshold, 2) the
maximum iteration limit is reached, 3) if the change in error
with consecutive iterations is less than 0.001. We empirically
observed that by initializing the pose transform matrix to
[I[3x3] | 0[1x3]], ICP convergence could be reached within 10
iterations for 99% of the ICP runs. A maximum limit of 25
iterations was set for the purpose of performance speed up
and avoid any deadlock conditions.
In Point-to-Plane approach, the objective function is to
minimize an error function which is defined as sum of the
squared distance between each point and the tangent plane at
its correspondence point. It has been shown that using Point-
to-Plane leads to faster convergence in comparison with
Point-to-Point [20]. Generally Levenberg-Marquardt method
is used for optimization phase which is in fact a non-linear
least square approach. In this work we were able to reduce
the run-time for each iteration of ICP algorithm, by
approximating the nonlinear optimization problem with linear
least-squares optimization (SVD). The problem can be
solved more efficiently, under the assumption that the
relative orientation between the two input surfaces is
negligible.
1755
The residual obtained from the ICP ( and
) is used to re-project the current sweep and
further added to the map. As the map keeps building, in order
to optimize the size of the data associated with the map, we
down sample the map every 10 sweeps. This helps us reduce
the computation as well as processing time. To select the
optimum section of map as the target point cloud for aligning
the next sweep, a radius search 9-6,-1

0
2*
6,)
8),-'/)B5

updated position in map is performed. Map data is stored in a
KD-Tree structure [22] for faster search.
D. Transform Integration
Our proposed algorithm requires transform integration
twice. First, when the residual from Sweep-to-Sweep Motion
Estimate is combined with the residual from ICP motion
estimate to get an updated Sweep-to-Sweep Motion Estimate.
Second, when the vehicle motion estimate from previous
sweep is combined with ICP residual from current sweep to
get an updated vehicle motion estimate at the end of current
sweep. This represents the updated location of vehicle in the
map.
Source Point Cloud
Target Point Cloud (Map)
(Current Sweep)
Calculate normal at every point to
eliminate outliers (Selection)
Edge and Planar Mapping Feature
Points (Selection)
Correspondence Estimation (Matching)
Correspondence Correspondence Correspondence
Rejection step 1 Rejection step 2
...
Rejection step n
Motion Estimation (Assigning and
Minimizing)
Figure 2. Process flow for Sweep-to-Map Registration based on ICP
algorithm
IV. REAL-TIME IMPLEMENTATION
Due to the nature of the software architecture several
opportunities for execution optimization arouse. We have
explored the Intel Architecture to make our application run
real-time entirely on a CPU. Parallelization of the code was
(21)
75-1+
16)/B5
!,4)%(-1+
7-/(-1+
/2'.5
!
9-6,

C++11 lambda features [17]. TBB acts as an extension to
C++ that enables the user to specify tasks. An internal
scheduler later maps these assigned tasks to system threads.
This eliminates hard-coded system thread implementations
and ensures optimizations to be scalable, portable, and
efficient on target systems [17].
As explained in Section III. B processing for Sweep-to-
Sweep Motion Estimation is split in four batches. A TBB
Task Group is assigned to each of the batches > allowing the
internal scheduler to intelligently balance threads according
to work load and run operations in parallel for efficient
utilization of CPU cores. Another high level threading
implementation added is an asynchronous function call. The
processing for mapping radius search is very time consuming
due to internal KD-Tree nearest neighbor search through a
large map. To optimize processing time, map is
downsampled after every 10 sweeps and further radius search
on the downsampled map is started through an asynchronous output on our own dataset collected in parking lot structure.
function call on a separate thread. This allows the map
around the updated vehicle pose to be collected as target
point cloud for registering next sweep while Sweep-to-Sweep
Motion Estimation for the next sweep starts. When Sweep-to-
Sweep Motion Estimation is finished and Sweep-to-Map
Registration is started, there is a blocking call to wait for the
radius search data to be collected. The data flow for the
algorithm tasks can be seen in Fig. 3.
In addition to high level task assignment, there were
several chances for deeper optimization. Functions
throughout the code using C++ for_loop have been
significantly sped up using TBB parallel_for. Additionally
compute intensive tasks like sorting were sped up using TBB
parallel_sort functionalities. In few instances where the
overhead for thread creation was more than the actual amount
of workload for each thread, multi-threading did not boost
performance enough. To boost performance for such
functions Single Instruction, Multiple Data (SIMD)
instructions -- Advanced Vector Extensions (AVX) SIMD
and Streaming SIMD Extensions (SSE) -- were implemented
for highly efficient vectorizing operations.

Figure 4. PCE SLAM output for Car moving outdoor parking lot
Sweep-to-Sweep Motion Estimation Sweep-to-Map Registration from A to B
Final
Vehicle
Batch 1 Blocking Call to Pose
(scans 1 - 4) Get Radius
S
T B1 Search Data
ICP
Batch 2 Motion
TSB2 Corrected Motion
(scans 5 -8) Downsample
Corrected
Point Point Cloud Map
Batch 3 Cloud
TSB3 using Current Vehicle Location &
(scans 9 - 12) Downsampled Map
TSB1-TSB4
TSB4 Updated
Batch 4 Async Call to
(scans 13 
16) Radius Search Map

Figure 3. Block diagram demonstrating multi-threaded data flow in Figure 5. Localization and Mapping outputs for car moving within an
PCE-SLAM indoor parking structure and closing the loop

V. RESULTS
The PCE-SLAM software developed was tested with
quad core 6th generation Intel Core i7 processor supporting
2.6 GHz to 3.4 GHz of frequency and 16 Gb memory in
Linux OS- Ubuntu 15.04. The algorithm has been tested to
run real-time in different test scenarios for vehicle speeds up-
to 25mph and limited dynamic objects in surroundings:
 City Roads
 Sub-urban roads
 Outdoor parking lot structures
 Indoor parking lot structures
Figure 6. PCE SLAM output on KITTI dataset for City Block.
The tests have been conducted by placing single
Velodyne VLP-16 LiDAR centered on the roof of a standard
passenger sedan. We have done live testing of our algorithm
on vehicle as well as on our own recorded datasets. The same
hardware and sensor set up is used for testing on recorded
dataset. Due to absence of a highly accurate IMU or
differential GPS unit, to demonstrate the accuracy of
localization output on our own datasets we have projected the
trajectory generated by our algorithm on Google Maps as
shown in Figure 4. Figure 5 shows localization and 3D Map Figure 7. PCE-SLAM output on KITTI datasets for Sub-Urban Roads.

1756
 The accuracy of the algorithm has been further tested on do not confirm a loop-closure status inside our algorithm.
KITTI [19] data. While KITTI data was recorded using a Since our application is developed for Automotive Space,
different LiDAR sensor (Velodyne VLP-64) our algorithm identifying loop-closure was out of our scope. Currently if
remains consistent and produces minimal error from ground the scene contains dynamic objects greater than speeds of
truth. Our algorithm even though developed for VLP-16, is %&276
03,
9)
2&5)48)
6,)
%/+24-6,0B5
%''74%':
-5

scalable and was not modified in any way to test our results affected. We plan to address this by adding support for
on KITTI dataset meaning that we still process sweep by tracking dynamic objects surrounding vehicle. This will
dividing it in four batches, each batch containing 16 scan further allow the solution to be scalable to highway driving
channels. Figures 6 and 7 show the localization and 3D- Map scenarios. We aim to add the above capabilities to our current
generated on KITTI datasets for City and Sub-Urban solution while maintaining real-time processing.
environment. Table I reports the average error in PCE-SLAM
localization output as evaluated using KITTI odometry REFERENCES
benchmark. [1] 

%56)//%125



216-)/

)-4%
%1(


!%4(25
?!,)

SPmap: A probabalistic framework for simultaneous localization and
TABLE I. AVERAGE ERROR FOR VEHICLE LOCALIZATION 0%3
&7-/(-1+A

!4%15%'6-215
21
2&26-'5
%1(
7620%6-21



.
[2] 
!,471
?2&26-'
0%33-1+

5748):A
!)',1-'%/
)3246
"-CS-
Average Average
Training Travel 02-111, School of Computer Science, Carnegie Mellon University,
Translation Rotation
Sequence Environment Distance Pittsburgh, 2002.
Error Error
Number (m) [3] P. Newman, D. Cole and K. Ho
?76(224

75-1+
#-57%/

(%) (deg/m)
33)%4%1')
%1(
%5)4
%1+-1+A
Proceedings of the 2006 IEEE
04 Sub-urban 397 1.4245 0.000230 International Conference on Robotics and Automation, 2006.
07 City Block 695 0.8915 0.010706 [4] A. Nuchter, K. Lingemann, J. Hertzberg, and H. Surmann
?6D SLAM-
3D mapping outdoor environmentsA Journal of Field Robotics, 2007.
09 Sub-urban 1717 1.0169 0.005627 [5] A. J. Davison, N. D. Molton, I. Reid, and O. Stasse, ?MonoSLAM:
10 Sub-urban 919 1.1857 0.005034 Real-time single camera SLAMA IEEE Transactions on Pattern
Percentage error in translation and rotation as generated from KITTI odometry evaluation benchmark
Analysis and Machine Intelligence, 2007.
[6] J. Levinson, M. Montemerlo, %1(

!,471
?%3-Based Precision
Table II reports the average run-time per frame for PCE- #),-'/)
2'%/-;%6-21
-1
"4&%1
18-4210)165A
2&26-'5
'-)1')
%1(

SLAM solution on different test datasets from KITTI which Systems Conference (RSS), 2007.
[7] 
25

%33%

215%
%1(

23);
?#-57%/
/%0
*24

uses 64 channel HDL-64E and our own datasets using 16 4-8)4/)55
%45

4-)*
748):A
IEEE Workshop on Navigation,
channel VLP-16 LiDAR module each refreshing at 50ms. Perception, Accurate Positioning and Mapping for Intelligent
The results show that our implementation produces Vehicles, 2012.
localisation and mapping outputs at a rate faster than refresh [8] C. F. Olson, ?Probabilistic self-localization for mobile robotsA IEEE
rate for VLP-16. Even with 4 times more data to be processed Transactions on Robotics and Automation, 2000.
from HDL-64E, PCE-SLAM still runs near run-time. [9] 
$,%1+
%1(

-1+,
?
LiDAR Odometry and mapping in
real-6-0)A
2&26-'5
'-)1')
%1(
:56)05
21*)4)1')
 
7/:

2014.
TABLE II. AVERAGE RUN TIME PER FRAME FOR PCE-SLAM [10] 
$,%1+
%1(

-1+,
?Visual-LiDAR Odometry and Mapping: Low-
SOLUTION drift, Robust, and FastA
IEEE International Conference on Robotics
and Automation (ICRA), 2015.
Number of KITTI (HDL-64E) PCE-Dataset (VLP-16)
Frames (ms) (ms) [11] T. Manderson, F. Shkurti a1(
4
7().
?Texture-Aware SLAM
Using Stereo Imagery and Inertial Information, 13th Conference on
271 60.4989 36.5144 Computer and Robot Vision, 2016.
1101 60.8742 40.1669 [12] Moosmann, F., & Stiller, C.
?Velodyne SLAMA
IEEE Intelligent
Vehicles Symposium (IV). Baden-Baden, Germany, 2011.
1591 67.4863 42.3497 [13] Pomerleau, F., Colas, F., Siegwart, R., & Magnenat, S. ?Comparing
ICP variants on real-world data setsA Autonomous Robots, 2013.
1201 63.2976 43.5988
[14] H. Dong, and T. Barfoot, ?Lighting-invariant visual Odometry using
LiDAR intensity imagery and pose interpolationA
The 7th International
VI. CONCLUSION AND FUTURE WORK Conference on Field and Service Robots, Matsushima, Japan, 2012.
[15] S. Anderson, and T. Barfoot, ?Towards relative continuous time
Only recently has the accuracy for SLAM solutions SLAMA IEEE Int. Conference on Robotics and Automation (ICRA),
matured enough to focus on real-time performance. In this Karlsruhe, Germany, 2013.
paper we presented our implementation of Simultaneous [16] 

7-8%16
%1(


)&26
?Optimization of the simultaneous
Localization and Mapping using Velodyne VLP-16 LiDAR localization and map building algorithm for real-time implementationA
in real-time. The paper addresses solution for a challenging IEEE Transaction on Robotics and Automation, 2001.
[17] J. Reinders, Intel Thread Building Blocks. 1st ed. Sebastopol, CA:
problem to estimate an accurate vehicle trajectory using only
LiDAR without fusing any IMU or GPS data at the refresh [18]
rate of LiDAR sensor. Our unique approach of batch-wise [19]
distortion correction in the LiDAR point cloud followed by
accurate pose estimation based on position of vehicle in map
[20]
using Iterative Closest Point are the differentiating features of
our work. Deep optimization of algorithm on Intel
Architecture has given us real-time performance [21]
demonstrated on a car. The algorithm has also produced good
results on KITTI datasets. While the localization output [22]
shows that the vehicle returns close to its starting location, we
1757
B)-//:
)(-%

#)/2(:1)5
"= (VLP-16) sensor User Manual
A. Geiger, P. Lenz, R. Urtasun
?Are we ready for autonomous
driving? the KITTI vision benchmark suiteA
Computer Vision and
Pattern Recognition (CVPR), 2012.
K. Low
?Linear least-squares optimization for point-to-plane ICP
surface registrationA Tech. report, TR04- 004, University of North
Carolina, 2004.

$,%1+
%1(

-1+,
?29-drift and Real-Time Lidar Odometry and
%33-1+A
Journal of Autonomous Robots, 2017.
M. de Berg, M. van Kreveld, M. Overmars, and O. Schwarzkopf,
Computation Geometry: Algorithms and Applications, 3rd Edition.
Springer, 2008