A FastSLAM-based Algorithm for Omnidirectional Cameras

JOURNAL OF PHYSICAL AGENTS, VOL. 7, NO. 1, JANUARY 2013 13 A FastSLAM-based Algorithm for Omnidirectional Cameras Cristina Gamallo, Manuel Mucientes and Carlos V. Regueiro Abstract—Environments with a low density of landmarks are difficult for vision-based Simultaneous Localization and Mapping (SLAM) algorithms. The use of omnidirectional cameras, which have a wide field of view, is specially interesting in these environments as several landmarks are usually detected in each image. A typical example of this kind of situation happens in indoor environments when the lights placed on the ceiling are the landmarks. The use of omnivision combined with this type of landmarks presents two challenges: the data association and the initialization of the landmarks with a bearing-only sensor. In this paper we present a SLAM algorithm based on the wellknown FastSLAM approach [1]. The proposal includes a novel hierarchical data association method based on the Hungarian algorithm, and a delayed initialization of the landmarks. The approach has been tested on a real environment with a Pioneer 3-DX robot. Index Terms—Simultaneous Localization and Mapping; Fast- SLAM; Omnidirectional camera; Hungarian Association; I. INTRODUCTION Simultaneous Localization and Mapping (SLAM) has attracted the attention of many researchers in the last decade. The first proposals were based on Extended Kalman Filters (EKFs) using range sensors like laser range scanners [2], [3] or ultrasound sensors [4], [5]. The use of cameras for SLAM, known as visual SLAM, is a more recent line of research and has received an increasing attention in the last years. The first work for visual SLAM was presented by Davison in 1998 [6]. The use of cameras is interesting as they are low-cost, light and compact sensors and, also, because they provide richer information of the environment, as colour and texture. One of the most widely used configurations in visual SLAM has been stereo vision [7], [8], [9]. Stereo cameras can provide 3D information into a single measurement, so traditional SLAM algorithms can be applied without modifications. The main drawback of this type of sensor is the limited 3D range. On the other hand, the use of monocular cameras allows the detection of very far objects. However, they are bearing-only sensors, i.e., they do not provide information about distance or depth. Therefore, a mechanism to estimate the 3D position of the landmarks needs to be incorporated. An important issue in visual SLAM is data association, which solves the correspondence between measurements and Cristina Gamallo and Manuel Mucientes are with the Centro de Investigaci´on en Tecnolox´ıas da Informaci´on (CITIUS), Universidade de Santiago de Compostela (Spain). E-mail: cristina.gamall[email protected], manuel.mucient[email protected] Carlos V. Regueiro is with the Department of Electronics and Systems, University of A Coru˜na (Spain). E-mail: [email protected] landmarks. The most simple technique for data association is Maximum Likelihood (ML) [10], which assigns the measurement to the nearest landmark that has not been associated yet. This approach is the most widely used for SLAM [11], [7], [12]. However, this technique is brittle if there are several equally likely hypotheses (Fig. 1(a)). This ambiguity is usual in robotics when landmarks are indistinguishable among each other, and it comes from two sources: i) pose uncertainty (Fig. 1(b)) and ii) because landmarks can be quite close to each other and have uncertainty. If a wrong data association is picked, this decision can have a catastrophic result on the accuracy of the resulting map. Several strategies [13] have been developed in order to deal with ambiguity in noisy environments. For example, Monte Carlo Data Association [14] assigns the correspondences probabilistically in accordance to their likelihoods. However, as happens with ML, it is a local method and it considers each measurement independently to establish the correspondences, and not the set of measurements as global methods do. Global methods make the joint data association between all measurements and landmarks based on a global score. This type of methods are better to cope with the ambiguity introduced by the noise of the sensors and the pose uncertainty. A typical representative of global methods is the Hungarian algorithm [15], which solves linear assignment problems in polynomial time for several measurements and landmarks. Another approach is presented in [16], [5], where they process multiple observations jointly, and consider the geometric relationships between a set of landmarks to test the correspondence vector. This method can only be applied on approximations that use features of the environment with geometric relationships. In this paper, we present a SLAM algorithm for omnivision. The omnidirectional camera has a fish-eye lens with a very wide field of view (FOV). The landmarks of the environment are the ceiling lights and, therefore, the camera is equipped with a band-pass IR filter. This makes features extraction easier, as the filter only detects those objects that emit in the IR spectrum. The proposal is based on the FastSLAM [1] algorithm, but modified for bearing-only sensors. The main contributions of the paper are: i) the data association, which is hierarchical and based on the Hungarian algorithm [15], to deal with the bearing-only sensor and, also, because the landmarks are indistinguishable among each other; ii) the initialization of the landmarks, that takes into account a bunch of measurements associated to the candidate landmark. The paper is structured as follows. First, we briefly review the related work for bearing only visual SLAM. Sec. III 14 JOURNAL OF PHYSICAL AGENTS, VOL. 7, NO. 1, JANUARY 2013 (a) Measurements ambiguity. Robot pose uncertainty Robot pose uncertainty (b) Different data association hypotheses for different robot poses. Fig. 1. Typical problems for data association in SLAM. Landmarks are represented by triangles, and the measurements by arrows. describes the proposed algorithm and Sec. IV presents the experimental results. Finally, Sec. V points out the conclusions. II. RELATED WORK IN BEARING-ONLY VISUAL SLAM Monocular cameras are bearing-only sensors, as they only provide information on the orientation of the objects detected in the image. As depth information is not available, 3D positions of objects cannot be obtained with a single image. The approaches to solve this problem, known as landmark initialization in bearing-only SLAM, can be grouped in two categories: delayed an undelayed solutions [17]. A representative approach to undelayed initialization is [18]. The initial state of the landmarks was approximated with a sum of Gaussians that augment the state of the EKF. However, the most usual approach is the delayed initialization mechanism, which requires the use of several images to estimate the 3D positioning of the landmark. [19] presents a landmark initialization algorithm based also in a sum of Gaussians but without including this information in the state vector. Also, in [20] a particle filter to estimate the initial position of the landmarks was described. The use of omnivision cameras in SLAM allows to track the detected features over long distances, as these cameras have a very wide FOV. Therefore, the initialization process is well conditioned by the numerous observations of the same landmarks. The first paper, as far as we know, that used this type of camera was [21]. In that proposal, the landmarks were initialized using a delayed mode for two poses: the triangulation of the measurements was compared to existing relationships of the tags already in the map. In [22], the true location of the landmarks was approximated by the intersection point of two lines. Finally, [11] presents a minimalist approach based on a topological map for environments of medium to large size. Although most of the visual SLAM approaches are based on EKFs [20], [22], [23], there are also approaches based on FastSLAM [12], on decoupling the pose error from the map error [21], etc. III. SLAM ALGORITHM The solution to the SLAM problem presented in this work is an extension of the FastSLAM 2.0 algorithm [1]. The novelties of the proposal are the data association process, and the initialization of the landmarks. This modifications are necessary as the sensor is bearing-only, and the landmarks are indistinguishable among each other. FastSLAM algorithms use a Rao-Blackwellized particle filter [10], i.e., a filter that represents the posterior with a combination of particles and Gaussians. For FastSLAM, the particles estimate the robot path, while the landmarks are filtered with EKFs. The main contributions of our approach are: •Hierarchical data association based on the Hungarian algorithm. As the sensor is bearing-only, and the landmarks are indistinguishable, it is necessary to implement a data association method. Our algorithm classifies the landmarks in two categories: regular landmarks (named landmarks in what follows) and candidate landmarks. The landmarks compose the map, while the candidate landmarks are those for which initialization was not possible yet. The data association has to take into account that the candidate landmarks are not reliable, i.e., most of the candidate landmarks will disappear, and only few of them will be transformed to landmarks. Therefore, priority is given to the association of the measurements to the current landmarks, while only those measurements that were not associated in the first stage will be included for the association with the candidate landmarks. This hierarchical data association is based on the Hungarian algorithm for each of the levels of the hierarchy. The Hungarian method is able to obtain the best association between the set of measurements and the set of landmarks in polynomial time. •Landmarks initialization. We propose an initialization mechanism for the landmarks in which the 3D position of each landmark is obtained through several consecutive detections from different positions. Moreover, the process requires to approximate the inverse model of the camera with a look-up table, in order to obtain the angles of each feature from the pixels in the image. Our proposal is shown in Alg. 1. It receives the set of Nzt measurements (zt) at the current time t, the control (ut) and the previous set of particles (Yt−1). Each particle kcontains an estimated robot pose denoted as (xk t−1), a map of Nk t−1 landmarks {Bk 1,t−1, . . . , Bk Nk t−1,t−1}, and the set of ηk t−1 candidate landmarks {Ck 1,t−1, . . . , Ck ηk t−1,t−1}. A landmark jis defined with a Gaussian of mean µk j,t−1and covariance Σk j,t−1, and with the number of times it has been detected (ik j,t−1). Each candidate landmark has a set of measurements GAMALLO ET AL: A FASTSLAM-BASED ALGORITHM FOR OMNIDIRECTIONAL CAMERAS 15 Algorithm 1 FasSLAM algorithm for Omnivision. SLAM (Zt, ut, Yt−1) 1: for k= 1 to Mdo 2: Get particle kfrom Yt−1: xk t−1,{Bk 1,t−1, . . . , Bk Nk t−1,t−1}, {Ck 1,t−1, . . . , Ck ηk t−1,t−1} 3: bxt=gxk t−1, ut 4: measurementsLikelihood() 5: Ψ = dataAssociation(Φ) 6: robotPoseUpdate() 7: landmarksUpdate() 8: end for 9: Yt=sampling(b Yt) {zk t−n, . . . , zk t−1}that could be transformed into a landmark if the initialization conditions are fulfilled. The steps of the algorithm can be grouped in the following blocks: •A main loop that iterates for each of the Mparticles (Alg. 1 lines 1- 8) to obtain the particle weights (wk). – Measurements likelihood. For each combination of landmarks and measurements, calculate the likelihood of the association (φl,j ) (Alg.2). – Data association. Solve the data association (Alg. 1 line 5). – Robot pose update. The mean and covariance of the proposal distribution will be calculated with the contribution of each of the associated landmarks. Finally, the pose of the robot will be sampled from that distribution (Alg. 3). – Landmarks update. Update each landmark using an EKF and calculate the importance weight (wk) for each particle (Alg. 4). •Resampling. The new set of particles (Yt) is generated by sampling the updated particle set (b Yt) with probabilities proportional to the particle weights (wk) using low variance sampling (Alg. 1 line 9). In the following subsections, the measurement model, the data association, the robot pose update, the landmarks update, and the landmarks initialization will be described in more detail. A. Measurement model The sensor model is a feature-based model, where the features are the lights placed on the ceiling of the environment. These features are extracted from the images obtained by an omnidirectional camera following a detection process that was widely described in [24]. The output of the feature extraction process is a list of pixel coordinates (ul, vl), that represent the centroid of each feature l. This list must be transformed into a measurements list (zt,l), where each measurement is given by the azimuth and elevation angles (ϕl,t, θl,t). The camera follows a projection model developed by Pajdla and Bakstein [25] that indicates how a 3D point can be transformed to a pixel in a 2D image. The model is described as a function of two angles, the azimuth (ϕ) and the elevation (θ): r=a∗tan θ b+c∗sin θ d ul=u0+r∗cos ϕ vl=β∗(v0+r∗sin ϕ)        (1) where a, b, c, d are parameters of the model, (u0, v0)are the coordinates of the pixel at the center of the image, and βis the ratio between the width and the height of a pixel. The transformation of the measurements requires the inverse camera model, i.e., given a pixel the inverse model returns the coordinates of the 3D point in the world. However, the camera model equations are not invertible. This has been solved through a look-up table: given the coordinates of a pixel, the look-up table provides the values of ϕand θ. Fig. 2 shows a graphical representation of the look-up table. The table only needs to be generated once, and this can be done off-line. The process is as follows: 1) Sample the values of ϕand θwith precisions δϕand δθ. Equations 1 are used to obtain the corresponding pixel coordinates. 2) Store, for each pixel, the maximum and minimum values of ϕand θ, as a range of values could correspond to the same pixel. (a) ϕ(white= π, black= −π) (b) θ(white= π/2, black= 0) Fig. 2. Graphical representation of the values of ϕand θprovided by the look-up table for each image pixel. B. Measurements likelihood The set of data associations (Ψ) between the measurements and the landmarks is decided based on the probability that feature lcorresponds to landmark j(φl,j ) (Alg. 2). The loop from lines 1 to 13 iterates for all landmarks in order to estimate all the φl,j values (Alg. 2 line 11). Each probability depends on the measurement (zt,l), the predicted measurement (bzl,j, Alg. 2 line 10) and the measurement innovation covariance matrix (Qj,l) (Alg. 2 line 6). The measurement innovation covariance matrix (Qj,l) is calculated taking into account the noise in the measurement (Qt), the previous covariance of the landmark (Σk j,t−1), and the Jacobian of h(measurement model) with respect to the measurement model variables. Moreover, the predicted covariance of the robot pose taking into account landmark j 16 JOURNAL OF PHYSICAL AGENTS, VOL. 7, NO. 1, JANUARY 2013 Algorithm 2 Measurements likelihood algorithm MeasurementsLikelihood() 1: for j= 1 to Nk t−1do 2: zj=hµk j,t−1,bxt 3: Hx,j =∇xthµk j,t−1,bxt 4: Hm,j =∇mjhµk j,t−1,bxt 5: for l= 1 to Nztdo 6: Qj,l =Ql,t +Hm,jΣk j,t−1Hm,jT 7: Σx,j,l = [Hx,jTQ−1 j,l Hx,j +R−1 t]−1 8: 9: µxt,j,l = Σx,j,lHx,jTQ−1 j,l (zt,l −zj) + bxt 10: bzl,j =h(µj,t−1, µxt,j,l) 11: φl,j = (2π)− Dim(Qj,l) 2|Qj,l|−1 2 exp −1 2(zt,l −bzl,j )TQ−1 j,l (zt,l −bzl,j) 12: end for 13: end for (Σx,j,l) depends on two terms: the motion noise (Rt), and the measurement innovation covariance matrix (Qj,l). The predicted measurement for landmark j(bzl,j) is estimated from µxt,j,l, which corresponds to the estimated robot pose (using the motion model) plus a correction due to the assignment of measurement lto landmark j. This correction is proportional to two terms. The first one can be interpreted as the gain (in the same sense as the Kalman gain), and is inversely proportional to the measurement innovation covariance matrix Qj,l, i.e. the higher the confidence in the measurement innovation (lower covariance), the higher the gain. Moreover, the gain is directly proportional to the proposal distribution covariance Σx,j,l, which means that the lower the confidence on the motion prediction (high covariance) the higher the gain (the correction due to the measurement has a high influence). On the other hand, the second term is the difference between the measurement and the prediction in the position of the landmark zt(Alg. 2 line 2). C. Data association Data association is carried out hierarchically following the Hungarian method. In the first level, measurements are assigned to the landmarks. Those measurements that have not been associated in the first level are associated to the candidate landmarks in the second stage. In this way, priority in the association is given to the landmarks, as the candidate landmarks are not reliable (many of them are created, but only a few will be initialized). The Hungarian method is a combinatorial optimization algorithm that solves the correspondence problem in polinomial time. The method requires the construction of a cost matrix (Φ) with size Nzt×Nk t−1+Nzt. Each element φl,j with j≤Nk t−1represents the probability that measurement lis assigned to landmark j(Line 11, Alg. 2). The elements with j > Nk t−1represent the probability that a measurement l comes from a new landmark (Pnew). Given the cost matrix, the Hungarian method returns a hypothesis or ambiguity matrix where each element (bcl,j) takes a value of 1 or 0, indicating whether or not measurement lis associated to landmark j. The ambiguity matrix fulfills the following conditions: X jbcl,j = 1,∀l and X lbcl,j ǫ{0,1},∀j(2) The first condition indicates that each measurement must be assigned either to a landmark or to a new landmark. The second condition reflects that a landmark can have an assigned measurement or not. D. Robot pose update The robot pose (Alg. 3 line 14) is sampled from a proposal distribution that considers both the motion and the observations. This proposal distribution is modelled as a Gaussian with mean µxtand covariance Σxt. The parameters of the Gaussian are estimated starting from the sampled pose bxtand Rt, and iteratively adding the corrections due to the assignment of measurement zt,ψjto landmark j(Alg. 3 lines 6 to 11). Algorithm 3 Robot pose update algorithm RobotPoseUpdate() 1: if Nk t−1 P j=1 ψj== 0 then 2: xk t∼pxt|xk t−1, ut 3: else 4: Σx,0=Rt 5: µx,0=bxt 6: for j= 1 to Nk t−1do 7: if ψj>0 & type (j) == 1 then 8: Σx,j = [Hx,jTQ−1 j,ψjHx,j + Σ−1 x,j−1]−1 9: µxt,j =µxt,j−1+ Σx,jHx,j TQ−1 j,ψjzt,ψj−zj 10: end if 11: end for 12: Σxt= Σx,j 13: µxt=µx,j 14: xk t∼N(µxt,Σxt) 15: end if It is important to be careful with the order in which the landmarks are processed to generate the proposal distribution as, in each iteration, the covariance of the proposal becomes smaller and the influence of the landmark in the mean of the proposal is lower. Therefore, landmarks with lower covariance (higher confidence) are processed in first place and, in case of having the same covariance, that with a lower ϕwill be selected. Finally, if none of the measurements have been assigned to previous landmarks, then the pose is generated sampling from the probability distribution of the motion model pxt|xk t−1, ut(Alg. 3 line 2). GAMALLO ET AL: A FASTSLAM-BASED ALGORITHM FOR OMNIDIRECTIONAL CAMERAS 17 E. Landmarks update The update of the landmarks and the estimation of bw(the contribution of each landmark to the weight of the particle) are defined in different ways depending on the type of landmark and measurement association (Alg. 4). Two different situations are possible: •Landmarks with assigned measurements (Alg. 4 lines 3- 18). •Landmarks not seen in the current iteration (Alg. 4 lines 18- 25). There are two possibilities: –The landmark is outside the perceptual range of the sensor. –The landmark is inside the perceptual range of the sensor. For the first situation, ik j,t is incremented as the landmark has been detected. The update of the landmarks follows the standard EKF update process: first the Kalman gain K is obtained (Alg. 4 lines 8) using the previous landmark covariance Σk j,t−1and the measurement innovation covariance matrix e Qj. Although the measurement innovation covariance matrix e Qjwas estimated previously, it must be estimated again because the Jacobian of the measurement model with respect to the measurement model variables e Hm,j depends of the robot pose and, now, the estimated pose (xk t) is more reliable than the predicted pose bxt. The same applies to the prediction of the measurement ezj(Alg. 4 line 5). Then, the mean µk j,t is updated proportionally to the gain and the difference between the measurement and its prediction. A high Kalman gain means that the confidence in the update is high. This can occur if the previous landmark covariance was high and also, if the measurement innovation covariance is low (high confidence). The contribution of the landmark to the weight of the particle (bw) represents the probability of association between the measurement zt,ψjand the landmark j. It is calculated from a Gaussian distribution with mean ezj(predicted measurement) and covariance L. This covariance is proportional to the noise motion (Rt) and e Qj. When the landmark has not been associated to any measurement, its mean and covariance remains unchanged. Moreover, if the landmark is in the perceptual range, the counter ik j,t is decremented (Alg. 4 line 22). The weight bwis estimated based of the probability of visibility of the landmark (pin). Finally, the weight of each particle wkis calculated as the product over all the weights for each landmark in the map (Alg. 4 line 26), as we assume independence among the landmarks. F. Landmarks types and initialization The generation of new landmarks from candidate landmarks, and the modification of the type of each landmark takes place at the end of the algorithm, in functions updateCandidateLandmarks() and updateLandmarksType(): 1) updateCandidateLandmarks(): The candidate landmarks are updated with the measurements that were not associated to the landmarks of the map. The correspondences of each candidate landmark with the features is decided on the second Algorithm 4 Landmarks update algorithm LandmarksUpdate() 1: wk= 1 2: for j= 1 to Nk t−1do 3: if ψj>0then 4: ik j,t =ik j,t−1+ 1 5: ezj=hµk j, t−1, xk t 6: e Hm,j =∇mjhµk j,t−1, xk t 7: e Qj=Qψj,t +e Hm,jΣk j,t−1e HT m,j 8: K= Σk j,t−1e HT m,j e Q−1 j 9: µk j,t =µk j,t−1+Kzt,ψj−ezj 10: Σk j,t =I−Ke Hm,jΣk j,t−1 11: if type (j) == 1 then 12: e Hx,j =∇xthµk j,t−1, xk t 13: L=e Hx,jRte HT x,j +e Qj 14: bw= (2π)−Dim(L) 2|L|−1 2 exp −1 2zt,ψj−ezjTL−1zt,ψj−ezj 15: else 16: bw= 1 17: end if 18: else 19: µk j,t =µk j,t−1 20: Σk j,t = Σk j,t−1 21: bw= 1 −pin 22: if µk j,t−1is inside perceptual range of xk tthen 23: ik j,t =ik j,t−1−1 24: end if 25: end if 26: wk=wk·bw 27: end for 28: {Ck 1,t, . . . , Ck ηk t,t}= updateCandidateLandmarks zt,Ψk t,{Ck 1,t−1, . . . , Ck ηk t−1,t−1}, 29: {Bk 1,t, . . . , Bk Nk t,t},{Ck 1,t, . . . , Ck ηk t,t}= updateLandmarksType {Bk 1,t−1, . . . , Bk Nk t−1,t−1},{Ck 1,t, . . . , Ck ηk t,t} level of the data association process (see Sec. III-C), based on the probability p(zt,l|xt, Cj,t−1). This probability is obtained from a Gaussian distribution with mean bzl,t and covariance Qj,l. Given the correspondences obtained in the data association, the following situations may occur: •Candidate landmark (Ck j) has an assigned measurement. The measurement is added to the candidate landmark, ik j is increased, and the algorithm checks if initialization of Ck jas a landmark is possible. •The measurement belongs to a new candidate landmark. The measurement is added to Ck jand ik jis set to 1. •Candidate landmark (Ck j) has no assigned measurements. Update the value of ik j=ik j,t−1−Inot,where Inot is the 18 JOURNAL OF PHYSICAL AGENTS, VOL. 7, NO. 1, JANUARY 2013 Zt-2,C1 Zt-1,C1 Zt,C1 Zt-NZ,C1 Xt-2 Xt-1 Xt Xt-NZ ANG Cross hCam Fig. 3. Landmarks initialization. number of consecutive iterations in which the candidate landmark was not observed. If ik jis not over 0, then delete the candidate landmark. Our sensor is bearing-only and, thus, from a single measurement only angles ϕand θcan be obtained for each feature. However, in order to estimate the 3D position of a landmark the distance to it (ρ) is also necessary. Therefore, at least NZMin measurements of the landmark must be taken from poses of the robot that are far enough from each other (Fig. 3). A candidate landmark (Ck j) will become a landmark if it fulfills the following requirements: 1) NZk j≥NZMin, where NZk jis the number of measurements associated along time to candidate landmark Ck j, and NZMin is a threshold. 2) Of all the calculated cross-points at least NCROSSVALID of them are valid. A cross-point is valid if it fulfills the following properties: a) The measurements used to obtain the cross-point were taken from robot poses that are separated by an angle ANG > ANGMin. b) The height of the cross-point is over the height of the camera. c) The probability of all the measurements (associated to the candidate landmark) and the cross-point is over PNew. 3) One of the valid cross-points has been generated from the current measurement zt. If the candidate landmark is initialized, its mean is set to the pose of the valid cross-point with the highest probability over all measurements, and its covariance is set to the default initial covariance Σ0. 2) updateLandmarksType(): To add more reliability to the system, landmarks in the map are classified into types I and II. A landmark will be type I if has been initialized with measurements taken from robot poses whose distances in the XY plane from the valid cross-point are lower than DMin. Those landmarks that do not meet this condition will be type II, although they have the same priority as type I landmarks for data association. The reliability of type II landmarks is lower that type I landmarks, as they were initialized from measurements in which the pose of the robot and the pose of the landmark were far away, and this can cause erroneous initial positions. Thus, they are not taken into account to estimate the pose of the robot (Alg. 3, lines 6 to 11) or the weights of the particles (Alg. 4, lines 11 to 17). At each iteration, updateLandmarksType() checks if the landmarks of type II fulfill the requirements of type I landmarks. Moreover, all landmarks whose value of ik jis negative will be deleted from the map. IV. EXPERIMENTAL RESULTS Fig. 4. Test environment. The proposed SLAM algorithm has been validated in a sports hall (Fig. IV) with a Pioneer 3-DX robot, and the landmarks (lights on the ceiling) were placed at a height of 6.5m. The test environment has big windows that modify GAMALLO ET AL: A FASTSLAM-BASED ALGORITHM FOR OMNIDIRECTIONAL CAMERAS 19 (a) Original image. (b) Detected features. Fig. 5. Features detection. the lightning conditions, occluding the landmarks that were situated near them. The onboard omnidirectional camera was placed at 1.8mover the floor, in order to minimize the occlusions due to people and a passband infrared filter was used to make features extraction easier (Fig. IV). The configuration parameters of the algorithm took the following values in all the experiments: NZMin = 3,NCROSSVALID = 5,ANGMin = 1.22 rad (7◦),DMin = 8.0m,Σ0= [0.0025 0.0; 0.0 0.0025] and PNew =1 2π|Ql,t|−1 2exp{−64 2}. The number of particles was set to 10, as this size is sufficient to warrant the stability of the system and to achieve accurate results. Fig. 6 shows the result of one of the tests1. The distance travelled by the robot was over 85 mand the area of the environment was 24×24 m2. The camera captured 320 images at 1Hz, and the linear and angular velocities of the robot were limited to 0.30 m/s and 0.52 rad/s respectively. Fig. 6(a) shows the estimated trajectory and the obtained map and, also, the real trajectory (GT robot) and the real map (GT map). At the beginning of the trajectory (steps 0-80) the error (Fig. 6(b)) increases as the landmarks have a high covariance due to a recent initialization. Once the landmark positions are stabilized (step 100), the error in the position of the robot is reduced. From steps 100 to 180 the error remains stable, as the system detects landmarks in all the directions. Then, the error starts to grow because the robot looses the references to its right side, as landmarks 13 and 14 are not initialized yet. The consequence of the increase in the position error is that landmarks 13 and 14 are poorly initialized. The robot is able to correct its pose when it sees again previous and well placed landmarks (steps 230-270), and the error goes down near zero. Next, as the covariances of landmarks 13 and 14 reduce, they get influence in the correction of the pose of the robot, and the error increases again as their positions at that moment are still incorrect (steps 270-300). On the other hand, the error in the angle of the robot is under 0.087 rad (5◦)in most of the time steps of the experiment. 1The associated video can be downloaded from http://www.gsi.dec.usc.es/mucientes/videos/JOPHA13-SLAM.mp4 SLAM map (m)GT map (m)Error (m) ID x y z x y z |x| |y| |z| ||xyz|| 5 13.53 -5.61 6.80 13.5 -5.5 6.5 0.03 -0.113 0.299 0.32 6 13.61 -0.73 6.80 13.5 -1 6.5 0.11 0.2723 0.3 0.42 7 13.59 4.01 6.83 13.5 3.5 6.5 0.09 0.508 0.325 0.61 8 13.60 8.92 6.98 13.5 8.0 6.5 0.1 0.924 0.481 1.05 9 22.56 -5.43 6.54 22.5 -5.5 6.5 0.06 0.07 0.039 0.10 10 22.51 -0.98 6.48 22.5 -1 6.5 0.01 0.0232 -0.016 0.03 11 22.63 3.79 6.69 22.5 3.5 6.5 0.13 0.291 0.192 0.37 12 22.73 8.58 6.85 22.5 8.0 6.5 0.23 0.579 0.353 0.72 13 30.87 -5.92 6.39 31.5 -5.5 6.5 -0.63 -0.424 -0.114 0.77 14 31.18 -1.28 6.48 31.5 -1 6.5 -0.32 -0.278 -0.025 0.42 TABLE I MAP ERROR In this test, the robot returned 3 times to the same position (steps 205, 298, 318) and, in all the cases, the robot recognized the landmarks and closed the loop. The final map and its comparison with the real map is detailed in Table I. The mean and maximum errors in the position of the landmarks are 0.48 mand 1.05 m. These values reflect the performance of the SLAM algorithm as, although the sensor is bearingonly, and the landmarks are detected far away from the camera (always with at a distance over 4.5m), their positions are quite accurate. This experiment was run 10 times, each one with a different seed. Fig. 7 shows the average and maximum errors for each seed, together with the mean and standard deviation of all the executions. These values reflect that the proposed SLAM algorithm is able to reliably estimate the pose of the robot and the map, independently of the randomness due to the sampling steps (Alg. 3 line 14, and Alg. 1 line 9). A. Comparison between Hungarian and Maximum Likelihood data associations The same experiment was executed replacing the Hungarian algorithm with ML data association [26]. The results are shown in Figure 8. The errors in angle are slightly worse in ML, but the errors in position are much higher for ML in comparison with the Hungarian association: the average error 20 JOURNAL OF PHYSICAL AGENTS, VOL. 7, NO. 1, JANUARY 2013 -8 -7 -6 -5 -4 -3 -2 -1 0 1 2 3 4 5 6 7 8 9 10 11 12 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 GT Robot Pose SLAM Robot Pose GT Map SLAM Map L5 L6 L15 L14 L13 L9 L10 L11 L12 10 20 30 40 50 60 70 80 90 100 110 120 130 140 150 160 180 190 200 210 220 230 240 250 260 270280 290 300 310 320 0 L7 L8 170 (a) Trajectory and Map. 0 0.25 0.5 0.75 0 10 20 30 40 50 60 70 80 90 100 110 120 130 140 150 160 170 180 190 200 210 220 230 240 250 260 270 280 290 300 310 320 Error (m) Steps X Error Y Error ||Error|| 0 0.0872 0.1744 0.2616 0 10 20 30 40 50 60 70 80 90 100 110 120 130 140 150 160 170 180 190 200 210 220 230 240 250 260 270 280 290 300 310 320 Error (radians) Steps Yaw Error (b) Errors in position and angle. Fig. 6. Trajectory, map and errors of one of the experiments. GAMALLO ET AL: A FASTSLAM-BASED ALGORITHM FOR OMNIDIRECTIONAL CAMERAS 21 0 0.5 1 1.5 2 2.5 3 0 1 2 3 4 5 6 7 8 9 Error (m) Seed Mean = 0.382 STD = 0.099 Mean = 0.828 STD = 0.246 Mean(||Error XY||) Max(||Error XY||) 0 0.0872 0.1744 0.2616 0.3488 0 1 2 3 4 5 6 7 8 9 Error (radians) Seed Mean = 0.051 STD = 0.012 Mean = 0.218 STD = 0.023 Mean(Error Yaw) Max(Error Yaw) Fig. 7. Mean and maximum errors for 10 runs with Hungarian data association. 0 0.5 1 1.5 2 2.5 3 0 1 2 3 4 5 6 7 8 9 Error (m) Seed Mean = 0.425 STD = 0.148 Mean = 1.339 STD = 0.669 Mean(||Error XY||) Max(||Error XY||) 0 0.0872 0.1744 0.2616 0.3488 0 1 2 3 4 5 6 7 8 9 Error (radians) Seed Mean = 0.053 STD = 0.018 Mean = 0.235 STD = 0.031 Mean(Error Yaw) Max(Error Yaw) Fig. 8. Mean and maximum errors for 10 runs with ML data association. in position is a 13% higher, and the maximum error in position is a 61% worse. Also, the standard deviation for position error is much higher for ML association. Moreover, the maximum errors with ML association are at the end of the trajectory, as the algorithm is unable to close the last loop. As a result, new landmarks are created in the bottom left part of the map Fig. 9, and the robot cannot recover its true location. Hungarian association consistently closes all the loops for all the seeds, while ML association fails to close the loops the 60% of the executions. V. CONCLUSIONS A SLAM algorithm, based on FastSLAM, using omnivision has been presented. Our system uses a bearing-only sensor, and the landmarks are indistinguishable. The main novelties of our proposal are the hierarchical data association based on the Hungarian algorithm and the way the landmarks are initialized. Experiments have shown a great accuracy, both in the pose of the robot and in the map, although the limitations of the bearing-only sensor and the distance between the robot and the landmarks. Also, we have evaluated the robustness of the algorithm through several runs with different seeds, obtaining in all the experiments good results. Moreover, we have studied the influence of the use of a global data association (Hungarian algorithm) in comparison with a local data association (Maximum Likelihood), showing that bearing-only visual SLAM with this type of landmarks requires a global data association to solve the SLAM problem reliably and accurately. ACKNOWLEDGMENTS This work was supported by the Spanish Ministry of Economy and Competitiveness under grants TIN2011-22935 and TIN2009-07737 and by the Galician Government (Consolidation of Competitive Research Groups, Xunta de Galicia ref. 2010/6). Manuel Mucientes is supported by the Ram´ on y Cajal program of the Spanish Ministry of Economy and Competitiveness. REFERENCES [1] M. Montemerlo, S. Thrun, D. Koller, and B. Wegbreit, “FastSLAM: A Factored Solution to the Simultaneous Localization and Mapping Problem,” in Proceedings of the AAAI National Conference on Artificial Intelligence, Edmonton, Canada, 2002. [2] S. Thrun, W. Burgard, and D. Fox, “A Probabilistic Approach to Concurrent Mapping and Localization for Mobile Robots,” Autonomous Robots, vol. 5, pp. 253–271, 1998. [3] J. A. Castellanos and J. D. Tard´os, “Mobile Robot Localization and Map Building: A Multisensor Fusion Approach,” in Proceedings of the International Symposium on Experimental Robotics, 1999, pp. 173–178.