arXiv:1402.4907v1 [cs.RO] 20 Feb 2014  Line Maps in Cluttered Environments  Leonardo Romero and Carlos Lara  Division de Estudios de Posgrado Facultad de Ingenieria Electrica Universidad Michoacana. Ciudad Universitaria. 58060, Morelia, Mexico  Abstract.   This paper uses the smoothing and mapping framework to solve the SLAM problem in indoor environments; focusing on how some key issues such as feature extraction and data association can be han- dled by applying probabilistic techniques. For feature extraction, an odds ratio approach to find multiple lines from laser scans is proposed, this criterion allows to decide which model must be merged and to output the best number of models. In addition, to solve the data association problem a method based on the segments of each line is proposed. Ex- perimental results show that high quality indoor maps can be obtained from noisy data.  1   Introduction  The Simultaneous Localization And Mapping (SLAM) is a classical problem in robotics. To solve it, a robot must use the measurements provided by its sensors to estimate a map of the environment and, at the same time, to localize itself within the map. While localization with a given map or mapping with known positions is relatively easy, the combined problem is hard to solve. Several approaches for SLAM have been proposed, the most successful are based on probabilistic techniques[1]. The principal difference between them are the map representation and the uncertainty description; feature–based SLAM approach uses a collection of primitives to represent the map. Features are rec- ognizable structures of elements in the environment. The feature extraction pro- cess reduces the complexity by capturing only the essential infomation of the raw data; this data reduction allows to represent and manage the sensor information efficiently but the resulting maps are usually less expressive and precise than when using the raw data. Lines and segments are commonly used to represent indoor environments, and the preferred sensor is the laser scan. Many algorithms are available in the literature for extracting multiple lines from range scans; unfortunately, some of these approaches are   ad hoc   methods that use a distance threshold. One of the contributions of this paper is a new method to find multiple lines from laser scans; the formulation presented focuses on estimating a likelihood ratio on the number of lines that are present in a laser scan. The proposed approach follows the Ockham’s razor principle; this principle states that the simplest explana- tion for some phenomenon is more likely to be accurate than more complicated
explanations. Other approaches that follows this principle are the Minimum De- scription Length (MDL) [2,3] and the Akaike Information Criterion (AIC) [4]. Another trouble that faces the feature–based SLAM approach is known as the association problem; this problem consists of associating the newest features with those already stored in the map. This is also a crucial problem, and a good option to solve it is the Joint Compatibility Test (JCT)[5]. In this paper, a validation gate is used within the JCT; the validation gate uses the segments associated with each line to improve the association. To solve the SLAM problem, the Smoothing and Mapping (SAM) framework[6] is implemented; SAM is a smoothing approach rather than the commonly used filtering one. The essential computational advantage arises due to the smoothing information matrix that remains sparse without the need for any approximations; SAM is based on QR and Cholesky matrix factorizations that greatly speed up the optimization procedure leading to a very efficient algorithm. The rest of the paper is organized as follow: Section 2 introduces the new approach to determine the best number of lines and their parameters; Section 3 overviews the Smoothing and Mapping approach; Section 4 focuses on the association problem; Experimental results are presented in Section 5; finally, Section 6 ends this paper.  2   Multiple Line Extraction  The principal problems to obtain multiple features are: 1) find the best number of features, 2) determine which points belong with each feature, and 3) estimate the feature parameters given its points. There are many techniques for multiple geometric feature extraction: Bottom-up [7,8,9], Probabilistic Techniques [10,11], voting schemes such as Ransac [12,13], Hough Transform [14], etc. Bottom–up approach has been used in many pattern recognition tasks. At first, local features are extracted from the data. Normally, a distance between each pair of current clusters is computed and the closest pair is merged iteratively until a stopping criterion is met; the simplest criterion involves a threshold: when the distance of the closest features is greater than the predefined threshold the process is stopped [9]. Here, the result depends strongly on the threshold and the distance used (usually a Euclidean or Mahalanobis distance). Robust regression techniques can manage data with a large proportion of data points that do not belong to the main model. The most widely used ro- bust algorithm is the Random Sampling and Consensus (Ransac) [12]. Due to its greedy nature, the sequential approach does not consider the relationship among different models; consequently, the result is usually imprecise for non- trivial cases. To estimate the number of features and their parameters, Thrun   et al. [10] use real-time variant of the expectation maximization (EM) algorithm; their method penalizes models with many features by using an exponential prior probability; the search for the best number of features is interleaved with the running EM
algorithm. The search involves a step for creating new features, and another one for removing features, both executed in regular intervals. Han   et al. [11] formulate the problem of finding features from range images in the Bayesian framework, where prior probabilities penalize the number of features. The algorithm simulates Markov chains with both reversible jumps and stochastic diffusions to traverse the solution space; reversible jumps are used to move among subspaces of different dimensions, such as switching feature models and changing the number of features. This paper formulates the problem of finding the number of features from a Bayesian viewpoint; specifically, we study the problem of finding a set of lines from a laser scan. Indoor environments are usually rich in planar surfaces, lines are the natural way to represent them. Nguyen   et al. [15] compare some algo- rithms to extract lines from laser scans.  2.1   Problem Statement and Notation  The multiple line extraction problem is stated as:   Given a set of points from a laser scan, find the best number of line segments and their parameters . The challenge for any line extraction algorithm consists of finding a realistic repre- sentation. Let   Z   =   { z 1 , . . . , z N   }   be a set of measurements obtained from a two–dimensional laser scan; the line extraction problem consists of finding the set of lines  Θ   =   { θ 1 , . . . , θ M   } ;   (1) that better represents   Z ; where,   θ j   represents the parameters of a line.  2.2   Initial Segmentation  The proposed algorithm to find lines from laser scan is similar to traditional agglomerative clustering. Initial clusters can be obtained by any conventional bottom–up technique (i.e. sliding window [9] or Iterative End Point Fit (IEPF) [7]). The segmentation step finds a set of   M   linear clusters   Z   =   { Z 1 , . . . , Z M   }  from   Z ; each linear cluster   Z i   is a group of adjacent points that follow a linear model. These clusters are pairwise disjoint, that is   Z i   ∩   Z j   =   {} | ∀ i   6   =   j . To find the resulting line map, similar clusters must be merged; merging phase is usually based on Euclidean or Mahalanobis distance, the following section formulates the merging phase from a probabilistic viewpoint.  2.3   Odds ratio test (ORT)  Let   Z a   ⊆   Z   be a point cluster that follows a linear model with parameters   θ a ; it is interesting to find the probability that   Z a   is generated by   θ a . Assuming
independence among measurements Pr( Z a   |   θ a )   ∝   ∏  z i ∈ Z a  exp  [  −   d 2  ⊥ ( z i , θ a )  2 σ 2  ]  = exp  [  −   ∑  z i ∈ Z a  d 2  ⊥ ( z i , θ a )  2 σ 2  ]  ∝   exp  (  −   χ 2  a  2  )  ,   (2) where  χ 2  a   =   ∑  z i ∈ Z a  d 2  ⊥ ( z i , θ a )  σ 2   .   (3) Using Equation (2) and considering independent gaussian measurements, the likelihood for the   M   clusters is Pr( Z   |   M, I ) =   1  (2 πr max ) M  ∫  . . .  ∫  exp    −   1  2  M ∑  j =1  χ 2  j      d M   r j   d M   α j  =   1  (2 πr max ) M M ∏  j =1  ∫   ∫  e −   χ 2  j  2   dr j   dα j   ,   (4) The ( M   −   1)–lines model is obtained by merging two clusters   Z a , Z b   ∈ Z   into a new cluster   Z c   =   Z a   ∪   Z b . The likelihood in that case is Pr( Z   | M   −   1 , I ) =   1  (2 πr max ) M − 1   ×  ∫   e −   χ 2  c  2   dr c dα c   ×   ∏ M j =1  ∫ ∫   e   − χ 2  j  2   dr j   dα j  ∫ ∫   e −   χ 2  a  2   dr a dα a  ∫ ∫   e −   χ 2  b  2   dr b dα b  (5) To choose between the   M   –lines model and the ( M   −   1)–lines model we com- pare their likelihood by using the ratio: Pr( Z   |   M   −   1 , I )  Pr( Z   |   M, I )   =   2 πr max  ∫ ∫   e −   χ 2  c  2   dr c dα c  ∫ ∫   e −   χ 2  a  2   dr a dα a  ∫ ∫   e −   χ 2  b  2   dr b dα b  .   (6) The integrals of Equation (6) can be solved by using a Taylor series expansion about the parameters of the least square line for each cluster. Let be   θ j ∗   =  〈 r j ∗   , α j ∗ 〉   the parameters of the least square line for the   j –th cluster and   χ 2  j ∗   its corresponding error. Taylor expansion about   χ 2  j ∗   gives  χ 2  j   ≈   χ 2  j ∗   + 1  2 ( θ   −   θ j ∗ ) T   ∇∇ χ 2  j   ( θ   −   θ j ∗ ) +   . . . ,   (7) where   ∇∇ χ 2   is the Hessian matrix, evaluated at   θ j ∗ . Finally, the odds ratio is found by solving Equation (6) with Equation (7):
R   = Pr( Z   |   M   −   1 , I )  Pr( Z   |   M, I )   =   r max  2  √  det( ∇∇ χ 2  a ) det( ∇∇ χ 2  b   )  det( ∇∇ χ 2  c   )  ×   exp  [   1  2 ( χ 2  a ∗   +   χ 2  b ∗   −   χ 2  c ∗ )  ]  .   (8) When the value of the odds ratio (Eq. 8) is equal to one, both models are equally like; lower values mean that the ( M   − 1)–lines model obtained by merging  Z a   and   Z b   is less likely to occur than the   M   –model; that is, clusters   Z a   and   Z b  should not be merged. On the other hand, values greater than one prefer the ( M   −   1)–model. Then, the odds ratio can be used to decide greedily which two clusters to merge. Equation 8 follows the Ockham’s razor principle: factors that multiply the exponential term penalizes models with more features.  2.4   Proposed Algorithm  As is shown in previous section, pairs of clusters can merged iteratively based on the ratio given by Equation 8. A tree provides a picture for agglomerative clustering techniques, in this sense the value obtained from Equation 8 also helps to decide the cutting height of the tree. That is, when the value of   R   is less than or equal to one, the best value for the number of models   M   has been found. This approach is valid because the densities involved are MLR (i.e. have a monotone likelihood ratio), and since the Gaussian model is MLR, the odds ratio test is applicable.  Algorithm 1:   Proposed Algorithm  Input : A laser scan   Z   =   { z 1 , . . . , z N   }  Output : A set of lines   Θ   =   { θ 1 , . . . , θ M   }   and their corresponding clusters  Z   =   { Z 1 , . . . , Z M   }  Find a set of local clusters   Z   =   { Z 1 , . . . , Z M   }   where   ⋃ M i =1   Z i   ⊆   Z , and  Z i   ∩   Z j   =   {}   for   i   6   =   j ;  repeat  Find the pair   Z a , Z b   ∈ Z   with the highest probability of the merged hypothesis ;  r   ←   Pr( Z | M   − 1 ,I )  Pr( Z | M,I )  if   r >   1   then  Z a   ←   Z a   ∪   Z b   ;  Z ← Z \ { Z b }   ;  M   ←   M   −   1  until   r   ≤   1;  Θ   =   { θ j   |   j   = 1 , . . . , M   }   where   θ j   is the best line for the cluster   Z j   in the least squares sense ;  return   Θ,   Z
u 1   u 2   . . . . . . . . .  x 0   x 1   x 2  z 1   z 2   z 3   z 4   z K  l 1   l 2  . . .  u M  l N  x M  Fig. 1.   Bayesian belief network for a SLAM problem. The objective of SLAM is to localize the robot while simultaneously building a map of the environment. The Full SLAM problem requires that the entire robot trajectory is also determined.  3   SLAM Framework  The Full SLAM problem can be formulated by a   belief net   representation as shown in Figure 1; here, the iSAM approach [6] is used to solve it. The trajectory of the robot is denoted by   X   = [ x 0 , . . .   x M   ], where   x i   is the   i –th pose of the robot. The map of the environment is denoted by the set of landmarks   L   =   { l j   |   j   = 1 , . . . , N   } , and the measurement set by   Z   =   { z k   |   k   = 1 , . . . K } . For line maps,  l j   is a line –we use the algorithm introduced in the previous section to obtain line measurements from laser scans. The joint probability model corresponding to this network is  P   ( X, L, Z ) = Pr( x 0 )  M ∏  i =1  Pr( x i   |   x i − 1 , u i )  K ∏  k =1  Pr( z k   |   x i k   ,   l j k   ) .   (9) Let us denote   τ   = ( X, L ) the variables we are looking for; the maximum a posteriori (MAP) estimate is obtained by  τ   ∗   = arg max  τ  Pr( X, L   |   Z ) = arg min  τ  ( −   log Pr( X, L, Z ))   .   (10) Assuming gaussian process and measurement models, defined by Pr( x i   |   x i − 1 ,   u i )   ∝   exp   −   1  2   ‖ f i ( x i − 1   ,   u i )   −   x i ‖ 2  Λ i   (11) Pr( z k   |   x i k   ,   l j k   )   ∝   exp   −   1  2   ‖ h k ( x i k   ,   l j k   )   −   z k ‖ 2  Γ k   ;   (12) where   ‖ e ‖ 2  Σ   ≡   e T   Σ − 1 e   is the Mahalanobis distance.
A standard non-linear least squared formulation is obtained by combining equations 11, 12 and 9, and considering as uniform the initial distribution Pr( x 0 ),  τ   ∗   = arg min  τ  {   M ∑  i =1  ‖ f i ( x i − 1 ,   u i )   −   x i ‖ 2  Λ i   +  K ∑  k =1  ‖ h k ( x i k   ,   l j k   )   −   z k ‖ 2  Γ k  }  .   (13) Kaess   et al. [6] propose to solve Equation 13 incrementally; their method, known as iSAM, provides an efficient and exact solution by updating a QR factorization of the naturally sparse smoothing information matrix, therefore recalculating only the matrix entries that actually change. iSAM is efficient even for robot trajectories with many loops as it avoids unnecessary fill-in in the factor matrix by periodic variable reordering. Also, to enable data matrix factorization of the smoothing information matrix in association in real-time; Kaess   et al.   suggest efficient algorithms to access the estimation uncertainties of interest based on the factored information matrix. We have adopted a total SLAM schema such as iSAM because the informa- tion of each line can be updated when necessary. It can be accomplished because the robot position where a given measurement was taken is known. Following section describes how to use geometric information to improve data association.  4   Association Problem  The association problem consists on determine where two measurements ac- quired at different times were originated from the same physical object. In the context of feature–based SLAM, the system must determine the correct cor- respondences between a recently measured feature and map landmarks. This problem is a challenging task because features are often indistinguishable. The solution of this problem is essential for consistent map construction since any single false matching may invalidate the entire process[16]. The direct solution consists of associating a measurement with the closest pre- dicted observation –A predicted observation is a function of the best robot pose and a map feature. This solution, known as   Single Compatibility Test   (SCT), commonly uses the Mahalanobis distance or   Normalized Innovation Squared  (NIS)[17,18]. Single compatibility ignores that measurement prediction errors are correlated; hence, it is susceptible to accept incorrect matchings. Neira and Tard ́ os [5] propose the   Joint Compatibility Test   (JCT); an im- plementation of the JCT known as the   Joint Compatibility Branch and Bound  (JCBB) algorithm, generates tentative sets of associations and searches for the largest set that satisfies joint compatibility. For a given set of association pairs, joint compatibility is determined by calculating a single joint NIS gate. The ben- efit of joint compatibility is that it preserves the correlation information within the set of observations and predicted observations. Note that a validation gates based on the NIS distance (such as SCT and JCT) provides no statistical measure for the rejection of false associations [18]. To overcome false association, the following section introduces a technique that uses
l i   ̄ s i, 1  s i, 2 s i, 1  Fig. 2.   An illustration of the segments   S i   =   { s i, 1 , s i 2   }   and free-segments  ̄ S i   =   {  ̄ s i, 1 }  of a line   l i .  both the compatibility test and geometric constraints to find correct matchings between lines.  4.1   Validation gate based on segments  Let us define   S i   the set of segments of the   i –th line   l i   and  ̄ S i   the set of free– segments (the intervals of a line where there is a high probability of free space). The segments   S i   are calculated with the inliers of a line, while the free-segments ˆ S i   are calculated with points such as their measurement ray cross the line   l i , see Figure 2. The set of segments from a set of points can be obtained straightfor- ward, altough general techniques can be used [19,20]. To perform geometric association based on segments, the   i –th line   l i   and the new line   l ′  j   are transformed into the same coordinate frame; this transformation allow to treat the segments of a line as a set of intervals. Then the intersection of two segments   A   ∧   B   can be easily calculated by finding those segments that are present both in A and B. The probability that two lines represent geometrically the same line is Pr( G | S i , S i , S ′  j   ) =      ‖ S ′  j   ∧ S i ‖  ‖ S ′  j   ∧ S i ‖ + ‖ S ′  j   ∧    ̄ S i ‖   if ( ∥ ∥ S ′  j   ∧   S i  ∥ ∥   +   ∥ ∥ S ′  j   ∧    ̄ S i  ∥ ∥ )   6   = 0 ,  1  2   otherwise, (14) where   ‖·‖   is the total length of a segment set. When ( ∥ ∥ S ′  j   ∧   S i  ∥ ∥   +   ∥ ∥ S ′  j   ∧    ̄ S i  ∥ ∥ ) = 0 the local segments are never seen before, and we assign a 0 . 5 probability. The Segment Validation (SV) gate can be used to improve the Joint Com- patibility test. That is, a pair of lines are used in the Joint Compatibility test only if Pr( G | S i , S i , S ′  j   )   ≥   0 . 5.  5   Experimental Results  To test the ideas presented in this paper, two environments were used: a simu- lated environment and a real environment shown in figures 3 and 4, respectively.
Fig. 3.   Synthetic environment used for the tests  Table 1.   Comparison of line extraction algorithms from synthetic laser scans  TP   ND   μ err r   μ err α   speed  %   %   [mm]   [rad]   [Hz]  SR   91 . 29   21 . 32   6 . 56   0 . 0116   196 . 54  SM   75 . 86   24 . 13   6 . 68   0 . 0143   327 . 76  SM + ORT   95 . 38   12 . 70   4 . 37   0 . 0062   31 . 56  LT   89 . 97   18 . 22   4 . 92   0 . 0077   59 . 63  LT+ORT   96 . 82   13 . 20   3 . 95   0 . 0055   17 . 08  5.1   Simulated Environment  The simulated environment shown in Figure 3 was used to test the ORT per- formance to find lines from lasers scans; this environment has 42 lines, some of them are parallel and very close one another (300mm between parallel lines representing a door and its corresponding wall); the robot takes 1000 laser scans from random poses. Laser measurements are corrupted with gaussian noise with  σ   = 10mm. To find lines from laser scans three algorithms were selected: the sequential Ransac (SR), Split and Merge (SM) and Line Tracking (LT). Table 1 shows the experimental results; the first two columns show the True Positives (TP) and the Not Detected Lines (ND). In general, algorithms that use the Odds Ratio Test (LT+ORT and SR+ORT) increase the True Positives and reduce the proportion of lines not detected. The following two columns show the precision of the algorithms; this indicator shows the advantage of the ORT. Finally, the last column shows the speed 1   of the algorithms –one drawback is the increment of the time complexity when using the ORT.  5.2   Real Environment  To test the smoothing and mapping framework –including the proposed tech- niques to line extraction and data association; we use a real robot to take 215 laser scans from our laboratory using a Sick LMS–200. This sensor is configured  1   Tests were performed on a ElliteBook 6930p, 2.40 GHz
Fig. 4.   Raw data registered with ICP + Lorenztian [21] and route (dotted line), the scale is in cm.  to read 30m over a 180 o   arc, with an angle resolution of 0 . 5 o . Figure 4 shows the resulting points map from the same information; as can be seen, the envi- ronment is highly cluttered. Resulting line maps by using the SAM framework are shown in Figure 5; here, only the results from two algorithms are presented: the Sequential Ransac (SR) and the Split and Merge with Odds Ratio Test (SM + ORT). Figures 5a and 5c show the results from the Joint Compatibility Test, while figures 5b and 5d show the results from the Joint Compatibility Test with Segment Validation (SV). As expected, the line maps are less expressive than the map of raw points; Sequential Ransac approach gives poor quality line maps (figures 5a and 5b) –see for example the inaccurate estimation of angles; The Split and Merge algorithm gives better results (figures 5c and 5d). Line maps are qualitatively better when the Segment Validation scheme is used. Wrong associations may become more evident in environments with parallel lines that are very close one another: note the correct detection of doors and walls in Figure 5d. The validation based on the segments of each line prevents wrong associations improving the final map.  6   Conclusions  We have presented an implementation of the Smoothing and Mapping approach for indoor environments. The contributions of this paper are a probabilistic algorithm to find line clusters from laser scan data, and a validation gate based on segments that improves the data association for line maps. The proposed algorithm to extract lines from laser scans uses a probabilistic criterion to merge clusters rather than an   ad hoc   distance metric; the criterion is stated as a ratio of marginal likelihoods. This criterion allows to decide which
(a) SR + JCT   (b) SR + (JCT + SV)  (c) (SM + ORT) + JCT  • • • •  (d) (SM + ORT) + (JCT + SV)  Fig. 5.   Map of our laboratory using different techniques. Left column shows the results obtained by using the Joint Compatibility Test (JCT) and the right column maps are obtained with the Joint Compatibility Test with Segment Validation (JCT + SV). The dashed polygon represents the robot trajectory, and doors correctly detected are marked by bullets.  model must be merged and to output the best number of models. The proposed algorithm only uses the noise model parameters and avoid to use unnecessary thresholds. Experimental results show that the proposed approach works well to find the correct number of lines, increasing the proportion of True Positives and improving their precision. In other hand, the Segment Validation scheme is helpful to improve the data association when parallel lines are present in the environment. As is shown in the real test, the complete SAM framework finds high quality indoor line maps from cluttered environments.  References  1. Thrun, S., Burgard, W., Fox, D.: Probabilistic Robotics (Intelligent Robotics and Autonomous Agents). The MIT Press (2005) 2. Gr ̈ unwald, P.D.: The Minimum Description Length Principle (Adaptive Compu- tation and Machine Learning). The MIT Press (2007)
3. Yang, M.Y., F ̈ orstner, W.:   Plane detection in point cloud data.   Technical Re- port TR-IGG-P-2010-01, Department of Photogrammetry Institute of Geodesy and Geoinformation University of Bonn (2010) 4. Akaike, H.: A new look at the statistical model identification. IEEE Transactions on Automatic Control   19   (1974) 716–723 5. Neira, J., Tard ́ os, J.: Data association in stochastic mapping using the joint com- patibility test. IEEE Transactions on Robotics and Automation (2001) 6. Kaess, M., Ranganathan, A., Dellaert, F.: isam: Incremental smoothing and map- ping. IEEE Transactions on Robotics   24   (2008) 1365–1378 7. Duda, R.O., Hart, P.E., Stork, D.G.:   Pattern classification and scene analysis. Wiley New York (1973) 8. Borges, G.A.:   A split-and-merge segmentation algorithm for line extraction in 2-d range images.   In: ICPR ’00: Proceedings of the International Conference on Pattern Recognition, Washington, DC, USA, IEEE Computer Society (2000) 1441 9. Siegwart, R., Nourbakhsh, I.R.: Introduction to Autonomous Mobile Robots. Brad- ford Book (2004) 10. Thrun, S., Martin, C., Liu, Y., H ̈ ahnel, D., Emery Montemerlo, R., Deepayan, C., Burgard, W.:   A real-time expectation maximization algorithm for acquiring multi-planar maps of indoor environments with mobile robots. IEEE Transactions on Robotics and Automation   20   (2003) 433–442 11. Han, F., Tu, Z., Zhu, S.C.: Range image segmentation by an effective jump-diffusion method.   IEEE Transactions on Pattern Analysis and Machine Intelligence   26  (2004) 1138–1153 12. Bolles, R.C., Fischler, M.A.:   A ransac-based approach to model fitting and its application to finding cylinders in range data. In: IJCAI. (1981) 637–643 13. Schnabel, R., Wahl, R., Klein, R.: Efficient ransac for point-cloud shape detection. Computer Graphics Forum   26   (2007) 214–226 14. Leavers, V.F.: Which hough transform? CVGIP: Image Underst.   58   (1993) 250– 264 15. Nguyen, V., G ̈ achter, S., Martinelli, A., Tomatis, N., Siegwart, R.: A Comparison of Line Extraction Algorithms using 2D Range Data for Indoor Mobile Robotics. Autonomous Robots   23   (2007) 97–111 16. Durrant-Whyte, H.F., Majumder, S., Thrun, S., Battista, M.D., Scheding, S.: A bayesian algorithm for simultaneous localisation and map building.   In: ISRR. (2001) 49–60 17. Zhang, Z., Faugeras, O.: A 3-d world model builder with a mobile robot.   11   (1992) 269–285 18. Bailey, T.: Mobile Robot Localisation and Mapping in Extensive Outdoor Envi- ronments. PhD thesis, Australian Centre for Field Robotics, University of Sydney (2002) 19. Castellanos, J.A., Tard ́ os, J.D.:   Laser-based segmentation and localization for a mobile robot. ASME PRESS, New York (1996) 101–108 20. Castro, D., Nunes, U., Ruano, A.:   Feature extraction for moving objects track- ing system in indoor environments.   In: in Proc. 5th IFAC/euron Symposium on Intelligent Autonomous Vehicles. (2004) 5–7 21. Romero, L., Arellano, J.J.: Robust local localization of a mobile robot using a 2-D laser range finder.   In: ENC ’05: Proceedings of the Sixth Mexican International Conference on Computer Science, Washington, DC, USA, IEEE Computer Society (2005) 248–255