Previous Likelihood Map: ॸ
௧ିଵ
Laser Scan: ܵ
௧
Estimate the Velocity of Scanner: ܸ
Optimize Rigid-body Transformation: ܶ
Transformed Laser Scan: ܵ
௧ כ
Generate Likelihood
Map: ܯ
௧
Update Likelihood Map
ॸ
Figure 2: The flowchart of our proposed SLAM algorithm. where T ⊗ S
t
is the set of laser points S
t
transformed by the rigid body transformation T.
The key issues of a grid-based probabilistic scan-matching al- gorithm can be summarized as follow: the generation of the
occupancy likelihood map from all previous maps, and an op- timization framework that allows us to find the optimal rigid-
body transformation.
In this paper, we propose a series of efficient techniques to solve those problems. Figure
1 shows the
mobile UGV with a single laser scanner SICK LMS-100 used in this paper. The flowchart of our proposed SLAM algorithm
is illustrated in Figure 2
. Let S
t
be the current laser scan at the time
t and M
t−1
be the occupancy likelihood map at the previous time
t − 1, which is learned from all previous scans. Firstly, we estimate the rigid-body transformation T between
the current scan S
t
with the previous map M
t−1
by combining the brute search and the gradient ascent implemented by the
Levenberg-Marquardt LM algorithm. At the same time, to
reduce the influence of the scan distortion caused by the motion of the UGV, we integrate the velocity of the laser scanner into
the LM optimization framework. Secondly, we transform S
t
into the same coordinate system of M
t−1
based on the rigid- body transformation T, which is denoted as S
∗ t
= T ⊗ S
t
. The likelihood map M
t
of S
∗ t
can be generated by applying the 2-dimension Gaussian blurring operation along the contours
connected by the points of the laser scan S
∗ t
. At last, we
learn the new occupancy likelihood map M
t
at the current time t via a novel strategy inspired by the background subtraction
method proposed by Yao and Odobez [ 25
]. All the above steps are repeated with the acquisition of laser scans and the final
occupancy likelihood map M can be robustly achieved.
2.2 Likelihood Map Generation
The fundamental component of the grid-based scan matching algorithm is the occupancy likelihood map generated from all
previous scans, which allows us to compute the occupancy likelihood of each point.
One common approach for map generation is to first store all points from all previous scans. Then,
for each point x in the current scan S
t
, the distance to the closest point x
′
is computed. Next, we apply the Gaussian probability model to simulate the laser measurement noise. In this way, the
occupancy likelihood of each point can be defined as: P x|M
t−1
∝ exp −dx, x
′
σ, 3
where dx, x
′
denotes the distance between x and x
′
, and σ stands for the standard deviation of the sensor measurement
noise. However, this approach probably fails in some situations due to
that it attempts to find correspondences for all points in the scans even though a number of points may not correspond to the current
scan. As we know, the main structure of indoor environments is comprised of planar surfaces, which results in a set of piecewise
linear line segments in a 2D laser scan. For the points in the current scan, we maybe can’t find the corresponding points in
all previous scans, but have the bigger chance to locate on the same surfaces represented by contours. Therefore, for a point
x in the current scan, computing its occupancy likelihood with
the distance dx, C to the closest contour C extracted from all
previous scans is more reasonable, such as P x|M
t−1
∝ exp −dx, Cσ. 4
In this contour-based approach, the grids crossed by the contours have the same and the biggest probability, and the occupancy
likelihood values of the remaining grids are obtained by applying 1-D Gaussian blurring along the contours. An example of the
last likelihood map is illustrated in Figure
3 b. However, this
approach assume that the grids crossed by the contours are all the robust features in this environment, and completely ignores
the error of the contours. Sometimes, the error of contour
connection maybe occurs, especially when the distance between two adjacent points become longer with increasing of the point
distance to the center of the laser scanner. For example, we assume that there is an open door on the wall, and two adjacent
wall points located in different sides of an open door are possibly connected into a same contour which represents the wall. In this
way, the grids covered by this door region are regarded as robust features. Obviously, it is unreasonable. Thus, in some cases, the
traditional contour-slope model maybe can’t accurately represent the scanned scenes.
In this paper, we propose a new improved contour-based ap- proach and combine the above-mentioned two common map
generation approaches. Firstly, all contours from the laser scan need to be extracted. All distances
{d
k
|d
k
= kx
k
− x
k−1
k}
K k=2
between all pairs of adjacent points are sorted in an ascending order, where
K denotes the number of all points in the laser scan. A pair of adjacent points
x
k−1
, x
k
is selected as the seed of an initial contour if it satisfies following constraints:
d
k
is the smallest and smaller than the predefined distance threshold, and x
k−1
or x
k
is not jointed by other extracted contours. The adjacent points
{x
i
} are iteratively joined until no more ones satisfy the following constraints:
d
i
≤ d
th
, maxd
i
, d
j
mind
i
, d
j
≤ r
th
, 5
where d
i
denotes the distance between x
i−1
and x
i
, i.e., d
i
= kx
i
− x
i−1
k, x
j
stands for one of the endpoints of the current contour, x
i
is next to x
j
, and d
th
and r
th
are two thresholds for the distance and the distance ratio, respectively, which were set
as d
th
= 150mm and r
th
= 2 in this paper. After collecting one contour, we iteratively repeat the above procedures to find new
contours from the remaining points of the scan. ISPRS Annals of the Photogrammetry, Remote Sensing and Spatial Information Sciences, Volume III-4, 2016
This contribution has been peer-reviewed. The double-blind peer-review was conducted on the basis of the full paper. doi:10.5194isprsannals-III-4-119-2016
121
a b
c d
Figure 3: Illustrations of the contour extraction and the occupancy likelihood map generation: a three contours in blue extracted from the raw laser measurements alongside the raw laser readings in red dots; b the likelihood map generated from the contours by applying
a 1-Dimension Gaussian blurring operation along the contour using the method presented in [ 20
]; c the occupancy likelihood map generated by our proposed method; d a detailed view for our proposed method Left: the original laser points in a contour; Right: the
occupancy likelihood map by applying our propsed 2D Gaussian blurring operation.
Figure 4: An illustration of the normal estimation by
RDPCA:Left the partial points of a laser scan; Right the corresponding orientations of the point normals marked in red
lines, which were computed by RDPCA.
Compared with the traditional contour-based approach, we re- place the 1-D Gaussian blurring operation with the 2-D one. For
each pair of points x
i
, x
j
in the same contour, we regard the direction of the line connecting x
i
and x
j
as the x-axis and
the y-axis is defined as the vertical direction of the x-axis. The
likelihood of a point x is computed as follow: P x|M ∝ exp −d
x
x, Cσ
x
− d
y
x, Cσ
y
, 6
where σ
x
and σ
y
stand for two standard deviations in the x- and
y-axes, respectively, which were set as 1.5 and 10 in this paper. d
x
x, C and d
y
x, C denote the distances to the contour along the
x- and y-axes, respectively, which are computed as follow: d
x
x, C = min
x
k
∈C
d
x
x, x
k
, d
y
x, C = dx, C, 7
where d
x
x, x
k
represents the distance from x to x
k
along the direction of the
x-axis. Figure 3
c shows an example of the occupancy likelihood map created by this new approach. From
Figure 3
a, we can find that this laser scan is comprised of three contours, which are numbered as
1, 2 and 3, respectively. The distance between two adjacent points becomes bigger as
the extension of contours, especially for the longest contour numbered as
1. Compared with the occupancy likelihood map shown in Figure
3 b, we can find that the occupancy likelihood
value of each grid crossed by the contour is not always the same and the biggest as shown in Figure
3 c, which is also
determined by the distance to the closest point along the x-axis,
especially in the local regions marked by the upper-right box. But if the distance between two points is small, the likelihood map is
similar for two methods, as shown in the local region marked by the lower-left box. Our proposed 2D Gaussian blurring results
are more easily observed in a detailed view of partial contour as shown in Figure
3 d.
2.3 Scan-to-Map Matching
