Natural corners-based SLAM with partial compatibility algorithm

Original Article

Proc IMechE Part I:J Systems and Control Engineering2014, Vol. 228(8) 591–611� IMechE 2014Reprints and permissions:sagepub.co.uk/journalsPermissions.navDOI: 10.1177/0959651814533533pii.sagepub.com

Natural corners-based SLAM withpartial compatibility algorithm

Rui-Jun Yan1, Jing Wu1, Quan Yuan2, Chao Yuan1, Lu-Ping Luo2,Kyoo-Sik Shin3, Ji-Yeong Lee3 and Chang-Soo Han3

AbstractThis article presents natural corner-based simultaneous localization and mapping (SLAM) using a new data associationalgorithm that achieves partial compatibility in a real unknown environment. In the proposed corners’ extraction algo-rithm, both the end points of an extracted line segment far away from the other segments and the intersection point ofthe two closer line segments are considered as corners. In data association, a partial compatibility algorithm obtaining arobust matching result with low computational complexity is proposed. This method divides all the extracted corners atevery step into several groups. In each group, the local best matching vector between the extracted corners and thestored ones is found by joint compatibility, while the nearest feature for every new extracted corner is checked by indi-vidual compatibility. All these groups with the local best matching vector and the nearest feature candidate of each newextracted corner are combined, and its joint compatibility is checked with the linear matching time. The experimentalresults in an indoor environment with natural corners show the robust matching result and low computational complex-ity of the partial compatibility algorithm in comparison with individual compatibility nearest neighbor and joint compat-ibility branch and bound.

KeywordsFeatures’ extraction, extended Kalman filter, simultaneous localization and mapping, unknown environment, data associa-tion, partial compatibility

Date received: 4 October 2013; accepted: 7 April 2014

Introduction

Simultaneous localization and mapping (SLAM) is theprocess of estimating the pose of a mobile robot andbuilding a map of an unknown environment based onthe extracted features of the environment. Due to thelow accuracy of using only odometry, the features areused as landmarks to correct the position of a mobilerobot by integrating the incremental information overtime to measure the distance or the angle transformed.To relate new observations with features stored in themap correctly, data association is posed as a searchproblem in the space of the observation–feature corre-spondences. The geometric shape of the features in theenvironment has an important effect on robot localiza-tion, navigation, perception and mapping. Most experi-ments, with laser sensor or vision sensor, of robotlocalization and mapping environments estimate theposition of a mobile robot using man-made landmarks,like a cup in Choi and Lee,1 a green circle in Mataet al.2 or other artificial landmarks as in Li and Jiang,3

Lee and Song,4 Meyer-Delius et al.5 and Ronzoniet al.6

Unfortunately, artificial landmarks can be appliedonly to a very narrow range with respect to the naturalfeatures in a real unknown environment. This is becausethe man-made ones cannot be put into the desired areathat people may not enter. Landmark features shouldbe carefully considered because the uncertainties of thelandmark position can affect the case of updating theposition of a mobile robot as well as other stored

1Department of Mechatronics Engineering, Hanyang University, Ansan,

South Korea2Department of Mechanical Engineering, Hanyang University, Ansan,

South Korea3Department of Robot Engineering, Hanyang University, Ansan, South

Korea

Corresponding author:

Chang-Soo Han, Department of Robot Engineering, Hanyang University,

Ansan 426-791, South Korea.

Email: [email protected]

at SungKyunKwan University on September 5, 2014pii.sagepub.comDownloaded from

http://pii.sagepub.com/

landmarks. For this reason, the corners in an unknownenvironments have been chosen as landmarks by usingvision sensors as in Rosten and Drummond,7 Shen andWang8 and Hwang and Song,9 while others have usedsonar or laser sensors10,11 but without the exact calcula-tion of the distance uncertainty and angle uncertaintyof the corner landmarks.

The basis of the corner extraction is line segments’extraction from the raw sensor data. Line features havebeen used successfully in many experiments,12,13 andthere have also been many methods of line extraction,including the split-and-merge,10,14 incremental prob-abilistic technique15 and Hough transform.16 Someexperiments have been done by the evaluating differentline extraction methods as in Nguyen et al.17 whichshowed that the split-and-merge and incrementalmethod demonstrate the best performance. The princi-ple of realization of line segments’ extraction in thisarticle is based on a substantially modified split-and-merge where the uncertainty of the extracted corner ispropagated from the uncertainty of the lines calculatedas in Arras and Siegwart.18

This article attempts to solve both the features’extraction problem and the data association problem.The corners are extracted by considering both the inter-section points of the possible line segments and the endpoints of some segments that are far away from theother line segments. Many researchers have extractedthe intersection point of the two adjacent line segmentsas corners. However, this method cannot obtain suffi-cient corners because of the high uncertainty of the linesegments extracted from the rugged position. The anglebetween the two line segments and the distance fromthe intersection point to the end points of the line seg-ment are compared with some limit value for extractingthe appropriate corner. The real corners are easilyignored if the conditions of the two continuous seg-ments are not satisfied. Even though the corner can befound by the previous method in some cases, the muchcloser corner to the real corner can be extracted by con-sidering all the possible line segments that can extract acorner. After extracting the corners from a new scan,these corners should be matched with the stored mapfeatures in the previous step. However, most researchdevelops a data association method with the assump-tion that the two measurements cannot come from thesame feature.19,20

For stochastic mapping, the two most commonmethods are the individual compatibility nearest neigh-bor (ICNN) algorithm with low computation complex-ity in Castellanos et al.21 and Feder et al.22 and thejoint compatibility branch and bound (JCBB) algo-rithm increasing the robustness of the data associationbut with much more computation time in Neira andTardos.19 The randomized joint compatibility algo-rithm detecting the overlap between the two local mapssuccessfully was developed based on JCBB in Pazet al.23 and was applied to a large-scale SLAM in Pazet al.24 When clutter is high and the features are sparse,

a multi-dimensional assignment (MDA)-based dataassociation algorithm for multi-target tracking isextended using the sensor location uncertainty with thejoint likelihood of the measurements over multipleframes as the objective function.25 Different from theabove algorithms based on the squared Mahalanobisdistance (SMD), the alternative matching likelihoodwas maximized in Blanco et al.26

The data association algorithm in this article alsorelies on SMD but considers the partial compatibilityof all the detected features. To reduce the computationtime and achieve better matching results, the detectedfeatures at every step in the proposed method are sepa-rated into different small groups with the local jointcompatibility matching vector and the nearest unitmatching pair for everyone in that group. Then the twotypes of matching pairs in all the small groups are com-bined by using the branch and bound algorithm. Inaddition, the computation time of the standardextended Kalman filter (EKF) SLAM in Thrun et al.27

is high. This result is due to the great costs of updatingthe covariance matrix for prediction and correction.Although the prediction part of the algorithm is opti-mized to reduce the computation complexity inGuivant and Nebot,28 the computation complexity inthe correction part is also very high. In this article, thecomputation time is decreased in both the predictionand correction, compared with the standard SLAMalgorithm based on the principle of the block matrix.

There are five sections in this article. The line extrac-tion algorithm with its covariance matrix and cornerextraction algorithm with the propagation of uncertain-ties is presented in section ‘‘Line segments’ extractionand corners’ extraction.’’ The partial compatibilityalgorithm (PCA) is explained in section ‘‘Data associa-tion algorithms’’ and the simplification of the EKF-SLAM algorithm is introduced in section ‘‘ImprovedEKF-SLAM.’’ The experimental results of the extractedline segments, extracted natural corners and SLAMwith proposed data association algorithm are shown insection ‘‘Experiment results.’’ Finally, this article is con-cluded in the final section.

Line segments’ extraction and corners’extraction

Line segments’ extraction

In this section, the line segments’ segmentation algo-rithm and the line segments’ merging algorithm areexplained in detail for separating the raw sensor datainto appropriate groups and extracting them as the cor-responding line segments. Moreover, the covariancematrix of every line segment is derived step by stepbased on the uncertainty of the sensor data in thatgroup.

Line segments’ extraction algorithm. The first step in theprocess of line segments’ extraction is splitting the

592 Proc IMechE Part I: J Systems and Control Engineering 228(8)



sensor points into separate data sets to reduce the seg-mentation time in the following steps. The distance dpbetween the adjacent points pi and pi+1 is calculatedand the data set is divided into two sets when this dis-tance is bigger than the limit value. For each separateddata set, a line segment is extracted based on the datapoints in this set. The extracted new line segment isexpressed by Lnew(., pi), in which pi is the last sensorpoint in the new data set. For a better segmentationresult, the limit value dmax dynamically changes withthe measured distance of the raw sensor data. Thepseudo code of the detailed process is summarized inAlgorithm 1.

The second step of Algorithm 1 is to check whetherthe sensor points in the previous segmented line set

belong to the same line segment or not by checking theangle between the two short line segments l1(pi, pi+1)and l2(pi+2, pi+3) as shown in Figure 1. If the angle isbigger than the limit value, this data set is separatedinto two sets, and the previous line segment is replacedby two new segments. In the final step of the line seg-mentation, the distance l from a sensor point of thedata set to the extracted line segment is computed. If l

is bigger than the maximum distance lmax, this singleline segment is divided into two segments at this sensorpoint, as expressed in Figure 2.

The last step of Algorithm 1 merges the line seg-ments that could be represented as a single line segment.Two parameters are used for this, the angle of the adja-cent line segments Ln and Ln+1 and the distance from

Algorithm 1: Line segments extraction algorithm

Input: Raw sensor data vector Vpoint

Output: Extracted line segments vector Vline

1: for raw sensor data point pi from point set Vpoint do

2: find the distance dp of point pi and pi+ 1;3: if dp . dmax then

4: Vline.add (Lnew(., pi));5: end if6: end for7: for line segment Lk in Vline do

8: for pj in the point set of line segment Lk do

9: FlagOne = false;10: calculate angle da of line (pj, pj+ 1) and (pj+ 2, pj+ 3);11: if da . amax then

12: Vline.insert(Lnew(., pj+ 1));13: FlagOne = true;14: end if15: end for16: if (FlagOne) then17: Vline.delete(Lk)18: end if19: end for20: for Lm in Vline do

21: for pn in the point set of line segment Lm do

22: FlagTwo = false;23: find the distance l from point pn to segment Lm;24: if l . lmax then

25: Vline.insert(Lnew(., pn));26: FlagTwo = true;27: end if,28: end for29: if (FlagTwo) then30: Vline.delete(Lm);31: end if32: end for33: for Ln in Vline do

34: calculating the distance dmerge and amerge of Ln and Ln+ 1;35: if (dmerge \ dmergMax and amerge \ amergeMax) then36: Lnew LineMerging (Ln, Ln+ 1);37: Vline.swap([Ln, Ln+ 1], Lnew);38: end if39: end for

Yan et al. 593



the second end point of Ln to the first end point ofLn+1 as shown in Figure 3. If these two values aresmaller than the corresponding limit value, the sensorpoints in these two sets are merged to a single new linesegment. The previous two line segments are replaced

by this new line segment and an example is shown inFigure 4, in which the data points belong to the sameline but are extracted as two line segments.

Uncertainty of the extracted line segments. Since the sensordata have uncertainties, the relative parameters of theline segments also have uncertainties. This sectionshows the derivation of the covariance matrix of theline segment from that of the raw sensor data. The ithraw sensor data pi is represented with two parameters,measuring distance ri and direction angle ui. The uncer-tainty of a raw sensor point is assumed to be Gaussiandistributed, which is expressed as follows

Pi;N ri,s2ri

� �,Qi;N ui,s2

ui

� �ð1Þ

where the uncertainty of the sensing distance Pi and theuncertainty of the rotation angle Qi are assumed to beindependent as in Arras and Siegwart.18 Each extractedline segment is represented with two parameters, inter-cept r and angle a, as in Figure 5. To extract the linesegment, the square of all the errors together with theweighted value wi = (1/su)

2+ (1/sr)2 for the measure-

ment points is summed as

S=Xi

wid2i =

Xi

wi ri cos ui � að Þ � rð Þ2 ð2Þ

The two parameters of line segment can be obtainedby calculating the derivatives of equation (2) withrespect to the variables a and r, expressed as follows

a=1

2atanP

wir2i sin 2ui � 1P

wi

PPwiwjrirj sin ui + uj

� �P

wir2i cos 2ui � 1P

wi

PPwiwjrirj cos ui + uj

� �24

35

=1

2atan

A

B

� �ð3Þ

r=

Pwiri cos ui � að ÞP

wið4Þ

After the derivation of the parameters of the linesegment, the first-order Taylor expansion is used to cal-culate the covariance matrix Cl of the extracted linesegment as follows

Cl =MrCrMTr +MuCuM

Tu ð5Þ

+1ip

ip2ip 3ip

Raw sensor data Extracted line segment

Figure 1. Angle between the four adjacent data points (pi,pi + 1, pi + 2 and pi + 3).

Desired line segmentsExtracted line segment

Raw sensor data

Figure 2. The distance l from a raw data point to theextracted line segment is calculated for all the data points inseparated group, and the single line segment extracted from thisgroup is divided into two at a point whose l is bigger than thelimit value lmax.

merge

merged

Raw sensor data Extracted line segment

Figure 3. Intersection angle and the distance of the twoadjacent line segments.

Raw sensor data Extracted line segment Desired line segment

Figure 4. Two line segments (solid line) belonging to the same line segment but extracted as two in the segmentation step can bemerged as one (dotted line) in merging step.




The first part of equation (5) is about the uncertaintyof the measuring distance of the laser sensor, which iscalculated by transforming the matrix Cr = diag((sr)

2) into the line space using the 2 3N Jacobianmatrix Mr. Here, N is the number of raw sensor data inthe data set of the extracted line segment. The secondentry in equation (5) is the uncertainty of the angle,which is expressed by Cu=diag ((su)

2) with a similar 23N Jacobian matrix Mu. The detailed expression ofequation (5) is

Cl =PNj=1

∂r∂rj

Crj

∂r∂rj

+PNj=1

∂r∂uj

Cuj∂r∂uj

PNj=1

∂r∂rj

Crj

∂a∂rj

+PNj=1

∂r∂uj

Cuj∂a∂uj

PNj=1

∂a∂rj

Crj

∂r∂rj

+PNj=1

∂a∂uj

Cuj∂r∂uj

PNj=1

∂a∂rj

Crj

∂a∂rj

+PNj=1

∂a∂uj

Cuj∂a∂uj

26664

37775ð6Þ

The entries in equation (6) are expressed by the follow-ing equations

∂r

∂ri

=1

Ncos ui � að Þ, ∂r

∂ui= � 1

Nri sin ui � að Þ ð7Þ

∂a

∂ri

=1

A2+B2ri sin 2ui�

1

N

XNj=1

rj sin ui+uj

� �" #B

(

� ri cos 2ui �1

N

XNj=1

rj cos ui+uj

� �" #A

)

∂a

∂ui=

1

A2+B2r2i cos 2ui�

1

N

XNj=1

rirj cos ui+uj

� �" #B

(

+ r2i sin 2ui�

1

N

XNj=1

rirj sin ui+uj

� �" #A

)ð8Þ

The two symbols A and B in equation (8) are shown inequation (3).

Natural corners’ extraction

Natural corner extraction algorithm. Two types of naturalcorners are extracted based on the extracted line seg-ments above. For extracting all possible corners, thedistance and the angle between one line segment andthe other feasible line segment candidates are computedand compared with two conditional values. For thefirst type of corner, the intersection point of the two eli-gible line segments is found and stored in the cornervector. Nevertheless, the corners cannot be effectivelyextracted by only checking the two adjacent line seg-ments because of the existing error of the raw datapoints. An example of the extracted corner by onlyconsidering the two adjacent line segments is shown inFigure 6(a). In the left figure, no corner is extracteddue to some inaccurate line segments located in theneighboring position of the real corner.

In order to extract the corner as in the bottominstance of Figure 6, the intersection points of all thepossibly intersecting line segments are considered ascorners. By using this method, the corner in the left sideof Figure 6(a) is extracted successfully, which is shownin the left side of Figure 6(b). Even though the goalcorner is found by only considering two adjacent linesegments in the right side of Figure 6(a), a much closercorner to the real corner can be found in the right sideof Figure 6(b). The reason for this is that the two linesegments of this new closer corner represent the realenvironment better than the two previous segments.

Not only the intersection points but also the endpoints of some line segments are stored as new corners.When the two adjacent line segments are far away fromeach other, the second end point of the first line andthe first end point of the second one are chosen as cor-ners since these two lines may be from two differentobjects. A detailed realization of the corner extractionis presented in Algorithm 2.

O

r

id

( , )i i

x

y Raw sensor data

Extracted line segment

Figure 5. Each extracted line segment is represented with twoparameters, intercept r and angle a.

(a)

(b)

Real cornerExtracted cornerExtracted line segment

Real line in the environment

Figure 6. Comparison of the extracted natural corners usingtwo methods: (a) only the intersection point of two adjacentline segments is considered as corner in the two examples and(b) much closer corners to the real corner in the examples areextracted because the intersection points of all the possiblyintersecting line segments are considered as corners (two).

Yan et al. 595



The uncertainty of the extracted corners. The natural cor-ners in indoor environments can be extracted as land-marks for mobile robot–based SLAM after theextraction of the line segments from the raw sensordata. Based on the two parameters (ri, ai) of the ith linesegment, it can be simply expressed as the traditionalequation y 2 ai3 x+ bi = 0 with ai = tan (ai+ 90),bi = ri 3 cos(ai) 3 ai2ri 3 sin(ai). There are threeparameters in the expression of every corner vector,including the position xcor, ycor and the angle ucor. Theintersection point of the two line segments with fourparameters (ri, ai, rj and aj) are shown in Figure 7 andthe detailed calculation is shown in equation (9).

Except for the intersection points, the second type ofcorner is the end point of the line segment which is faraway from the adjacent line segments. This point canbe considered as the intersection point of this line seg-ment and its normal line, which is perpendicular to thissegment and passes through the end point in the corre-sponding data set. The final expression of this type ofcorner is similar to the first type of corner because allthese corners are extracted as the intersection points ofthe two line segments

xcor =ri cos ai tan ai +

p2

� �� ri sin ai

� �� rj cos aj tan aj +

p2

� �� rj sin aj

� �tan ai +

p2

� �� tan aj +

p2

� �ycor =tan ai +

p

2

� �xcor � ri cos ai tan ai +

p

2

� �+ ri sin ai

fcor = jai � ajj

ð9Þ

Every extracted corner is expressed by the positionin the two-dimensional (2D) Cartesian coordinate sys-tem with the additional Gaussian noise

( x y f )T = xcor ycor fcorð ÞT +N 0,Ccorð Þð10Þ

The covariance matrix of the corners is propagatedfrom the covariance matrix of the two lines. The propa-gation is a multi-input and multi-output system basedon the first-order Taylor expansion.29 The covariancematrix Ccor is a 33 3 matrix expressed as

Algorithm 2: Corner extraction algorithm

Input: The line segments vector Vline

Output: The corners vector Vcor

1: for line segment Li (i:1 - Vline.size)do2: j = i+1;3: Calculate the distance dc, angle uc between Li and Lj;4: if (dc . dmin or uc . umax) then5: Vcor.add(Li.SecondEndPoint, Lj. FirstEndPoint);6: continue;7: end if8: while (line segment Lj (j: i+1 - Vline.size)) do9: if (dc \ dmin and uc \ umax) then10: calculate the intersection point pc of Li and Lj;11: calculate the distance dp-1(pc to Li) and dp-2 (pc to Lj);12: if (dp-1\ dpMax and dp-2\ dpMax) then13: Vcor.add (pc);14: end if15: update dc and uc;16: end if17: end while18: end for

i

j

ir jr

Raw sensor data

Extracted corner

x

y

Extracted line segment

Figure 7. Intersection point of the two line segmentsextracted from the raw sensor data.




Ccor =FcorCliljFTcor ð11Þ

which is the similarity transformation of the 43 4matrix Clilj of Li and Lj

Clilj =Cli 023 2

023 2 Clj

� �ð12Þ

where two line segments are assumed to be independentbecause the sensor data in each data set are indepen-dent. The Jacobean matrix Fcor is a 33 4 matrix

Fcor =

∂xcor∂ri∂ycor∂ri

∂fcor

∂ri

∂xcor∂ai∂ycor∂ai∂fcor

∂ai

∂xcor∂rj∂ycor∂rj

∂fcor

∂rj

∂xcor∂aj

∂ycor∂aj

∂fcor

∂aj

26666664

37777775

ð13Þ

Data association algorithms

Existing data association algorithms

The two most widely employed data associationalgorithms in SLAM are ICNN with individualcompatibility21 and JCBB with joint compatibility.19

The ICNN algorithm matches the map feature forevery new measurement based on the maximum likeli-hood without considering the other new measurement.The new measurements are considered independently

in this method so that it may accept an incorrectmatching easily leading to divergence in the estimationpart. Although the matching accuracy is lower, theadvantage of ICNN is the lower computation complex-ity O(mn) where m is the number of new sensor mea-surements and n is the number of stored map featuresin the matching step. The result will be at most m unitmatching pairs, which is shown in the top-left of Figure8. Here, the unit matching pair includes only one mea-surement and one stored feature, of which the individ-ual compatibility should satisfy the limiting condition.

Unlike ICNN, the dependence of the new detectedmeasurements from the same sensor scan is consideredin the JCBB algorithm. For checking the joint compat-ibility and finding the best matching, all the possibleunit matches are combined by using the branch andbound method and its joint maximum likelihood (ML)are calculated. Even though the robustness of the dataassociation is increased, huge computation time is spentin the matching step, especially when the number ofmeasurements and map features are very large. Thesolution space of JCBB is (c+ 1)m with c (c \ n) fea-ture candidates and the computational complexity isO((1.53)m) obtained from the experiment results inNeira and Tardos.19 The interpretation tree of theJCBB is expressed in the lower left picture in Figure 8,in which the dimension of the testing matchingvector increases with the increasing number ofmeasurements.

1bestH

: empty

: 1 1( , )nearestM f






:

2bestH:

1

2

: 3 3( , )nearestm mM f



:( , )nearestm mM f

mpbestH:

mp

1M

2M

mM

ICNN: PCA:

JCBB:

: empty

1M

2M

mM

1mM

1 1( , )nearestM f

2 2( , )nearestM f

( , )nearestm mM f

1,*M

1

2

,*,*

MM

1

2

,*,*

,*m

MM

M

Figure 8. Interpretation tree of three data association algorithms: ICNN (left-top), JCBB (left-bottom) and PCA (right). In ICNNmethod, each new extracted corner M is matched with one nearest stored map corner f nearest to construct a unit matching pair (M,f nearest). In JCBB method, many possible stored map corners are matched with one new corner to construct the unit matching pairs.These unit matching pairs of each new corner are combined with those of all the other new corners to find the best matchingvector. In PCA method, p unit matching pairs and one partial best matching vector H are constructed for the p new corners in eachseparated group. The entire members for m/p groups are combined to find the best matching vector for all these m new corners.ICNN: individual compatibility nearest neighbor; JCBB: joint compatibility branch and bound.

Yan et al. 597



These two methods cannot be used in an environ-ment with dense features because of the low matchingaccuracy of ICNN and the high computation complex-ity of JCBB. Except for these disadvantages, the linear-ized computation time of ICNN and the robustmatching of JCBB can be merged into one good dataassociation algorithm. However, not all the best match-ing vectors using JCBB matched very well, as is shownin the following experimental result. In spite of the jointcompatibility, some incorrect unit matching can beincluded into the matching result.

Proposed algorithm

Based on these two data association algorithms, a PCAwith a more robust matching result and lower compu-tation time is proposed by combining the advantages ofICNN and JCBB. All the extracted corners from theraw sensor data in every step are divided into severalgroups with a small number of corners. In stochasticmapping, the shape formed by these small numbers ofcorners is also stable. The small number lowers theprobability of drawing into the wrong unit matchingpair. The computation time will be low in each groupfor the same reason. The JCBB algorithm is used tofind the local best matching in each group, while theICNN algorithm is used to check the nearest storedcorner for every new corner in a group.

Assume that there are p new extracted corners inevery group. There will be at most 1+ p members: pnearest unit matching pairs from ICNN and one bestmatching vector from JCBB. After obtaining all thepossible unit matching pairs and local best matchingvectors, the group will be integrated into a big groupby the algorithm shown in the right picture of Figure 8.If the total number of the new extracted corners is m,the number of groups is m/p. Because the number ofmembers in every group is smaller than the constantvalue p+1, this algorithm can do well in cases wherelarge quantities of stored features are matched with thenew detected features. The number of nodes of theinterpretation tree will be much bigger if all the newfeatures are matched with the stored ones by usingJCBB. However, for PCA, a little of these corners is

included, and the computation time of checking thejoint compatibility is lower than JCBB. In addition, theprocess of combining the groups is not affected bythe large number of possible feature candidates.

To easily understand the interpretation tree, a dataassociation example with the proposed PCA method isshown in Figure 9. There are 17 stored map corners(n = 17) and 15 new extracted corners (m = 15) in thefigure. However, due to the error of the odometry data,the positions of the extracted corners transformed inthe absolute coordinate system are not exactly the sameas the position of the corresponding stored corners. Tomatch the new corners with the stored corners usingPCA, the new corners are sequentially divided intothree groups with five corners in each group. By usingICNN and JCBB, five unit matching pairs and one par-tial best matching vector are found. Finally, all thegroups with six members are combined to find the bestmatching vector by using branch and bound method.From Figure 9, it can be seen that the stable geometry

Scanning areaof laser sensor

Mobile robot

Stored corner (q) New extracted corner (p)

Group 1Group 2

Group 3

Real environment

q1 q2

q3q4

p1p2

q5

p3

q6

q7p4

p5q8p6

q9

p7

q10

q11q12p8

p9p10

q13

p11

q14

p12

q15q16

q17p13

p14

p15

Figure 9. A data association example with proposed PCAmethod. There are 17 stored map corners and 15 newextracted corners. All these new corners are divided into threegroups with five corners in each group.

Algorithm 3: Partial compatibility algorithm

Input: New measurement vector mtotal

Output: Stored feature vector ftotal in the map

1: GroupOfMeasurement GroupDivision(mtotal, p);2: Vgroup [];3: for GroupOfMeasurement[i] (i: 1!m/p) then4: HGroupBest JCBB(GroupOfMeasurement [i], ftotal);5: UGroupICNN ICNN(GroupOfMeasurement [i], ftotal);6: Vgroup [i]. H = HGroupBest; Vgroup [i]. U = UGroupICNN;7: i = i+ 1;8: end for9: HTotalBest Total_Best_Matching_PCA(Vgroup);




of the corners in each group is formed. Due to thesmall number of corners in each group, the partial bestmatching vector can be constructed using JCBB withlow computation complexity. If the JCBB method isused for all the new corners, huge computation com-plexity would occur in the calculation of the bestmatching vector. If the ICNN method is used, a wrongunit matching pair (p6, q9) is obviously constructed ingroup 2 by considering the nearest neighbor. Actually,the new corners p6 and p7 should match with the storedcorners q8 and q9, respectively.

The detailed process of PCA is presented inAlgorithm 3, in which the inputs are the stored mapfeatures ftotal and the new measurements vector mtotal

which is the extracted corners’ vector Vcor in this arti-cle. The output of this algorithm is the best matchingvector HTotalBest with partial compatibility. First of all,all the new detected features are separated into differ-ent groups and then the local best matching vectorusing JCBB and the nearest unit matching for everynew corner using ICNN are stored in each group. TheML of every unit matching in JCBB is calculated with

Algorithm 4: Total_Best_Matching_PCA

Input: The vector Vgroup stored best matching vector and nearest unit matching pair in every group

Output: The best matching vector HTotalBest

1: Best_Matching_Checking(Ugroup);2: HTotalBest = []; Hiter = [];3: PCBB(Hiter, 1,1,1);4: return HTotalBest;5: PCBB(Hiter, i, t, s); //Consider the s-th element in group t.6: if i . m/p then //Is it the last group for the whole measurement?7: if dim(Hiter) . dim (HTotalBest) then8: HTotalBest Hiter;9: else if dim(Hiter) = dim (HTotalBest)10: && joint_compatibility(Hiter) \ joint_compatibility(HTotalBest)11: HTotalBest Hiter;12: end if13: else14: if feature_collision_checking(H, Ugroup[i]. mbest)15: && joint_compatibility(H, Ugroup[i]. mbest) then16: Hiter [Hiter, Vgroup[t]. H];17: PCBB(Hiter, i + Vgroup[t].size(), t + 1, s);18: end if19: NUBB(Hiter, i + 1, t, s + 1);20: //Checking the next nearest unit matching in group t.21: end if

Algorithm 5: NUBB

Input: Four parameters (Hiter, i, j, k)

Output: Instantaneous matching vector Hiter

1: if Feature_Collision_Checking(Hiter, Vgroup [i].U[k])2: && joint_compatibility(Hiter, Vgroup [i].U[k]) then3: Hiter [Hiter, Vgroup [i].U[k]];4: if (s == p) then5: PCBB(Hiter, i + Vgroup[j].size(), j + 1, k);6: else7: NUBB(Hiter, i, j, k + 1);8: end if9: end if10: if dim(Hiter) +m– (p*(j-1) + k) . dim (HTotalBest) then11: if (s == p) then12: PCBB(Hiter, i + Vgroup[j].size(), j + 1, k);13: else

14: NUBB(Hiter, i, j, k + 1);15: end if16: end if

Yan et al. 599



the nearest neighbor using ICNN easily found by com-paring the ML of the possible candidate.

To check the compatibility of the combination ofsome matching pairs from different groups, two recur-sive functions, Partial Compatibility Branch and Bound(PCBB) and Nearest Unit-matching Branch and Bound(NUBB), based on the branch and bound method arestated in Algorithms 4 and 5. In every group, the localbest matching vector is considered first by PCBB, andthen the nearest unit matching is checked one by one byNUBB. We assume that two new features cannot matchwith the same stored features successfully so that thematched feature is checked every time whether it hasbeen matched with the other ones in the current match-ing vector or not. The reason for the collision checkingis that there is just one feature close to the real feature inthe environment. In this article, this means that there isonly one good corner which can be used for correctingthe state of the mobile robot, while the other stored cor-ners are in a rugged place or a real corner. In addition,the function dim(H) in these algorithms is the numberof unit matching pairs of the current matching vectorH.

Computation complexity of PCA

One of the advantages of PCA is its low computationcomplexity which is proven based on that of ICNNand JCBB. The same assumption is established asbefore: the number of new features is m, the number ofmap features is n and the number of features in eachgroup is p with (p � m and p � n). For every group,the computational complexity by using JCBB andICNN is O((1.53)p+ pn). To simplify the process of theproof, we assume that all the combinations of the unitmatching pair for every new feature should be checked;however, only part of the combination is tested inPCA. The maximum computational complexity in thechecked part is about the computation of the covar-iance matrix of the matching vector, whose dimensionis 3p and the complexity is O((3p)3) according to theanalysis of the matrix computation.

Due to the p unit matching pairs of each group,there are at most 2p testing vectors in the group, andthe result is O(2p*(3p)3). The total computational com-plexity of all the calculating in m/p groups can beexpressed as

Om

p(1:53)p +mn+

m

p� 2p � p3

� ð14Þ

After finishing all the calculations in all the groups,the groups with at most 1 + 2p elements are thenchecked by using JCBB. It can be regarded as m/p newmeasurement with 1 + 2p stored feature candidatesincluding one empty matching vector, whose solutionspace is (1 + 2p)m/p. Because p is a small constant, thecomputation complexity can be considered asO((1.53)m/p). The sum of equation (14) and this partwill be larger than the real complexity of PCA.

Furthermore, that sum is smaller than O((1.53)m),which can be proven as

Om

p(1:53)p +mn+

m

p� 2p � p3 + (1:53)

mp

� ’O (1:53)

mp

� � ð15Þ

The computation complexity is decreased with theincrease in the value p. Even when p = 3, the equationchanges as O(1.15m) and the comparison result withICNN, JCBB is

O(mn)\O (1:53)mp

� �\O 1:53

13

� �m� �=O 1:15mð Þ ð16Þ

and

O(PCA)\O (1:53)mp

� �) O(PCA)\O 1:53mð Þ ð17Þ

For having the appropriate value p, the logarithmiccomputation complexities of JCBB and PCA in equa-tion (15) are compared with different numbers of mea-surements in Figure 10. The value p changes from 1 to50 and the number of stored map features n areassumed as 1000 in the four cases. The lowest computa-tion complexity of PCA occurs when p = 2–5 for m =50, p = 4–6 for m = 100, p = 10–12 for m = 500 andp = 16–18 for m = 1000. The value of p increases withthe increase in the number of new measurements.There are two special cases that can be seen from thepicture, p = 1 and p = m. If just one new feature isincluded in every group, the best matching of the totalnew features is the combination of the unit matchingpairs by using ICNN. Moreover, if there is just onegroup, the matching result and the matching time arethe same as the use of JCBB.

Improved EKF-SLAM

Prediction

The two important parameters of the standard EKF-SLAM algorithm, introduced in Thrun et al.,27 in thenth step are the state vector mn21 from the n2 1th stepand its covariance matrix Sn21. There are two parts inthe state vector mn21, the state of the mobile robot,coordinate xn21, yn21 and direction angle fn21, and thestate of the stored landmarks

mn�1 =

xn�1, yn�1,Fn�1ð Þ, m1

x,m1y,m

1f

� �, . . . ,

mNx ,m

Ny ,m

Nf

� �!ð18Þ

where N is the number of stored features in the previ-ous step n 2 1. Three parameters are used to expressthe natural corners, which are positions (x, y) in the 2Dcoordinate system and the angle u between the two linesegments. The position of both the robot and the land-marks is expressed in the absolute coordinate system,




of which the origin is the initial position of the mobilerobot in the whole experiment. If the odometrydata (dntrans, dnrot) are known, the state vector can bepredicted as

�mrobotn =mrobot

n�1 +dntrans � cos Fn�1 + dnrot

� �dntrans � sin Fn�1 + dnrot

� �dnrot

0@

1A+N(0,Rn)

ð19Þ

Assume that the first two entries on the right side ofthe equation are function gn(un, Xn21) (Xn21 = (xn21,yn21, Fn21)), which can be approximated by EKF line-arization using the Taylor expansion as

gn un,Xn�1ð Þ’ gn un,mn�1ð Þ+Gn Xn�1,mn�1ð Þ ð20Þ

The Jacobian matrix of the mobile robot is the par-tial derivative of gn(un, mn21) with respect to mn21 whichcan be expressed as

Gn Xn�1,mn�1ð Þ=1 0 �dn

trans � sin Fn�1 + dnrot

� �0 1 dn

trans � cos Fn�1 + dnrot

� �0 0 1

24

35

ð21Þ

In the prediction part, if the Jacobian matrix Gn andthe covariance matrix Rn of the odometry data areknown, then the covariance matrix of the entire statevector can be predicted through changing part of thecovariance matrix Sn21 as follows

�Xn=

Gn

P33 3n�1 GT

n +Rn Gn

P33 3Nn�1

Gn

P33 3Nn�1

� �T P3N3 3Nn�1

0@

1A ð22Þ

Here, the 3 3 3 sub-matrix from the n2 1th step is thecovariance matrix of the robot and the 3 3 3N sub-matrix is the covariance of the robot and the extractedcorners.

Correction

After the successful matching of the ith new extractedfeature (corner) Zi

n with the stored map feature Zj(i)n ,

the state vector and the covariance matrix should beupdated by these matching pairs using the conventionalmethod in Thrun et al.27

�mn = �mn +Kin zin � zj(i)n

� �ð23Þ

�Xn= I� Ki

tHj(i)n

� � �Xn ð24Þ

where the Kalman gain and the matrix H are shown as

Kin =

�XnHj(i)

n

�Tc�1j(i) ð25Þ

Hkn =

1Hkn 033 3 . . . 033 3

kHkn 033 3 . . . 033 3

� �ð26Þ

with

kHkn =

1 0 00 1 00 0 1

0@

1A, 1Hk

n =�1 0 00 �1 00 0 0

0@

1A ð27Þ

0 5 10 15 20 25 30 35 40 45 500

5

10

15

20

25

p−−The number of measuremens in each group

Loga

rithm

ic C

ompu

tatio

n C

ompl

exity

PCAJCBB

0 5 10 15 20 25 30 35 40 45 500

5

10

15

20

25


Loga

rithm

ic C

ompu

tatio

n C

ompl

exity

PCAJCBB

0 5 10 15 20 25 30 35 40 45 500

20

40

60

80

100


Loga

rithm

ic C

ompu

tatio

n C

ompl

exity

PCAJCBB

0 5 10 15 20 25 30 35 40 45 500

50

100

150

200


Loga

rithm

ic C

ompu

tatio

n C

ompl

exity

PCAJCBB

Figure 10. Comparison of the logarithmic computation complexity of JCBB and PCA (left-top: m = 50; right-top: m = 100; left-bottom: m = 500; right-bottom: m = 1000 with n = 1000, p = 1:50).PCA: partial compatibility algorithm; JCBB: joint compatibility branch and bound.

Yan et al. 601



The dimension of the state vector and its covariancematrix augments dramatically with the robot movingbecause many more new landmarks are detected. Thecomputational complexity increases with the dimensionthat can be simplified using the principle of the blockmatrix. The new method can be realized asX

n= I� Ki

tHj(i)n

� �Xn

= I�X

nHj(i)

n

�Tc�1j(i)H

j(i)n

� �Xn

=X

n�X

nHj(i)

n

�Tc�1j(i)H

j(i)n

Xn

=X

n�X

nHj(i)

n

�Tc�1j(i)

XnHj(i)

n

�T� �ð28Þ

Also, the covariance matrixP

n can be written as

�Xn= A0 . . . Ak . . . ANð Þ with Ai : (3N+3)3 3

ð29Þ

Then, the entry in equation (28) can be computed by

�XnHj(i)

n

�T=A0

1Hkn +Ak ð30Þ

And another 3 3 3 matrix can be calculated by theabove sub-matrix

ck =Hkn A0

01Hk

n +A0k

� �+Ak

01Hk

n +Akk +Ccor ð31Þ

with

Aj = A0j . . . Ak

j . . . ANj

� �and Am

j : 33 3 ð32Þ

The computation time decreases dramatically withthese simplifications, in which the number of multipli-cations or divisions and the number of additions or sub-tractions are shown in Table 1. From this table, we cansee that the computation complexity of the improvedextended Kalman filter (IEKF)-SLAM is smaller thanthat of the standard EKF-SLAM algorithm.

Experiment results

The experiments were done in the first undergroundfloor of Engineering building #5 at Hanyang Universityby using a Pioneer 3-DX mobile robot and Hokuyolaser sensor with a measurement range of 270� as shownin Figure 11. Due to the calculation of the covariancematrix of the extracted features, the uncertainty distri-bution of the distance and the angle of the Hokuyolaser sensor used in the following experiments aregiven as

sr =(630)mm��30! 1000mm(60:03 � r)mm��1000! 8000mm

� ð33Þ

su =p

1024ð34Þ

in which there are two sections in different calculatingprinciples of the distance error with a sensing distancefrom 30 to 8000 mm on the basis of the instructionbook and the variance of the angle error in Muelaneret al.30 The sketch map of the robot trajectory and thepictures of the real experimental environment with amagnitude of 50 m 3 15 m are shown in Figure 12.

The map and robot trajectory constructed by theraw sensor data based on the robot odometry data areshown in Figure 13, in which the small black points arethe sensor data from the environment and the positionof the mobile robot is instantaneous where the sensorscan is obtained. The sensor data of the same objectobtained at different positions with the robot movingcannot overlap very well owing to the error of the robotencoder. For eliminating these uncertainties, the natu-ral corners are extracted from the raw laser sensor dataand the IEKF-SLAM algorithm is used for correctingthe state vector based on these corners.

Extracted line segments and extracted corners

The basis of SLAM in an unknown environment is thatthe natural features can be extracted very well when a

Table 1. Comparison of the computation time with two methods.

Method N of multiplications or divisions N of additions or subtractions

IEKF-SLAM (3n + 3) 3 (9n + 18) (3n + 3) 3 (9n + 15)EKF-SLAM (3n + 3) 3 (3n + 3) 3 (3n + 3) (3n + 3) 3 (3n + 3) 3 (3n + 2)

EKF: extended Kalman filter; IEKF: improved extended Kalman filter; SLAM: simultaneous localization and mapping.

Figure 11. Experimental tools: Hokuyo laser sensor, Pioneer3-DX mobile robot and PC.PC: personal computer.




MotionTrtajectory

Initial-FinalPosition

Trash Can

Figure 12. Sketch map of the mobile robot trajectory and pictures of the real unknown environment in the experiment.

Position of mobile robotwith odometry data

Raw sensor data

Figure 13. The raw laser sensor data in SLAM experiment and the robot trajectory using odometry data.

Yan et al. 603



End point of a line segment far fromother segmentsIntersection point of two closer linesegmentsExtracted line segment

Position of mobile robot

Intersection point of two adjacent linesegmentsExtracted line segment


Figure 14. Corners (pentagon and dots) extracted from the line segments (straight line) by using two different methods: left—corner extraction algorithm in Algorithm 2; right—intersection points of two adjacent line segments li and li + 1.


End point of a line segment far from other segmentsIntersection point of two closer line segmentsExtracted line segment

Uncertainty ellipse of an extracted line segment

Uncertainty ellipse of extracted corner ( ), plotted using covariance matrixUncertainty ellipse of a laser pointUncertainty ellipse of the extracted corner by combining the ellipse of a laser point and a line segment

Figure 15. Proof of the covariance matrix of the second type of corner, the end point of the line segment, by comparing theuncertainty ellipse plotted using its covariance matrix and the uncertainty ellipse plotted by combining the uncertainty ellipse of theline segment and the uncertainty ellipse of the last raw sensor data in the data set of this line segment.




real feature is located in the range of the sensor. Thisproblem is solved perfectly based on Algorithms 1and 2. If we only consider the intersection point of theline segments li and li+1, nothing can be extracted inmost of the steps because of the restrictive condition.However, if both the end points of the line segment andthe intersection point of the possible segments are con-sidered, a sufficient number of corners can be extractedfrom the broad environment with only a few objects.

A comparison of the corners’ extraction result at theinitial step of the SLAM experiment by using these twomethods is shown in Figure 14, in which the extractedline segments are also displayed by the blue lines. Wecan see that there is only one corner in the right figurewith the previous method, but there are 14 corners withour algorithm: five intersection points drawn by a bolddot and eight end points of the line segments drawn bya pentagram. These corners are extracted from the edgeof the box, column and doors in the environment. Theuncertainty ellipse of the pentagonal corner inFigure 15 is proven by comparing the ellipse drawn bythe covariance matrix of this corner and the ellipsederived from the ellipse of the point and the linesegment.

There are three ellipses expressed with a solid line inFigure 15, the ellipse of an extracted corner, the ellipseof the line segment and the ellipse of the end sensingdata in this set. All these three solid ellipses are plottedusing their corresponding covariance matrix. Accordingto the properties of an uncertainty ellipse, the pointslocated in the ellipse of the line segment are the intersec-tion points of the line segment with its normal lines.Therefore, the lines connecting the center point of lasersensor and the points in the ellipse are the normal linesof the line segment. Four normal lines are chosen in thefigure, and the possible line segment candidates are theperpendicular line of these normal lines. To findthe end points of the possible line segments, four pointsare selected from the small ellipse of the end sensingpoint. Then, the perpendicular lines of the possible linesegments passing through these selected points are con-structed. The intersection points of these lines and thepossible line segments are the corner candidates. Byusing this method, many more corner candidates can befound. The ellipse covering these corners is the uncer-tainty ellipse of the extracted corner, plotted with adashed line in Figure 15. It can be seen that this ellipseis almost the same as the ellipse of the corner plottedusing the covariance matrix, which shows that the deri-vation of the covariance matrix of the extracted corneris correct.

Comparison of the SLAM results

Based on the extracted natural corners, the SLAMexperiment is done with three different data associationalgorithms: ICNN, JCBB and PCA. In the experiment,a mobile robot moves from the initial position, which isassumed as the origin point of the absolute coordinate

system, and moves back to this position at the finalstep. Therefore, the loop closure is checked at the laststep, with which the computation time should begreater due to the large number of matched corners.The sensor points from the environment are cumulatedstep by step with the robot movement.

The SLAM result with ICNN is shown at the top ofFigure 16, in which the sensor points are not very welloverlapped with the data points in the previous step.Some objects are not constructed the same as the realshape of the object, proving the poor accuracy of thecorrection of the mobile robot and landmarks. On the

(a)

(b)

(c)

Robot trajectory with motion command

Robot trajectory with odometry dataCorrected position of mobile robot using ICNN (a), JCBB(b)and PCA (c)

Figure 16. Comparison of the SLAM results by using differentdata association algorithms: (a) ICNN, (b) JCBB and (c) PCA.Also the robot trajectory with odometry data (red line), themotion command (black line) and correction result usingalgorithms are shown.ICNN: individual compatibility nearest neighbor; JCBB: joint

compatibility branch and bound; PCA: partial compatibility algorithm.

Yan et al. 605



contrary, the SLAM result by using JCBB is better thanusing ICNN, which can be seen from the middle ofFigure 16. Most of the sensor points overlap with theprevious data, but some do not cover them very well.Although many more new measurements are includedin the best matching vector of JCBB, the correctionwith the matched stored corners in this matching vector

is not perfect due to some possible incorrect unit match-ing pairs. The SLAM result by using PCA in the bot-tom of Figure 16 shows that the correction of themobile robot is successful at every step. The sensorpoints based on the corrected position with PCA alwayscumulate in the same position, which is better than theresults with ICNN and JCBB. In this experiment, the

0 20 40 60 80 100 1200

5

10

15

20

25

Number of steps

Tim

e(se

c)

Prediction TimeMatching TimeUpdate Time

0 20 40 60 80 100 1200

500

1000

1500

2000

2500

3000

3500

Number of steps

Tim

e(se

c)


0 20 40 60 80 100 1200

20

40

60

80

100

Number of steps

Tim

e(se

c)


Figure 17. Computation time in the prediction part, matching part and update part with three data association methods: ICNN(top), JCBB (middle) and PCA (bottom).SLAM: simultaneous localization and mapping.




new extracted corners at each step are divided into dif-ferent small groups with three corners in every group.

The prediction time, matching time and update timeat every step are shown in Figure 17. The predictiontime by using three algorithms at every step is almostconstant because there is only a slight change in the

number of extracted corners. The matching time withICNN is linear with an increasing number of storedcorners which can be seen from the top figure. Thematching time with JCBB increases exponentiallybecause of the large number of possible candidates forevery new corner. The large computation time can be

0 20 40 60 80 100 1200

2

4

6

8

10

12

14

Number of steps

Num

ber o

f Cor

ners

(n)

Detected CornerMatched Corner

0 20 40 60 80 100 1200

2

4

6

8

10

12

14

Number of steps

Num

ber o

f Cor

ners

(n)


0 20 40 60 80 100 1200

2

4

6

8

10

12

14

Number of steps

Num

ber o

f Cor

ners

(n)


Figure 18. Number of new extracted corners and matched corners at every step with three data association methods: ICNN(top), JCBB (middle) and PCA (bottom).SLAM: simultaneous localization and mapping.

Yan et al. 607



found in a special case in the 47th step, at which the 12new corners are extracted and the number of possiblecorners for every new one is expressed as the followingvector

0 0 0 9 8 3 6 7 1 0 1 0½ � ð35Þ

Before checking the joint compatibility, the unitmatching pairs of the stored corner and the extractedcorner are checked for individual compatibility withthe ML. Seven new corners are matched with at leastone stored corner and four of them are a little bigger.The matching time at this step takes 3091 s as shown inthe middle of Figure 17, which gives the conclusionthat JCBB are not appropriate for environments withdense features. The matching time with PCA in thesame special case takes just 72 s, which is much fasterthan JCBB. In the final step, the matching time is thelargest of all these steps because the loop closure isdetected. The number of new extracted corners and thematched corner at every step is shown in Figure 18.

To obtain a clear comparison result, the total match-ing and the total corners after every step, with ICNN,JCBB and PCA, are displayed in Figure 19. We can see

that the matching time of ICNN and PCA increasesmuch slower than that of JCBB. Of course, the match-ing time of PCA is bigger than ICNN and smaller thanJCBB. In contrast to the matching time, the number oftotal corners of the PCA is smaller than that of JCBBand bigger than that of ICNN. This result can beunderstood easily because the more stored corners thenew corners match with at every step, the greater thecomputation time this takes. If many more corners arematched successfully, fewer new extracted corners areregarded as new features stored in the state vector.

Except for a comparison of the SLAM results, theline segments extracted from a real environment basedon the corrected position of the mobile robot withPCA are shown in Figure 20(a). Most of the line seg-ments from the same place of the environment arelocated in the same position. Although this result isbetter than the other two methods, not all the robotpositions at each step can be perfectly correct becauseof this real experiment without any idealization. Inaddition, the distribution of the extracted corners, usedas landmarks, in the entire SLAM process is shown inFigure 20(b), in which all the corners come from thewall or edge of the environment and more than one

0 20 40 60 80 100 1200

500

1000

1500

2000

2500

3000

3500

4000

4500

5000

Number of steps

Tim

e(se

c)

ICNNJCBBPCA

0 20 40 60 80 100 1200

50

100

150

200

250

300

Number of steps

Num

ber o

f Cor

ners

ICNNJCBBPCA

Figure 19. Comparison of the total matching time (top) and number of total corners (bottom) in the previous steps with threedata association algorithms, ICNN, JCBB and PCA.ICNN: individual compatibility nearest neighbor; JCBB: joint compatibility branch and bound; PCA: partial compatibility algorithm.




corner is extracted from the real corner of the environ-ment. The experimental results of the line segments andthe extracted corners demonstrate the reliability of allthe algorithms presented in this article.

Conclusion

In this article, natural corners are extracted from rawlaser sensor data and considered as the landmarks usedfor robot localization and mapping with the proposednew data association algorithm. The corners can besuccessfully extracted from a roomy environment withfewer features with our corner extraction algorithm.The main idea of this algorithm is that not only theintersection points of the line segments but also the endpoints of the lines are considered as landmarks and theintersection points should be used to extract from allthe possible line segments that can be intersecting. Thedistinction between these two types of corners is notmade in the SLAM experiments.

To robustly match the stored map features with thenew extracted features, a new data association algo-rithm called PCA is proposed based on two well-knownalgorithms, ICNN and JCBB. In order to have a robustmatching result at a lower matching time, all the

features are divided into different groups with a smallconstant number of features because the shape of thesefeatures is much more stable than the shape of all thefeatures. Then, a SLAM experiment with these threedata association algorithms was done based on theredundant natural corners. From a comparison result,we can see that the accuracy of the robot position cor-rection by using PCA at every step is more robust thanusing ICNN, and the computation time is smaller thanusing JCBB. Also, the matching time of the PCA ismuch smaller than that of JCBB when the number ofpossible stored corners for the newly extracted cornersis very high. All these experimental results show thatPCA is efficient and that can be used in unknown envir-onments with dense features.

Declaration of conflicting interests

The authors declare that there is no conflict of interest.

Funding

This research was supported by the research fund ofHanyang University (HY-2013-P), and the Ministry ofKnowledge Economy, under the Industrial FoundationTechnology Development Program supervised by the

(a)

(b)

Extracted line segment Extracted cornerCorrected position of mobile robot using PCA

Figure 20. (a) Experimental result of the SLAM drawn by the extracted line segments with corrected robot position and(b) distribution of the total extracted corners (pink point) during the entire process of the experiment.PCA: partial compatibility algorithm.

Yan et al. 609



Korea Evaluation Institute of Industrial Technology(No. 10038660, Development of the control technologywith sensor fusion based recognition for dual-arm workand the technology of manufacturing process withmulti-robot cooperation). Also, this work was fundedby Building-facxade Maintenance Robot ResearchCenter, supported by Korea Institute of Constructionand Transportation Technology Evaluation andPlanning under the Ministry of Land, Transport, andMaritime Affairs (MLTM).

References

1. Choi KS and Lee SJ. Enhanced SLAM for a mobile

robot using extended Kalman filter and neural networks.

Int J Precis Eng Man 2010; 11(2): 255–264.

2. Mata M, Armingol JM, Escalera A, et al. A visual land-

mark recognition system for topological navigation of

mobile robots. In: Proceedings of the 2001 ICRA/IEEE

international conference on robotics and automation,

Seoul, Korea, 21–26 May 2001, vol. 2, pp.1124–1129.

New York: IEEE.3. Li GH and Jiang ZJ. An artificial landmark design based

on mobile robot localization and navigation. In: Interna-

tional conference on intelligent computation technology

and automation, Shenzhen, China, 28–29 March 2011,

pp.588–591. New York: IEEE.4. Lee SY and Song JB. Mobile robot localization using

infrared light reflecting landmarks. In: International

conference on control, automation and systems, Seoul,

Korea, 17–20 October 2007, pp.674–677. New York:

IEEE.5. Meyer-Delius D, Beinhofer M, Kleiner A, et al. Using

artificial landmarks to reduce the ambiguity in the envi-

ronment of a mobile robot. In: Proceedings of the IEEE

international conference on robotics and automation,

Shanghai, China, 9–13 May 2011, pp.5173–5178. New

York: IEEE.6. Ronzoni D, Olmi R, Secchi C, et al. AGV global locali-

zation using indistinguishable artificial landmarks. In:

Proceedings of the IEEE international conference on

robotics and automation, Shanghai, China, 9–13 May

2011, pp.287–292. New York: IEEE.7. Rosten E and Drummond T. Machine learning for high-

speed corner detection. In: Proceedings of the 9th Eur-

opean conference on computer vision (ECCV ‘06), Graz,

7–13 May 2006. Berlin/Heidelberg: Springer-Verlag.8. Shen F and Wang H. Corner detection based on modi-

fied Hough transform. Pattern Recogn Lett 2002; 23:

1039–1049.

9. Hwang SY and Song JB. Monocular vision-based SLAM

in indoor environment using corner, lamp, and door fea-

tures from upward-looking camera. IEEE T Ind Electron

2011; 58(10): 4804–4812.10. Namoshe M, Matsebe O and Tlale N. Corner feature

extraction: techniques for landmark based navigation

systems. In: Thomas C (ed.) Sensor fusion and its applica-

tions. Rijeka: InTech, 2010, pp.347–374.11. Nunez P, Vazquez-Martin R, del Toro JC, et al. Feature

extraction from laser scan data based on curvature esti-

mation for mobile robotics. In: IEEE international confer-

ence on robotics and automation, Orlando, FL, 15–19 May

2006, pp.1167–1172. New York: IEEE.

12. An SY, Kang JG, Lee LK, et al. SLAM with salient line

feature extraction in indoor environments. In: Interna-

tional conference on control, automation, robotics & vision,

Singapore, 7–10 December 2010, pp.410–416. New York:

IEEE.13. Bahari N, Becker M and Firouzi H. Feature based locali-

zation in an indoor environment for a mobile robot based

on odometry, laser, and panoramic vision data. ABCM

Symp Series Mechatron 2008; 3: 266–275.14. Borges GA and Aldon MJ. A split-and-merge segmenta-

tion algorithm for line extraction in 2D range images. In:

Proceedings of the 15th international conference on pattern

recognition, Barcelona, 3–7 September 2000, pp.441–444.

New York: IEEE.15. Brunskill E and Roy N. SLAM using incremental prob-

abilistic PCA and dimensionality reduction. In: Proceed-

ings of the IEEE international conference on robotics and

automation, Barcelona, 18–22 April 2005, pp.342–347.

New York: IEEE.16. Pfister ST, Roumeliotis SI and Burdick JW. Weighted

line fitting algorithms for mobile robot map building and

efficient data representation. In: Proceedings of the IEEE

international conference on robotics and automation, Tai-

pei, Taiwan, 14–19 September 2003, pp.1304–1311. New

York: IEEE.17. Nguyen V, Gachter S, Martinelli A, et al. A comparison

of line extraction algorithms using 2D range data for

indoor mobile robotics. Auton Robot 2007; 23(2): 97–111.18. Arras KO and Siegwart RY. Feature extraction and scene

interpretation for map-based navigation and map build-

ing. In: Proceedings of SPIE, mobile robotics XII, vol.

3210, Pittsburgh, PA, 14 October 1997, pp.42–53. Belling-

ham, WA: SPIE.19. Neira J and Tardos JD. Data association in stochastic

mapping using the joint compatibility test. IEEE T

Robotic Autom 2001; 17(6): 890–897.20. Kaess M, Ranganathan A and Dellaert F. iSAM: incre-

mental smoothing and mapping. IEEE T Robot 2008;

24(6): 1365–1378.21. Castellanos J, Montiel J, Neira J, et al. The SPmap: a

probabilistic framework for simultaneous localization

and mapping. IEEE T Robotic Autom 1999; 15(5): 948–

953.22. Feder HJS, Leonard JJ and Smith CM. Adaptive mobile

robot navigation and mapping. Int J Robot Res 1999;

18(7): 650–668.23. Paz LM, Tardos JD, Neira J, et al. Divide and conquer:

EKF SLAM in O(n). IEEE T Robot 2008; 24(5):

1107–1120.24. Paz LM, Pinies P, Tardos JD, et al. Large scale 6 DOF

SLAM with stereo-in-hand. IEEE T Robot 2008; 24(5):

946–957.25. Wijesoma WS, Perera LDL and Adams MD. Toward

multidimensional assignment data association in robot

localization and mapping. IEEE T Robot 2006; 22(2):

350–364.26. Blanco JL, Gonzalez-Jimenez J and Fernandez-Madrigal

JA. An alternative to the Mahalanobis distance for deter-

mining optimal correspondences in data association.

IEEE T Robot 2012; 28(4): 980–986.27. Thrun S, Burgard W and Fox D. Probabilistic robotics.

Cambridge, MA: MIT Press, 2005, pp.39–84.28. Guivant J and Nebot E. Optimization of the simulta-

neous localization and map building algorithm for real




time implementation. IEEE T Robotic Autom 2001; 17:242–257.

29. Hartley R. Multiple view geometry in computer vision. 2nded.Cambridge: Cambridge University Press, 2003,pp.157–163.

30. Muelaner JE, Wang Z, Jamshidi J, et al. Study of theuncertainty of angle measurement for a rotary-laser auto-matic theodolite (R-LAT). Proc IMechE, Part B: J Engi-

neering Manufacture 2009; 223(3): 217–229.

Appendix 1

Notation

C covariance matrix of line segments orcorners

K Kalman gainm number of new measurementsn number of stored map featuresN normal distributionO time complexity of algorithm

p number of features in each separatedgroup

P uncertainty of the sensing distance of rawsensor data

Q uncertainty of the rotation angle of rawsensor data

r intercept of extracted line segmentS sum of the square error between raw

sensor data and extracted line segmentw weighted value

a slope angle of extracted line segmentu rotation angle of laser sensorm state vector, including pose of mobile

robot and features of extracted cornersr measuring distance of laser sensorS covariance matrix of state vectorf intersection angle of extracted cornerF direction angle of mobile robot in

absolute coordinate system

Yan et al. 611



Documents

Natural corners-based SLAM with partial compatibility algorithm