[2006] IEEE. Reprinted, with permission, from [Damith C. Herath, Sarath Kodagoda and Gamini Dissanayake, Simultaneous Localisation and Mapping: A

Transcription

1 [6] IEEE. Reprinte, with permission, from [Damith C. Herath, Sarath Koagoa an Gamini Dissanayake, Simultaneous Localisation an Mapping: A Stereo Vision Base Approach, Intelligent Robots an Systems, 6 IEEE/RSJ International Conference on, 9-5 Oct. 6]. This material is poste here with permission of the IEEE. Such permission of the IEEE oes not in any way imply IEEE enorsement of any of the University of Technology, Syney's proucts or services. Internal or personal use of this material is permitte. However, permission to reprint/republish this material for avertising or promotional purposes or for creating new collective works for resale or reistribution must be obtaine from the IEEE by writing to pubspermissions@ieee.org. By choosing to view this ocument, you agree to all provisions of the copyright laws protecting it

2 Simultaneous Localisation an Mapping: A Stereo Vision Base Approach Damith C. Herath, Sarath Koagoa an Gamini Dissanayake ARC Centre of Excellence for Autonomous Systems (CAS) Faculty of Engineering University of Technology, Syney Broaway, NSW, Australia {.herath, s.koagoa, g.issanayake}@cas.eu.au Abstract - With limite ynamic range an poor noise performance, cameras still pose consierable challenges in the application of range sensors in the context of robotic navigation, especially in the implementation of Simultaneous Localisation an Mapping (SLAM) with sparse features. This paper presents a combination of methos in solving the SLAM problem in a constricte inoor environment using small baseline stereo vision. Main contributions inclue a feature selection an tracking algorithm, a stereo noise lter, a robust feature valiation algorithm an a multiple hypotheses aaptive winow positioning metho in closing the loop. These methos take a novel approach in that information from the image processing an robotic navigation omains are use in tanem to augment each other. Experimental results incluing a real-time implementation in an ofce-like environment are also presente. Inex Terms SLAM, KLT, Loop closure, Stereo vision. I. INTRODUCTION There have been various attempts in using cameras as range sensors in robotic navigation. They in most cases fall in to two categories, namely in obstacle etection[, ] an in solving localization an mapping problem[3, 4]. In SLAM there is a wie an iverse range of implementations. From active stereo vision [5], static binocular an trinocular vision [6] to single camera bearing only SLAM[7]. The latter not use as a range sensor in the strict sense. The limite ynamic ranges in current CCD imagers make them vulnerable in outoors. In an inoor environment with more control over lighting conitions, it is possible to push further the state of the art in robotic navigation with these sensors. In the onset a feature base SLAM implementation using stereo camera looks straightforwar. However several issues neee to be aresse before a robust implementation can be achieve. In this work we aress some of these issues an present the results achieve with experiments conucte using a pioneer robot equippe with a stereo camera in an ofce environment. Especially when interpreting the results presente it is imperative to note the small baseline (.88m) of the camera an the wie angle lenses ( 9 o ) use. Accoring to [8] a baseline/epth ratio of less than /3 woul not make much sense. This translates to an inconsistent lter performance an in this exposition we present several ltering techniques that make the Extene Kalman Filter (EKF) base estimations consistent. In this work, the well establishe Kanae-Lucas-Tomasi (KLT) [9] algorithm is use in tracking features between image frames essentially reinforcing the ata association an a novel multiple hypotheses aaptive winow positioning metho is use in establishing a loop closure. Also we show with empirical evience that an EKF base algorithm still can be inconsistent ue to gross errors present in observations an limitations in the current error moels that efy conventional outlier etection methos. As a solution, we present a robust ata valiation algorithm with substantive experimental evience. Rest of the paper is organize as follows. Section II contains a summary of the 3D SLAM implementation whilst a summary of the (KLT) tracking algorithm is presente in Section III. Section IV presents two methos in ealing with stereo noise. Section V escribes the loop closure. In section VI we present comparative results of several SLAM implementations with our propose implementation. Section VII conclues the paper. II. 3D SLAM FORMULATION The SLAM frame work base on Kalman ltering is well establishe [], hence here we present only a summary to the 3D extension. The robot state is ene by X [ ] T r = xr yr ϕr, where x r an y r enotes location of the robot s rear axle centre with respect to a global coorinate frame an φ r is the heaing with reference to the x-axis of the same coorinate system. Lanmarks are moele as point features, P [ ] T i = xi yi zi, i =,,N an represente by Cartesian coorinates as in Fig.. Stereo Camera 3D features Gx-Global coorinate frame Zx Robot Fig. The robot in 3D worl coorinates observing a feature in 3 space.

3 A. Vehicle an lanmark augmente process moel The vehicle motion through the environment is moele as a conventional iscrete time process moel as in (). xr( k ) xr( k) ΔT V( k)cos( ϕr( k)) yr( k ) yr( k) ΔT V( k)sin( ϕr( k)) = ϕr( k ) ϕr( k) ΔT ω( k) ΔT is the time step, V(k) is the instantaneous velocity an ω(k) is the instantaneous turn-rate. The robot is assume to be travelling on a horizontal plane. The lanmarks in the environment are assume to be stationary point features an hence the process moel is, xi( k ) xi( k) y ( k ) y ( ) i = i k z ( ) zi ( k) i k where, i (=,,N) is the lanmark number. Using () an (), the augmente state transition matrix for the complete system can be represente. B. Observation moel The observation moel can be represente as, where zx ( k ) a Z ( k ) = zy ( k ) = b zz( k ) z( k ) a = ( x ( k ) xr ( k ))cos( ϕr ( k)) ( y ( k ) yr ( k ))sin( ϕr ( k)) b = ( x ( k ) x ( k ))sin( ϕ ( k)) ( y ( k ) y ( k ))cos( ϕ ( k)) r r r r It is to be note that each feature is ene by a point in 3D space, x ( ) ( ) ( ) ( ) T k = x k y k z k. The measurement error covariance is, T T xyϕ r xyϕ x y z x y z () () (3) P = g P g g R g (4) f σ uf σ vf σ B uf σ u σ uv σ R = σ u (5) vf σ uv σ v σ σ v where P r is the error covariance matrix of the robot location estimate extracte from the state covariance matrix P(k/k) an R is the measurement noise covariance. g is the Jacobean of the observation function. An σu, σv, σ represents the pixel uncertainties in image u,v location an the isparity respectively. f an B are the camera focal length an base line. Having ene the process moel an the observation moel, the stanar Kalman lter base realization [] is carrie out. III. KLT IMPLEMENTATION Data association is very crucial in a robust SLAM implementation. One strong avantage of using vision is its ability to track features in the image plane, an hence enhancing ata association. Kanae, Lucas an Tomasi [, ] have propose a feature selection an robust tracking algorithm (KLT). The main avantage of KLT over other escriptor base (e.g. [4] )methos is its ability to efciently track features between consecutive images in real time. A escription of the KLT algorithm is given in [9] an the complete erivation of the algorithm is presente in the unpublishe note by Birchel [3]. The tracker an the feature selection metho compliment each other in that the combination is optimal by esign. In this work we use KLT to extract reliable features an track them efciently between images. The tracking algorithm is escribe in brief below. Given two images I,J assuming small inter-frame isplacements, image motion can be escribe by suitably moving every point in the current frame to achieve the next frame I( x, y, t τ ) = I( x ξ ( x, y, t, τ), y η( x, y, t, τ)) (6) isplacement of a point at x is given by δ = ( ξ, η) an using the afne motion moel where, δ = Dx (7) xx xy D = yx is the eformation matrix an is the yy isplacement vector. A point at x in the rst image I moves to point Ax in the secon image J. J ( Ax ) = I( x ) (8) Where, A = D.In orer to n the above motion parameters A an D minimise issimilarity, [ ( Ax ) ( x)] ( x) x (9) ε = J I w w where w( x ) is a weighting function, which is set to in our implementation. In orer for the SLAM to perform real-time the number of features intialise/tracke per image was set to. IV. MANAGING NOISE A. Noise Filter KLT provies with goo features to track. However, it can pick up illusive features in the image plane like an intersection of two far apart real worl features. Those features are catastrophic for SLAM as they violate the

4 funamental xe lanmark assumption. Such features can be etecte by analyzing epth (isparity) iscontinuties in a small support region surrouning the point of interest as follows. Given the stereo images, a corresponing epth map (isparity image) of the scene is create. Then a small patch of the isparity image aroun the corresponing feature location selecte by KLT is obtaine (Our experiments showe that a 3x3 patch yiels better results). A histogram for this small patch is generate an the mean an the stanar eviation are calculate. Then the resulting stanar eviation σ is compare against a preetermine threshol (.m). If the test is successful the feature is use in the lter. For successful new features a elaye initialization is carrie out in orer to assert the stability of the feature (Fig..) B. Feature Valiation Algorithm One of the most ifcult issues in a SLAM implementation is the ata association problem, where the hypothesis of assigning an observation to an existing map location is teste. In our implementation the KLT tracker solves this problem given the high frame rates use to capture images. Since the consecutive images are only slightly isplace, the tracker successfully tracke over 95% of the features in our trial run. However initial SLAM implementation with KLT base ata association showe that the EKF base SLAM was not consistent (this is illustrate in Section VI). Further investigations reveale that this was in part ue to stereo mismatches an in part ue to KLT tracking features inconsistently. Both issues coul be attribute to the poor texture presence in the test environment an especially the former issue to the use of a smaller baseline, wie angle camera. In Fig. 3, the left image is a portion of an image taken from a trial run insie our lab. This is a typical image of our test environment where the pathways are very narrow with little texture throughout the camera s el of view. This generates large number of stereo mismatches. The image to the right is the isparity image generate by the stereo corresponence algorithm with lighter shaes inicating objects closer to camera. Though there are several small light patches to the mile an right of this image inicating features closer to the camera, in reality these features are quite far from it. This incorrect estimation of isparity in stereo vision gives rise not only to mean shift (gross errors) but also to incorrect estimations of uncertainties (5) in observations. Here we propose a solution to this by utilizing the correlations between features an the camera pose corresponing to a single image. The algorithm begins with the intuition, Given a single image frame, the features selecte are correlate with the camera pose that it was taken. Thus a single successful upate in the pose estimate using any of these features shoul reflect this fact resulting in very small innovations for the remainer of features observe with previously initialize states. I.e. given a set of features erive from a single image; a feature is selecte ranomly to upate the prior state estimate at current time step: the primary upate. A new prior estimate is erive for the features. Then a very tight gate (.8 σ ) is use to valiate the remainer of features. If a t (8%) number of features satisfy this test, the corresponing feature set is use to upate the state. This proceure is iterate until a solution is foun or, if not the set that encompasses the maximum no of conforming features is use for the upate, once all the features are exhauste in primary upates. The metho has its inspirations in the RanSaC [4] algorithm, hence the term t, the threshol parameter. RanSaC was use in [5] for global localisation in matching groups of escriptors to a global map. V. CLOSING THE LOOP Loop closing is important as it can signicantly reuce the uncertainty of the system. A novel aaptive winow positioning metho is use in testing the hypothesis of seeing an ol feature again at an eminent loop closure. Multiple hypothesis of association are generate for a newly observe feature with a subset of features previously seen an boune by a istance measure base on the state estimates of ol features an the estimate position of the new feature. Then Fig. KLT in combination with the noise lter uring feature initialization Fig. 3 Part of an image from the stereo camera (left) an the corresponing isparity image (right). Note the gross isparity errors in the right han sie.

5 each hypothesis is teste with the KLT base issimilarity measure by aapting the KLT search winow position to accommoate the hypothesise feature location. If a unique solution is foun for a given issimilarity threshol corresponing hypothesis is accepte. Else the feature is initialise. Essentially this metho relaxes the small interframe isplacement assumption in the original KLT formulation. Fig. 4 illustrates this process. Image on the top left shows a new feature foun by the KLT feature selection criteria (re ot on the black square on the far wall-magnie in the inset). Right image correspons to the same part of the environment but taken from a ifferent location uring the robot test run. Re ots inicate features selecte by KLT. There are three features on the same black square (magnie in the inset) on the wall an this subset of features form three hypotheses accoring to the above criteria. Bottom image shows that ones the issimilarity criterion in KLT is applie to them it picks up the feature correctly. This shows that the loop closing is possible with a reasonable viewpoint variance in the two images. However a prevailing shortcoming in the current metho of using KLT base loop closure etection is its inability to hanle larger view point variations. This is mainly attribute to the KLT s poor afne an scale invariance properties. VI. EXPERIMENTAL RESULTS A. Experimental Setup A Pioneer robot equippe with a MEGA-DCS stereo camera (see Fig.) from Viere Design is riven through an arbitrary path in an ofce like environment while the camera is capturing stereo images at 4Hz. Pioneer is also equippe with a SICK laser, which is use separately to capture range an bearing to a set of laser beacons lai in the environment. These observations are utilize in a separate D SLAM algorithm for comparison purposes. In this experiment, the number of features selecte in each image was limite to. This number was kept constant by replacing lost features when tracking of features between images faile. Three ifferent SLAM algorithms were implemente in this experiment an we will reference them later by the names given below.. SLAM3D_batch - 3D feature base EKF SLAM with innovation gating to valiate KLT tracke features with a batch upate [] algorithm. SLAM3D - 3D feature base EKF SLAM with the previously escribe feature valiation algorithm. Utilizes a batch upate when the nal successful feature set is foun. 3. A laser base D SLAM algorithm was use for benchmarking the results. It is worth noting that although we consier the laser base SLAM provies the true path, it has a gross error of about 5cm in both cross-track an along-track irections. Same tuning parameters were use for both vision base SLAM algorithms. Fig. 4 Multiple hypothesis approach an aaptive winow positioning. A new feature etecte in the current image (left-inset) an the hypotheses generate (right-inset) an a unique solution is foun (bottom). B. Experimental Results Fig. 5 shows the robot pose error plots using the SLAM3D_batch along with the -sigma error bouns estimate by the EKF. Previously mentione laser base SLAM was use to generate the true path. It is apparent (by the uner estimation of error) that even with goo ata association provie by KLT the lter is inconsistent. Fig. 6 shows the results of the same ataset as above with the secon algorithm (SLAM3D) without loop closure. Both noise lters iscusse in section IV are implemente in this algorithm. The improvement in lter performance is reaily noticeable inicating a well tune EKF. The threeimensional map generate by the algorithm is shown in Fig. 7. The D path estimate from the laser base SLAM is shown in black an light-green line inicates the SLAM3D estimate of the path. It can be seen qualitatively that the estimate robot path from the SLAM3D agrees closely with the more stanar D laser base SLAM. The errors are only apparent in the nal leg of the robot path. Those errors are x pose y pose heaing (ra) time Fig. 5 Robot pose estimate error for SLAM3D_batch relative to the laser base SLAM estimate with the -sigma error bouns

6 x pose y pose heaing (ra) time (s) Fig. 6 Relative error between SLAM3D estimate robot pose an the laser base SLAM pose estimate with the -sigma error bouns. (without loop closure) x pose y pose heaing (ra) time (s) Fig. 8 Relative error between SLAM3D an the laser base SLAM pose estimates along with the -sigma error bouns. (with loop closure) Fig. 7 Map an the estimate path (light-green) along with the laser base EKF SLAM estimate of the path (otte). (Without loop closure) mainly ue to the accumulation of SLAM errors in the long run an lower number of goo features registere in some portions of the run (iscusse later in the section). As apparent from the misalignment of the wall along the y- irection near x= cumulative gross error is prominent in the x-irection while errors in orientation an y-irection are consierably lower. Fig. 8 plots the state errors an -sigma error bouns using SLAM3D with the previously iscusse metho of loop closure. The loop closure is inicate by the large uncertainty reuction especially in the x an y-irections. Since even without the loop closure the gross errors in the heaing estimate were smaller (Fig. 6) it is expecte to see only a minor improvement in the relative error in estimates between SLAM3D an the laser base benchmarking algorithm. Fig. 9 is inicative of a successful loop closure with the apparent lack of misalignment of the wall that was present in Fig. 7. Fig. inicates the feature survival histogram. It coul be note that some goo features coul last more than 6 frames, which is esirable for SLAM, whilst the others can isappear instantly especially uring the thir leg of the traverse only a few features have been registere. This is mainly ue to KLT picking up features far away along the corrior (ue to lack of well texture features nearby) which the stereo algorithm was not able to register proper isparities Fig. 9 Map an the estimate path (light-green) along with the laser base EKF SLAM estimate of the path (otte). (with loop closure) hence the noise lter has attenuate them. In such illconitione situations oometry contributes locally. C. Real-time Implementation Currently we have an implementation of the iscusse algorithms in real-time sans the loop closure on a pioneer (Fig. & ). The interface provies a real-time Occupancy gri (Fig. ) base on the pose estimates from the SLAM algorithm an the laser observations. This provies a visual fee back of the performance of the SLAM algorithm. We have conucte several test runs in our lab area with statistically reliable results. Table I shows the prominent parameters involve in this implementation. However Fig. Feature survival histogram

7 TABLE I REAL-TIME IMPLEMENTATION PARAMETERS Parameter Value Camera baseline/.88 Image resolution/(pix ) 3x4 Robot spee/(m/sec).~.3 σ,σ u,σ v /(pix).5,.,. Processing power Intel Pentium M,.3GHz, 5 MB RAM epening on the quality of the features picke up by KLT uring some of the trials we have observe large absolute errors (>3%). This is suggestive of the necessity for more experiments an further improvements to the algorithms. VII. CONCLUSION Stereo vision base SLAM is still a challenging problem ue to the gross errors in epth map generation, feature occlusions an other factors present in the environment. We have presente a comprehensive set of methos that has roots in both image processing an Bayesian estimation omains to achieve consistent small baseline stereo vision base SLAM. We believe that to achieve robust SLAM solutions using vision sensors requires an augmentative approach between these two omains. We have presente a methoology for etermining goo features with the ai of KLT. Further, the ata association problem was minimize by utilizing the image feature tracking capability of the KLT. A RanSaC inspire algorithm in the ata association phase was also presente. The loop closure was aresse by utilizing a multiple hypothesis lter base on the KLT issimilarity criterion. This is fairly robust to scale an afne changes. Experiments were carrie out in an ofce like environment to Fig. Stereo vision base real-time SLAM interface Fig. Pioneer equippe with various sensors incluing the stereo camera assess the robustness an they show that the lter is consistent. Finally we have presente the real-time implementation of the iscusse algorithm without loop closure. We are furthering our investigations to improve the loop closure with larger view point variations which the current version is not capable of. Especially in ways of utilizing the knowlege of estimate state of the robot an map. We are also in the process of integrating a real-time loop closure metho in to the interface. ACKNOWLEDGMENT This work is supporte by the ARC Centre of Excellence programme, fune by the Australian Research Council (ARC) an the New South Wales State Government. REFERENCES [] M. Kolesnik, "Vision an Navigation of Marsokho Rover," presente at ACCV, 995. [] S. Se an M. Bray, "Stereo Vision-Base Obstacle Detection for Partially Sighte People," Thir Asian Conference on Computer Vision (ACCV '98), pp. 5-59, 998. [3] A. J. Davison an N. Kita, "3D Simultaneous Localisation an Map- Builing Using Active Vision for a Robot Moving on Unulating Terrain," presente at IEEE Computer Society Conference on Computer Vision an Pattern Recognition (CVPR ),. [4] S. Se, D. Lowe, an J. Little, "Mobile Robot Localization An Mapping with Uncertainty using Scale-Invariant Visual Lanmarks," International Journal of Robotic Research, vol.,. [5] Davison, A.J., Murray, an D.W., "Simultaneous localization an mapbuiling using active vision," IEEE Transactions on Pattern Analysis an Machine Intelligence, vol. 4, pp [6] Murray, D., Jennings, an C., "Stereo vision base mapping an navigation for mobile robots," presente at IEEE International Conference on Robotics an Automation, 997. [7] N. M. Kwok an G. Dissanayake, "Bearing-only SLAM in Inoor Environments Using a Moie Particle Filter," presente at Australasian Conference on Robotics & Automation, Brisbane, 3. [8] I. K. Jung, "Simultaneous localization an mapping in 3D environments with stereovision," in LAAS, vol. PhD. Toulouse: Institut National Polytechnique, 4, pp. 8. [9] J. Shi an C. Tomasi, "Goo Features totrack," presente at IEEE Computer Society Conference on Computer Vision an Pattern Recognition (CVPR '94) Seattle, 994. [] M. W. M. G. Dissanayake, P. Newman, S. Clark, H. F. Durrant-Whyte, an M. Csorba, "A Solution to the Simultaneous Localization an Map Builing (SLAM) Problem," IEEE Transactions On Robotics An Automation, vol. 7, pp. 9-4,. [] C. Tomasi an T. Kanae, "Detection an tracking of point features," Carnegie Mellon University CMU-CS-9-3, April [] B. D. Lucas an T. Kanae, "An Iterative Image Registration Technique with an Application to Stereo Vision," presente at International Joint Conference on Articial Intelligence (IJCAI '8), Vancouver, BC, Canaa,, 98. [3] S. Birchel, "Derivation of Kanae-Lucas-Tomasi Tracking Equation," birchel97erviation.pf, E., 997. [4] M. A. Fischler an R. C. Bolles, "Ranom Sample Consensus: A paraigm for moel tting with applications to image analysis an automate cartography," Communications of the ACM, vol. 4, pp , 98. [5] S. Se, D. G. Lowe, an J. J. Little, "Vision-base global localization an mapping for mobile robots," IEEE Transactions on Robotics, vol., pp , 5.