Auton Robot (2008) 24: 13–27
DOI 10.1007/s10514-007-9050-y
A line feature based SLAM with low grade range sensors using
geometric constraints and active exploration for mobile robot
Young-Ho Choi · Tae-Kyeong Lee · Se-Young Oh
Received: 27 February 2007 / Accepted: 29 August 2007 / Published online: 19 October 2007
© Springer Science+Business Media, LLC 2007
Abstract This paper describes a geometrically constrained
Extended Kalman Filter (EKF) framework for a line feature
based SLAM, which is applicable to a rectangular indoor
environment. Its focus is on how to handle sparse and noisy
sensor data, such as PSD infrared sensors with limited range
and limited number, in order to develop a low-cost naviga-
tion system. It has been applied to a vacuum cleaning robot
in our research. In order to meet the real-time objective with
low computing power, we develop an efficient line feature
extraction algorithm based upon an iterative end point fit
(IEPF) technique assisted by our constrained version of the
Hough transform. It uses a geometric constraint that every
line is orthogonal or parallel to each other because in a gen-
eral indoor setting, most furniture and walls satisfy this con-
straint. By adding this constraint to the measurement model
of EKF, we build a geometrically constrained EKF frame-
work which can estimate line feature positions more accu-
rately as well as allow their covariance matrices to converge
more rapidly when compared to the case of an unconstrained
EKF. The experimental results demonstrate the accuracy and
robustness to the presence of sensor noise and errors in an
actual indoor environment.
Keywords Line feature · SLAM · EKF · Geometric
constraint · Environmental modeling
Y.-H. Choi () · T.-K. Lee · S.-Y. Oh
Department of Electrical Engineering, Pohang University of
Science and Technology, Pohang, Republic of Korea
e-mail: rockboy@postech.ac.kr
T.-K. Lee
e-mail: devilee@postech.ac.kr
S.-Y. Oh
e-mail: syoh@postech.ac.kr
1 Introduction
In most applications, a mobile robot must be able to self-
localize and build a map to perform a variety of useful long-
term tasks. Therefore, simultaneous localization and map-
ping (SLAM) has been one of the most crucial issues for
mobile robot navigation. It is the process of building a map
of the environment while concurrently generating an esti-
mate for the location of the robot whose objective is to min-
imize the localization and mapping errors simultaneously.
The history and critical issues of SLAM are well discussed
in Frese (2006).
In the last ten years, a great many SLAM algorithms
have been proposed. The laser range finder based SLAM
(Diosi and Kleeman 2005; Hähne et al. 2003; Tomono 2004;
Nguyen et al. 2006) has been the most popular in various ap-
plications owing to the sensor’s superior characteristics in-
cluding long range, high angular resolution, and dense scan-
ning. Diosi (2005) proposed a method for 2D laser scan
matching called Polar Scan Matching (PSM), which be-
longs to a family of point to point matching strategies but
avoids exhaustive searching of all point associations by sim-
ply matching against points with the same bearing, which
makes it faster than the popular iterative closest point (ICP).
Hähnel et al. (2003) presented a SLAM algorithm that com-
bines Rao-blackwellized particle filtering and scan match-
ing. In this approach, scan matching is used for minimizing
odometric errors during mapping and a probabilistic model
of the residual errors from scan matching is then used for
the resampling steps. This way, the number of samples re-
quired is considerably reduced as well as the particle de-
pletion problem typically preventing the robot from closing
large loops was mitigated. Tomono (2004) proposed a new
scan matching method based on geometric hashing, which
utilizes the Euclidean invariant features in order to match an
14
input scan with reference scan without an initial alignment.
Nguyen et al. (2006) proposed the orthogonal SLAM us-
ing geometric constraints between the lines. They reduced
the complexity of the SLAM algorithm by mapping only
the lines that are parallel or perpendicular to each other,
which represent the main structure of most indoor environ-
ments. Despite their relative successes, due to their high
cost, processing requirement and safety issues, these active
sensor-based systems had limited utilities in practical com-
mercial applications.
Recently, a number of researchers have proposed vision
based SLAM solutions using relatively low cost cameras
(Davison 2003; Lowe 2004; Murray and Jennings 1998).
Davison (2003) proposed a vision-based real-time SLAM,
called Mono-SLAM, which uses only a single camera with-
out odometry information. Although it improved localiza-
tion accuracy by incorporating the camera velocity into the
object variables, it still requires an initial manual calibration
process to obtain the scale information. Lowe (2004) intro-
duced a triclops-vision system using their own wide base-
line matching technique called SIFT (Scale Invariant Fea-
ture Transform). It maintains the robot pose and landmark
positions separately. Murray (1998) proposed a stereo vision
based mapping technique which is able to build a grid map
using a trinocular system.
These laser range finder or vision based SLAM tech-
niques have been developed and are used in many real ap-
plications. However, these methods are only useful to more
expensive platforms such as guidance or security robots but
not applicable to the low cost platforms such as vacuum
cleaning robots which have been the most successful com-
mercially. Most low cost robots use only sparse and noisy
range sensors such as sonar or PSD range sensors due to the
cost limit. Therefore, it is necessary to develop a new effi-
cient SLAM algorithm that can work with sparse and noisy
sensors. Along this line, some researchers have recently pro-
posed sonar range sensor based localization (Großmann and
Poli 1999) and SLAM (Burguera et al. 2005; Tardos et al.
2002; Kleeman 2003; Choi and Oh 2006) techniques. Al-
though their performance is in general lower than that of
the laser range finders primarily due to their short range and
high angular uncertainty (up to 30 degrees), it still is signif-
icant in view of their cost and availability. Details of these
systems are reviewed in Sect. 2. Unfortunately, research on
low grade sensor based SLAM has been quite scarce.
This paper has four main contributions. First, we develop
an efficient line extraction algorithm which remarkably re-
duces the search space by combining the Iterative End Point
Fit (IEPF) clustering algorithm and LS line fitting with the
original Hough transform. Second, we demonstrate that it is
feasible to model the general indoor environment as a com-
bination of line features. Third, we enhance the performance
of the EKF framework for our line feature based SLAM by
Auton Robot (2008) 24: 13–27
using geometric constraints between the lines. Thanks to
this, it is now possible to implement a reliable SLAM al-
gorithm for low cost robots equipped with sparse and noisy
range sensors. Finally, we propose an efficient active explo-
ration control scheme for mapping which incrementally es-
timates the locations of the invisible line features from the
line feature based environment model and then moves the
robot around that vicinity.
The rest of this paper is organized as follows. Section 2
reviews the previous work. Section 3 describes a new ef-
ficient line feature extraction scheme. Section 4 then de-
velops our geometrically constrained EKF formula whereas
Sect. 5 describes the line feature based environment mod-
eling and the active exploration control scheme. Section 6
finally shows the experimental results.
2 Previous work based on sparse and noisy range
sensors
In the last decade, several researchers addressed the sparse
and noisy sensor based localization and SLAM problems.
Großmann (1999) proposed a position tracking method for
a mobile robot with seven sonar sensors consisting of three
steps. It first computes a two-dimensional feature space by
applying a straight-line Hough transform to the sonar read-
ings. The detected features are then matched with those in
the world map. The correlation counts obtained in the pre-
vious step are used for updating the position probability
grid. Burguera (2005) proposed a scan matching localiza-
tion strategy which is the extension of the ICP based al-
gorithm to sonar range sensors. In this approach, the sen-
sor readings are grouped to overcome their sparseness and
the whole robot trajectory involved in the grouping process
is corrected in the post process. Tardos (2002) proposed a
perceptual grouping process that permits a robust identifica-
tion and localization of the specific environmental features,
such as straight segments and corners, from the sparse and
noisy sonar data. He also proposed a map joining technique
that allows the system to build a sequence of independent
limited-size stochastic maps and join them in a globally con-
sistent way. Kleeman (2003) proposed a SLAM algorithm
for indoor environments using two advanced sonar range
sensors which can produce accurate range and bearing mea-
surements, classify targets, and reject interferences within
one sensing cycle. He also presented techniques for detect-
ing sonar feature clutter and selecting strong candidates for
sonar landmarks. Drumheller (2006) proposed a feature-
based localization algorithm for mobile robots equipped
with a sonar range finder. He introduced the idea of sonar
segments, which are straight-line segments extracted from
the measurements obtained in a 360-degree sweep of the
sonar range finder. Moreover, he used a sonar barrier test,
Auton Robot (2008) 24: 13–27 15

which eliminates implausible robot positions by exploiting bias incurred in sensor data accumulation can increase the
the physical constraint that sonar beams do not penetrate possibility of slow convergence or even divergence of the
solid objects. However, the algorithm is not able to exploit feature locations. Therefore, we need to eliminate these in-
readings from different positions. Crowley (1989) proposed correct line features in the association step by using a pre-
a system for dynamically maintaining a description of the defined set of geometric constraints between the lines. The
limits to the free space for a mobile robot using a belt of ul- following two sections show how these problems are solved.
tra sonic range devices. These techniques are based on the
principle of explicitly representing the uncertainty of the ve-
hicle position as well as the uncertainty inherent in the sens- 3 Line feature extraction and filtering
ing process. However, he only used three points to extract
a line segment and use only a simplified measurement un- We develop our own efficient algorithm using a multi-level
certainty model which disregards the correlation of the line filtering scheme shown in Fig. 1.
parameters.
As outlined above, there have been a variety of ap- 3.1 PDS building and resampling
proaches for sparse and noisy data based SLAM. However,
these solutions are still not adequate for a low cost vacuum Most SLAM algorithms based on sparse sensors have their
cleaning robot whose goal is to cover the entire floor space
own sensor data grouping method to overcome sparseness.
due to following reasons. In case of using the corner as the
We name our method Pseudo Dense Scan (PDS) reported
feature, its observation error is in general larger than that of
in Choi and Oh (2006) to overcome sparseness and reduce
the line feature because the line feature is extracted using
noise as the same time. PDS is basically an odometry-based
several tens of range readings while the location of a corner
temporal accumulation of sensor readings, each sampled af-
feature is generally calculated using only two range read-
ter a certain amount of pose changes, and this raw PDS is re-
ings. Therefore, the calculated location of corner feature is
sampled using a gravity based resampling scheme to reduce
more intensely contaminated by the sensor noise than that
noise as well as the number of data points (details in Choi
of line feature. Especially, when using low cost noisy range
and Oh 2006). In order to resample the scan points, we first
sensors, this problem manifests itself significantly because
sort the PDS points using Quicksort algorithm, based on θ of
the range reading error is largely affected by the orienta-
each point after being transformed to the polar coordinates.
tion of sensor with respect to the reflecting surface. This
Then we calculate the mean of each point group which has
is not a significant problem in case of the usual start-goal
been clustered based on the distance between points and the
navigation but it can increase the possibility of leaving un-
number of points within a group. Figure 2 shows an example
covered areas due to the trajectory discontinuities in case of
process of PDS building and resampling.
coverage path planning. In case of scan matching using raw
scan data, it can eliminate the step of feature extraction and
thus be applicable to a feature-less environment, but its ac-
curacy should be lower than that of the feature based method
because the scan constructed by accumulating sensor read-
ings observed from different viewpoints is much more noisy
than that of a scan from the laser range finder as shown in
Fig. 2. Therefore, it is difficult to expect the scan matching
accuracy as high as that of using a laser range finder. How-
ever, using line features as in our approach, this noise can
be greatly reduced in the feature extraction process. Due to
these reasons, in this paper, we only use the line features
whose observed locations are less significantly affected by
the robot’s viewpoint than other feature types (e.g. corner).
However, the original Hough transform based line extrac-
tion used in earlier works is very time consuming because it
should vote each point extensively against the entire Hough
space, which is not suitable for a low-end computing envi-
ronment. For a low cost system, we need to develop a much
more efficient line extraction algorithm. Furthermore, when
using the EKF framework for SLAM, an incorrect initial
feature vector estimation due to sensor noise or directional Fig. 1 Overall process of line extraction and filtering
16 Auton Robot (2008) 24: 13–27
Fig. 2 Example of PDS
building and resampling

3.2 IEPF based clustering and 1st filtering

In this step, the points of the resampled PDS are grouped

into several line segments through the IEPF algorithm which
is the most popular and effective line extraction method
based on Split-and-Merge (Nguyen et al. 2005). The initial
grouping is carried out by checking the distance between
the two consecutive points. Starting from j = 0, the dis-
tance between two adjacent points (xj , yj ) and (xj +1 , yj +1 )
are checked. If it is smaller than a preset threshold value D,
then (xj , yj ) and (xj +1 , yj +1 ) are considered to be in the
same cluster and otherwise to a new cluster. This proce-
dure continues until all the points are tested. If the num-
ber of points in a certain cluster is smaller than a preset Fig. 3 Example of an IEPF based clustering
threshold value N or the length of a cluster is shorter than
a preset threshold L, then the cluster is considered negli-
gible and not counted as a cluster. In addition, a cluster

N 
N 
N 
N
made up of multiple line segments is divided into several b̂ = yi xi2 − xi xi yi , (3)
cluster regions by the iterative end point fit (IEPF) method i=1 i=1 i=1 i=1
(Duda and Hart 1973). This algorithm recursively splits a set  2
of points C = {(x1 , y1 ), (x2 , y2 ), . . . , (xN , yN )} into C1 = 
N 
N 
N
ĉ = xi yi − xi2 yi2 . (4)
{(x1 , y1 ), . . . , (xa , ya )} and C2 = {(xa , ya ), . . . , (xN , yN )} if
i=1 i=1 i=1
Tmax is longer than a preset threshold T. The same proce-
dure is repeated for C1 and C2 until Tmax becomes shorter
than T. After splitting, two consecutive regions are merged After finding the approximate parameters â, b̂ and ĉ, clusters
if the maximum distance to the line formed by the ex- with line fitting errors larger than δ are eliminated.
treme points of these regions is smaller than T as shown
in Fig. 3(d). The figure shows an example of the IEPF based
3.4 Constrained Hough transform
clustering of points lying on 3 line segments. As a final step,
clusters with smaller number of points than N or shorter
length than L are pruned. In the previous step, we calculated an initial estimate of
the line parameters. However, it is well-known that the LS
3.3 Least square line fitting and 2nd filtering line fitting is significantly affected by outliers. Therefore,
we used the Hough transform to remove the effect of out-
In this step, we calculate an initial estimate of the line para-
liers as well as to estimate the uncertainties of the line pa-
meters in (1) for each cluster found in the previous subsec-
rameter. Figure 4 shows the geometric relation between the
tion using a least square line fitting scheme as follows:
line equation x cos θ0 + y sin θ0 = ρ0 in Hough space and
ax + by + c = 0, (1) âx + b̂y + ĉ = 0 in the x–y plane. Here, we refine the initial
line parameters âx + b̂y + ĉ = 0 of each cluster using the

N 
N 
N 
N
â = xi yi2 − yi xi yi , (2) Hough transform (HT). To save time, we restrict the Hough
i=1 i=1 i=1 i=1 voting space within the vicinity of the initial values ρ0 , θ0 as
Auton Robot (2008) 24: 13–27 17

shown in Fig. 5, calculated from the initial line parameter â, 3.5 Measurement uncertainty estimation and last filtering
b̂ and ĉ as follows: using the combined uncertainty measure
 
|ĉ| −1 b̂ To properly estimate the location of the features with the
ρ0 =  , θ0 = tan − . (5)
â 2 + b̂2 â EKF framework, we need a measurement uncertainty model
for the constrained HT:
Table 1 shows a comparison of the needed voting space
and the total voting counts between the original Hough z = h(x) + v,
 (6)
transform and our constrained Hough transform when ap- cρρ cρθ
plying each algorithm to the same PDS with 100 points. C(v) = ,
cθρ cθθ
Here, we quantize the Hough space with the resolution of
1 cm in ρ axis and 1◦ in θ axis, while limiting the range of where z is the measurement model and v is the sensor mea-
ρ to 1.8 m in case of the original Hough transform whereas surement noise with zero mean and covariance C(v). In gen-
and constrain the voting space to ±5 cm in ρ axis and ±5◦ eral case, C(v) is modeled as a combination of constant el-
in θ axis in case of the constrained Hough transform. After ements due to the difficulty in estimating it. However, we
voting every point of a given cluster to the Hough space, we developed our own measurement noise model based on the
find (ρMax , θMax ) acquiring maximum voting counts that are constrained Hough space voting result as follow:
used as an element of the line feature vector describing the 
cρρ cρθ
given cluster. C(v) =
cθρ cθθ

1  (ρk − ρMax )
= C(ρk ,θl )
Ctotal (θl − θMax )
k l
 T
(ρk − ρMax )
× (7)
(θl − θMax )

where Ctotal is the total voting count and C(ρk ,θl ) , voting
count in the Hough space whose ρ is ρk and θ is θl . From

Table 1 Comparison of the constrained HT with the original HT

Original HT Constrained HT

Needed voting space 180 × 360 11 × 11

Fig. 4 Geometric relationship between the two line models in two dif-
Total voting counts 180 × 360 × 100 11 × 11 × 100
ferent spaces

Fig. 5 Example of a
constrained Hough space setting
18 Auton Robot (2008) 24: 13–27

Fig. 6 Parameters of a line feature vector

Fig. 7 Measurement value of line feature

cρρ and cθθ in (7), we calculate the combined uncertainty

measure U of each cluster as follows: 4.1 State vector and covariance matrix

U = wl / l + wρ cρρ + wθ cθθ , (8)

In an EKF based framework, the state vector and its covari-
where l is the length of the estimated line as shown in ance matrix are partitioned into the robot pose and the land-
Fig. 6 and wl , wρ and wθ are the empirical weights which mark positions. In our constrained EKF, they are formulated
can be determined statistically (details in Appendix 1). as follows:
Using this combined uncertainty measure U , we select a ⎡ ⎤ ⎡ ⎤
r Prr Prl1 · · PrlN
set of reliable clusters whose U is lower than a thresh- ⎢ l1 ⎥ ⎢ P l1 r
⎢ ⎥ ⎢ P l1 l1 · · P l1 lN ⎥
⎥
old and encode these clusters as a line feature vector, l =
x=⎢ ⎥
⎢ · ⎥, P=⎢
⎢ · · · · · ⎥ ⎥, (9)
{ρ, θ, l, (xs , ys ), (xt , yt )} as depicted in Fig. 6. ⎣ · ⎦ ⎣ · · · · · ⎦
lN P lN r P lN l1 · · P lN lN

4 Geometrically constrained EKF framework and line
feature association
In this paper, we employ a constrained EKF framework
for a SLAM using line features. The constrained EKF im-
poses a certain relationship between the lines of interest.
These relationships between the lines have also been used
in the orthogonal SLAM (Nguyen et al. 2006). The orthog-
onal SLAM is similar to our geometrically constrained EKF
framework in aspect of using parallelism and orthogonality
between lines. However, big differences exist as follows:
1. They assume a constant uncertainty for all the observed
lines. However, we build our own measurement uncer-
tainty model.
2. They separate orthogonalization of the observed lines
and estimation of their positions using EKF. Therefore,
the uncertainty of the matched feature in the map with
the observed feature is not considered in orthogonaliza-
tion. However, we carry out orthogonalization in con-
junction with estimation of the observed line feature lo-
cation through a single measurement update step.
The orthogonal SLAM is an algorithm which dose not well
consider the influence of uncertainties in the estimation
process.
where r = [rx , ry , rθ ]T is the robot pose, lk = [ρk , θk ]T rep-
resents the kth line feature position in Hough space. Note
that although l, (xs , ys ) and (xe , ye ) of the line feature vec-
tor are not used in our EKF formulation, they will be used
to associate line features in Sect. 4.4.
4.2 Measurement model for the line feature
Figure 7 shows the measurement value of line feature
lk = [ρ k , θ k ]T observed by a robot whose pose is r =
[rx , ry , rθ ]T . The measurement model in EKF is the projec-
tion function that maps the line features in the state vector
from the world to the robot centered coordinate, described
by:
  k  
zρ ρr dx 2 + dy 2
= = ,
zθ θrk tan−1 (dy/dx) − rθ
(10)
dx = (−rx cos θ k − ry sin θ k + ρ k ) cos θ k ,
dy = (−rx cos θ k − ry sin θ k + ρ k ) sin θ k ,
where [zρ , zθ ]T denote a projected location of lk in the ro-
bot centered coordinate and tan−1 (dy/dx) returns the angle
from the xw to a point (dx, dy).
Auton Robot (2008) 24: 13–27 19
Fig. 8 Geometric constraints
imposed on the lines

4.3 Measurement model with geometric constraints and the length of overlap (Lov ) between lines as follows:

j
We consider three kinds of geometric constraints between DH i = w̄ρ (ρ j − ρ i )2 + w̄θ (θ j − θ i )2 , w̄ρ + w̄θ = 1,
the lines as depicted in Fig. 8. When the first line is ex-
(13)
tracted then we align the first extracted line l1 parallel with 
x axis, i.e., l1 ’s angle θ 1 is fixed to π/2. In this aligning j j
Lov i = l i + l j − (xe − xsi )2 + (ye − ysi )2
j
process, we assume that the robot always starts SLAM with 
wall following, after finding and approaching it. Therefore, positive if there exists overlap,
= (14)
the first extracted line l1 is believed to be a line from one negative otherwise,
of the walls. Figure 8(a) shows the parallel (l ) and the or-
thogonal (l⊥ ) relationships between the first line l1 and the j
where DH i is the weighted Euclidean distance, w̄ρ and w̄θ
others. The kth line lk is parallel with l1 if |θ 1 − θ k | < εθ and j
are normalized valued of wρ and wθ in (8), and Lov i is the
orthogonal if |θ 1 − θ k − π/2| < εθ , where εθ is an empiri- i j
length of overlap between l and l . Using these two mea-
cally determined threshold. Figure 8(b) shows the collinear sures, we can associate an observed line feature lobv with

relationship between two lines, lw and lw , extracted from line feature in the state vector as follows:
the same wall but separated by the furniture contacting that

wall, which satisfies the conditions |θ w − θ w | < εθ and lmat = arg min DH kobv

|ρ − ρ | < ερ . These constraints are formulated as fol-
w w k

lows: such that DH kobv < εDH and Lov kobv > 0, (15)

zang = [θ 1 − θ k + α], l col

= arg min DH kobv
k

0 if |θ 1 − θ k | < εθ , such that DH kobv < εDH and Lov kobv < 0 (16)
α= (11)
−π/2 if |θ 1 − θ k − π/2| < εθ ,
 w where lmat is the matched line with lobv . When two line fea-

ρ − ρw tures are associated, we first estimate the new line parame-
zcol = , (12)
θ1 − θk ters of lmat through a measurement update using (10). We
then estimate the new end-points by projecting the outer-
where zang is a measurement model for describing the par- most point of the observed cluster to the updated line fea-
allel and orthogonal relationships between lines l1 and lk ture as shown Fig. 6 if Lov > 0 and Lov < l of lobv . Else, we
and zcol is for describing the collinearity between lines lw estimate the new end-point by projecting the previous end-

and lw . These two measurements zang and zcol become zero point instead to the updated line feature. After updating the
when corresponding constraints are satisfied. end-point, we update the length l of lmat by calculating the
distance between the start-point and the new end-point. lcol
4.4 Line feature association is collinear with lobv without overlap as shown in Fig. 8(b)
and we use the measurement model (12) and insert lobv to
Data association has been known to be a crucial issue for the state vector. To this end, the line features which do not
a successful SLAM. We associate line features through a satisfy conditions (15, 16) but satisfy the condition in (11)
weighted Euclidean distance measure in Hough space (DH ) are inserted to the state vector as a new feature. We then
20
apply measurement model (11) according to the geometri-
cal relationship with l1 , but the others are not mapped to the
feature space for localization.
4.5 Line merging
In the loop closing phase shown in Fig. 9, the last extracted
line feature lN which is collinear with l1 but has no over-
lap is enlarged as the robot approaches to the starting posi-
tion. When lN is enlarged enough to satisfy condition (15)
with l1 , these two lines must be merged to a single line.
This merging process is completed by updating the el-
ements of l1 and deleting lN , its related variance elements
which belong to a gray part in (9) from the state vec-
tor and covariance matrix. Figure 10 shows the merging
process of line l1 and lN . First, line parameter (ρ 1 , θ 1 )
of l1 is updated using the measurement model (10) then
Fig. 9 The loop closing phase
Fig. 10 The line merging
process
Auton Robot (2008) 24: 13–27
the end-point (xt1 , yt1 ) of l1 and the start-point (xsN , ysN )

of lN are projected onto the updated line, l1 . These pro-
   
jected points, (xt1 , yt1 ) and (xs1 , ys1 ), are used as the end-

point and the start-point of l1 , respectively, and distance
     
l 1 between (xs1 , ys1 ) and (xt1 , yt1 ) is used as length of l1 .

Consequently, the elements of the merged line l1 become
      
{ρ 1 , θ 1 , l 1 , (xs1 , ys1 ), (xt1 , yt1 )}.
5 Line feature based environment modeling and active
exploration
Because we use wall following to explore a given environ-
ment, there may be invisible lines which can not be ob-
served during wall following as shown in Fig. 11. In this
section, we develop a line feature based environment mod-
eling scheme to first reconstruct the environment with pre-
defined objects consisting of connected line segments and
then judge whether there is any invisible line. If there is, we
move the robot to the vicinity of the estimated position of
the invisible line and explore its circumference by contour-
following.
5.1 Line feature based environment modeling
In a general indoor environment, most long line segments
come from walls and furniture in a room. From this assump-
tion, we define three kinds of objects consisting of line seg-
ments to model an indoor environment.
5.1.1 Definition of a room
A room R is an object consisting of the 4 walls where each
wall consists of lines. We define a room object in Fig. 11 as
follows:
R = {W b , W r , W t , W l } = {{l1 }, {l3 }, {l5 }, {l7 , l11 }},
Auton Robot (2008) 24: 13–27
Fig. 11 Wall-following and initial environment modeling
where W b , W r , W t and W l are the bottom, right, top and
left walls, respectively.
5.1.2 Definition of a wall-contact object
A wall-contact object, Ow is a furniture object represented
by 2 vertical and 1 horizontal lines because one side of the
furniture in contact with the wall can not be observed by the
robot. The object O1w in Fig. 11 is thus represented by:
O1w = {l8 , l9 , l10 },
where l8 , l9 and l10 are the first vertical line, a horizontal line
and the second vertical line, respectively.
5.1.3 Definition of a free object
A free object, Of is a furniture object which is located apart
from all the walls and thus consists of 4 lines because its
all sides can be observed. The two free objects, O1f , O2f in
Fig. 11 is:
O1f = {l2 , l14 , l13 , l12 },
O2f = {l15 , l4 , l6 , l16 }.
5.2 Active exploration to fill in the invisible lines
The invisible lines in the wall-following process should be-
long to free objects as shown Fig. 11. Therefore, we can
21
Fig. 12 Active exploration and environment modeling
check the existence of invisible lines by checking the num-
ber of lines belonging to each free object. If there is any
free object which is not complete, i.e., short of 4 lines,
then the robot needs to explore around that free object. To
this end, we need to decide on where to start contour fol-
lowing among the set of lines belonging to the incomplete
free object. We choose the starting location at the vicin-
ity of the center of the nearest line from the robot’s cur-
rent position as depicted in Fig. 12. Here, we need to com-
plete two incomplete free objects after the wall-following
process. To do this, we find the nearest free object, O1f ,
from the set of incomplete free objects, {O1f , O2f }, then
find the nearest line l2 of {l2 , l12 } belonging to O1f and
then move to P1 which is at the vicinity of the center
of l2 . Here, the distance between P1 and l2 depends on
the marginal distance for contour following control. After
arriving at P1 , the robot follows the contour of O1f using
the right-hand rule and find the invisible lines l13 , l14 in
sequence and complete O1f . Then, we repeat the same to
complete O2f .
6 Experimental results
We have conducted simulated and real experiments to ver-
ify the performance of our approach. Figure 13 shows the
sensor configuration of the robot used as an experimental
testbed. It is a differential-drive robot equipped with seven
IR sensors with the range from 20 cm to 150 cm. However,
22 Auton Robot (2008) 24: 13–27

we had to limit the detection range to 120 cm due to its large 6.1 Simulation results
sensor noise.
The proposed algorithm has been tested on a simulated envi-
ronment coded with Visual C++ on Pentium 4 3.0 GHz PC
with a 1 GB RAM. Three types of simulation were carried
Fig. 13 Sensor configuration of out.
our robot platform
6.1.1 Modeling of simple rectangular objects
The environment pictured in Fig. 14 is a 4 m × 5.5 m room
containing one wall-contact object and two free objects. In

Fig. 14 Simulation result in the

indoor environment with simple
rectangular objects. The red
ellipse in (g) represents a
Gaussian distribution of
uncertainty of each line feature
and its horizontal and vertical
axes stand for cρρ and cθθ ,
respectively
Auton Robot (2008) 24: 13–27
Fig. 15 Localization errors
during the environment
modeling process
Fig. 14(a), the robot explores a room by wall following and
builds an initial incomplete model in Fig. 14(d) which con-
sists of one wall-contact object and two incomplete free ob-
jects. After building this initial model, P1 , the starting po-
sition of a contour following trajectory around the nearest
incomplete free object, O1f , is estimated. After the robot
reaches P1 , it follows the contour of O1f and completes O1f
by filling in the two remaining lines as shown in Fig. 14(b),
(e). After completing O1f , the same process is applied to O2f
as shown in Fig. 14(c), (f). Figure 14(g) shows the accuracy
of the map result. Figure 15 shows the localization errors
during the entire mapping process depicted in Fig. 14 and
demonstrates that each time a correction step is performed,
the absolute error in position or orientation is drastically re-
duced during a 340-second simulation.
6.1.2 Modeling of compound rectangular objects
The environment shown in Fig. 16 is a 4 m × 4 m room
containing two wall-contact objects and two free objects. In
23
Fig. 16(a), the robot explores a room by wall following and
builds an initial incomplete model in Fig. 16(d) which con-
sists of four wall-contact objects and two incomplete free
objects. Here, the top of O1w and the bottom of O2w is cal-
culated as the mean of y values of the start-point of l2 and
the end-point of l3 , and the top of O4w becomes the bottom
of O3w rather than the top of the room model. After build-
ing this initial model, P1 , the starting position of a contour
following trajectory around the nearest incomplete free ob-
ject, O2f , is estimated. After the robot reaches P1 , it follows
the contour of O2f and tries to complete O2f by filling in re-
maining lines as shown Fig. 16(b), (e). In this filling step,
two lines, l15 and l16 , which are candidates of the right side
of O2f are extracted and the left most line, l15 , is used as the
right side of O2f . Therefore, O2f is modeled using the ini-
tially extracted three lines, l11 , l13 and l14 , and the additional
line, l15 . After completing O2f , O3f is built using the remain-
ing line, l16 and the lines of O2f as shown in Fig. 16(e). Af-
ter completing O2f and O3f , the same process is applied to
24
Fig. 16 Simulation result in the
indoor environment with
compound rectangular objects
O1f . The completing result of O1f and O4f is different from
that of O2f and O3f in the separating process due to the rea-
son that the initially extracted lines are used as elements of
O2f with higher priority than the additionally extracted lines.
This simulation result shows that the set of contacting fur-
nitures which can not be modeled with a single rectangular
model is able to be modeled using the compound rectangular
model.
6.1.3 Comparison of the convergence performance
In this simulation, we compare the EKF convergence per-
formance with and without the geometric constraint. Fig-
ure 17(a) shows a situation where the robot meets the verti-
cal wall while following the horizontal wall. In this phase,
the robot gathers the sensor readings and builds a PDS while
conducting the turning motion. Figure 17(b) shows that a
PDS built in turning motion is biased (i.e. not orthogonal
to the horizontal wall) due to the beam with (5 degrees in
our simulation) and wheel slippage. Figure 17(c) shows that
this bias can be overcome efficiently by inserting a geomet-
ric constraint in the measurement update step. This bias can
affect the entire mapping result and make the covariance ma-
trix of the line feature converge more slowly when not using
the geometric constraint.
6.2 Real-world results
To verify our algorithm in a real environment, we carried
out two kinds of experiments. In the first one, the robot au-
Auton Robot (2008) 24: 13–27
tonomously repeated the same trajectory five times by wall-
following to evaluate the accuracy of localization as shown
in Fig. 18. Although the ground truth is not shown, the result
shows that the error of the corrected pose is bounded while
the odometry error gradually increases without bound. In the
second experiment, we tested the performance of our line
feature based environment modeling under two experimen-
tal setups shown in Fig. 19(a) and Fig. 20(a) in a 4 m × 4 m
room. The setup 1 in Fig. 19(a) consists of 4 free objects. Af-
ter wall-following, 4 incomplete free objects show up in the
initial model. Note that the robot does not follow the con-
tour of the fourth free object because some invisible lines of
this object are already observed when the robot follows
the contour of the first and the third free objects. The robot
trajectory is plotted with a solid blue line using the corrected
pose. The setup 2 in Fig. 20(a) consists of two wall-contact
objects and two free objects. In the initial model, two wall-
contact objects and two incomplete free objects are shown
in Fig. 20(b). Later on, these two incomplete objects are
properly completed through active exploration as shown in
Fig. 20(c).
7 Conclusion
We have presented a novel line feature based SLAM based
on active exploration that are very efficient for the low cost
service robots (such as cleaning robot) with sparse and noisy
range sensors. The methodology is based on a geometri-
Auton Robot (2008) 24: 13–27
Fig. 17 Comparison of the
convergence performance with
and without the geometric
constraint
Fig. 18 Trajectory with and without correction
cally constrained EKF framework which imposes geomet-
ric constraints between the lines in the measurement model.
To our knowledge, this is the first effort to use geometric
constraints between the lines for a line feature based SLAM
using only sparse and noisy range sensors. By imposing
geometric constraints, we can estimate the 2D line feature
positions more accurately as well as allow their covariance
25
matrices to converge more rapidly than without these con-
straints. In addition, we developed an efficient line feature
extraction algorithm based on a constrained Hough trans-
form whose voting space is restricted within the vicinity of
the initial line parameters which were estimated by applying
the least square line fitting technique to each cluster obtained
by an IEPF clustering algorithm. Moreover, we developed a
line feature based modeling scheme which models the en-
vironment with three kinds of objects, Room, Wall-contact
and Free objects and also presented an active exploration
control scheme which is able to observe invisible lines au-
tonomously without human intervention. Future work will
focus on how to efficiently cover the target environment by
decomposing it into a set of regions based on our line feature
based environment model.
Appendix 1: The choice of weighting parameters in (8)
We can determine these weighting parameters statistically
using an accumulated histogram of each uncertainty. To do
this, we gather a set of 100 scans from the test environment
and apply our line extraction algorithm to each scan and
store l, cρρ and cθθ of the extracted lines whose length l
is longer than 70% of L, which is a preset minimum length
26
Fig. 19 Modeling process of the first test configuration
Fig. 20 Modeling process of the second test configuration
eligible for mapping. Using the stored data, we build an ac-
cumulated uncertainty histogram of cρρ using N stored data
as shown in Fig. 21. In this accumulated histogram, we first
d , where the accumulated number of lines is 50% of
find cρρ
Auton Robot (2008) 24: 13–27
Fig. 21 wρ determination from the accumulated histogram of cρρ
N and then calculate wρ using (17) and (18):
Uρρ = (cρρ − cρρ
d
+ Max(cρρ
min max
, cρρ ))
min max
/Max(cρρ , cρρ )
= cρρ /Max(cρρ
min max
, cρρ ) + const
∼ min
= cρρ /Max(cρρ max
, cρρ ). (17)
wρ = 1/Max(cρρ
min max
, cρρ ), (18)
where Uρρ is a weighted uncertainty of cρρ . The same pro-
cedure is repeated to calculate wl and wθ as well. These
parameters are closely related to the stability of the distance
measure of the range sensor. Therefore, it is not always ro-
bust. If the range sensor model is changed, new weighting
parameters must be found.
Appendix 2: Possible failure models of our algorithm
Our algorithm’s main goal is to extract the main structure of
a given rectangular environment whose utility in localization
is higher than other features. Therefore, we decided to use
only line features which are long enough because this long
line is likely to be extracted from the static objects (e.g. fur-
niture or wall) and therefore hardly changed while carrying
out the cleaning task. We certainly can have chairs and ta-
bles, but simply do not consider these easily movable objects
in feature mapping. However, these easily movable objects
still can affect the result of our algorithm. The following re-
sults present some possible failure models of our algorithm.
Figure 22(a) shows that our algorithm can fail to build a
Room model when only small line segments of a wall can be
observable due to a wall-contact object. Here, the left side of
Room is incorrectly modeled using the lines extracted from
the two wall-contact objects because we assume that the left
most line is the left side of Room. Figure 22(b) shows that
Auton Robot (2008) 24: 13–27
Fig. 22 Possible failure models of our algorithm
a wall-contact object can not be correctly built due to the
chair in front of it. In this case, we may complete this object
by estimating the right side of the object using the right most
points of the two horizontal lines, as a possible solution.
References
Burguera, A. et al. (2005). Robust scan matching localization using
sonar range finders. In Proceedings of IROS ’05 (pp. 1367–1372),
Alberta, Canada.
Choi, Y., & Oh, S. (2006). Grid-based visual SLAM in complex envi-
ronment. In Proceedings of IROS ’06 (pp. 2563–2569), Beijing,
China.
Crowley, J. L. (1989). World modeling and position estimation for a
mobile robot using ultrasonic ranging. In Proceedings of ICRA
’89 (pp. 674–680), Scottsdale, AZ, USA
Davison, A. J. (2003). Real-time simultaneous localization and map-
ping with a single camera. In Proceedings of ICCV ’03 (pp. 1403–
1410), Nice, France.
Diosi, A., & Kleeman, L. (2005). Laser scan matching in polar coor-
dinates with application to SLAM. In Proceedings of IROS ’05
(pp. 1439–1444), Canada.
Duda, R. O., & Hart, P. E. (1973). Pattern classification and scene
analysis. New York: Wiley.
Frese, U. (2006). A discussion of simultaneous and mapping. Au-
tonomous Robots, 20(0), 25–42.
Großmann, A., & Poli, R. (1999). Robust mobile robot localisation
from sparse and noisy proximity readings. In Proceedings of
IJCAI ’99 workshop on reasoning with uncertainty in robot navi-
gation (RUR-99).
Hähne, D., Burgard, W., Fox, D., & Thrun, S. (2003). An efficient
fastslam algorithm for generating maps of large-scale cyclic en-
27
vironments from raw laser range measurements. In Proceedings
of IROS ’03 (pp. 206–211), Las Vegas, Nevada.
Kleeman, L. (2003). Advanced sonar and odometry error modeling
for simultaneous localisation and map building. In Proceedings
of IROS ’03 (pp. 699–704), Las Vegas, Nevada.
Lowe, D. G. (2004). Distinctive image features from scale-invariant
keypoints. International Journal of Computer Vision, 60(2), 91–
110.
Murray, D., & Jennings, C. (1998). Stereo vision based mapping
and navigation for mobile robots. In Proceedings of ICRA ’98,
(pp. 1694–1699), Leuven, Belgium.
Nguyen, V., Martinelli, A., Tomatis, N., & Siegwart, R. (2005). A com-
parison of line extraction algorithms using 2d laser rangefinder for
indoor mobile robotics. In Proceedings of IROS ’05 (pp. 1768–
1773), Edmonton, Canada.
Nguyen, V., Harati, A., Martinelli, A., & Seigwart, R. (2006). Orthog-
onal SLAM: a step toward lightweight indoor autonomous nav-
igation. In Proceedings of IROS ’06 (pp. 5007–5012), Beijing,
China.
Tardos, J. D., Neira, J., Newman, P. M., & Leonard, J. J. (2002). Ro-
bust mapping and localization in indoor environments using sonar
data. The International Journal of Robotics Research, 21(4), 311–
330.
Tomono, M. (2004). A scan matching method using euclidean invariant
signature for global localization and map building. In Proceedings
of ICRA ’04, (pp. 866–871), LA, USA.
Young-Ho Choi received the B.S. degree in
Hanyang University, Seoul, Korea in 2003. He
is currently working toward a Ph.D. degree in
the Department of Electrical Engineering, Po-
hang University of Science and Technology
(POSTECH), Pohang, Korea. His research in-
terests include SLAM and autonomous naviga-
tion of mobile robot.
Tae-Kyeong Lee received the B.S. degree in
Pohang University of Science and Technol-
ogy (POSTECH), Pohang, Korea in 2006. He
is currently working toward a Ph.D. degree in
the Department of Electrical Engineering, Po-
hang University of Science and Technology
(POSTECH), Korea. His research interests in-
clude complete coverage path planning and au-
tonomous navigation of mobile robot.
Se-Young Oh received B.S. degree in Electron-
ics from Seoul National University, Seoul, Ko-
rea in 1974, and the M.S. and Ph.D. Degrees in
Electrical Engineering from Case Western Re-
serve University, Cleveland, Ohio, USA in 1978
and 1981 respectively. He is currently a profes-
sor at POSTECH, Korea. His research interests
include soft computing technology (neural net-
works, fuzzy logic, and evolutionary computa-
tion) and its application to robotics, face detec-
tion and recognition, and intelligent vehicles. He is a senior member of
IEEE.