Environmental Mapping by Trinocular Vision for Self-Localization Using Monocular Vision

Transcription

1 OS3-3 Envronmental Mappng by rnocular Vson for Self-Localzaton Usng Monocular Vson Yoo OGAWA, Nobutaa SHIMADA, Yosha SHIRAI Rtsumean Unversty, No-hgash, Kusatu, Shga, Japan he hrd Jont Worshop on Machne Percepton and Robotcs (MPR27) Abstract- hs paper presents a SIF based map buldng and self localzaton method for moble robots. 3-D postons of landmars are useful for the robot to localze the self poston, 3- D nput system occupes larger space and computatonal cost s too hgh to mplement on the embedded system. Although moton stereo technques usng only a monocular vson system have been developed, the accuracy of the depth measurement s not enough to buld navgaton maps. We proposed the followng scheme: Frst a mappng robot wth trnocular camera bulds the 3-D eypont map of unnown envronment wth a hgh accuracy and then the worng robot wth a monocular camera localze the own poston by matchng the SIF eyponts to the map. Expermental results of mappng and localzaton for a real ndoor scene are shown. I. INRODUCION Automatc envronmental map buldng s necessary for moble robot navgaton n unnown envronment. here has recently been consderable progress n developng real-world envronment mappng system based on the use of vsual appearances of natural or artfcal landmar features [1, 2, 3, 4]. Whle 3-D postons of landmars are useful for the robot to localze the self poston, the nput system occupes larger space and computatonal cost s too hgh to mplement on the embedded system. Although moton stereo technques usng only a monocular vson system have been developed, the accuracy of the depth measurement s not enough to buld navgaton maps. Snce most of sgnfcant landmars n ndoor scenes are not often changed, the followng scheme s proposed: at frst a mappng robot wth trnocular vson once explores the envronment and bulds the map of hgh qualty, then worng robot wth a monocular vson moves n the envronment by matchng the vsual cues to landmars n the map. hs paper descrbed the above scheme and shows expermental results of map buldng and navgaton n a real scene. Here egomoton s estmated only from the mage feature because we use an omn drectonal vehcle where nstallaton the rotary encoder s dffcult. II. EXRACION AND RACKING KEYPOIN USING SIF In order to ntally explore the envronment, we utlze a trnocular camera system of L shaped confguraton (Dgclops by Pont Gray Research Inc.). From the three captured mages at each tme frame, SIF (Scale Invarant Feature ransform) [5, 1] eyponts are extracted as landmars. rnocular correspondences are made to obtan ther depths. he eyponts are also traced over the tme sequence. he robot s egomoton and the 3-D postons of the features are estmated and then regstered on the map. A. Feature Extracton and Depth Measurement he SIF eyponts are extracted from DoG (Deference of Gaussan) dmenson extreme over a threshold. Each SIF eypont has mage coordnate [x y], scale s, orentaton o, contrast c and feature vector f. he feature vector s a 4x4x8 dmenson vector whch represents local ntensty patterns of the neghbor regon. he unque correspondences are searched for betweentwo mages. A par (rght-left and rght-top) of unque correspondence composes a stable eypont. Canddatesof correspondng pars of eyponts should satsfy the followng constrants: the eppolar constrants the dfference of the scales s wthn 1 level the dfference of the orentaton s less than 2 degrees the dstance between the SIF feature vectors below a threshold Correspondences are decded based on the dstance between the feature vectors among those canddates. If the shortest dstance s smaller than 7% of that of the secondshortest, the correspondence s establshed between the shortest par. he stable eyponts have a par (rght-left and rght-top) of Fgure 1 rnocular correspondences: he whte flled crcles are the stable eyponts, the blac lnes from the stable eyponts are horzontal and vertcal dspartes.

2 unque correspondence and the dfferent of the vertcal and horzontal dspartes less than 2% of each other. Each stable eypont has 3-D coordnate [X Y Z], dsparty d (mean of vertcal and horzontal dspartes) and the SIF eypont. he eypont was extracted from the rght mage. Fg. 1 shows the stable eyponts and ther depths. B. racng of Keyponts emporal correspondences of the stable eyponts are made between two successve frames by usng the rght mage of the trnocular mages as Fg. 2. When n eyponts are extracted at tme t and m eyponts are extracted at tme t- 1, f eyponts ( = 1,2, L, n) and ( = 1,2, L, m) satsfy the followng condtons: W Y ) x y yy ) + Y W yy ) x ) y s ) s s ) s s s d ) d ) d d ) d o d Fg. 2 racng eyponts ) where W s the observaton error covarance matrx. hen they are correspondng canddates. Among those canddates, correspondences are decded n the same way as n Sec.2.1. ) o ( f ) f ) ( f ) f ) III. ESIMAING EGOMOION AND MAP BUILDING he postons of the eyponts n the camera coordnate changes accordng to the robot egomoton. he egomoton of the robot s calculated by the moton of eyponts. Once the egomoton s obtaned, the 3-D poston of eyponts n the world coordnate s determned, and the envronment map s bult. A. Estmatng Egomoton he estmated poston of eypont x+1) n the camera coordnate at tme t+1 s predcted from the prevous poston x (t) as: xˆ + 1) = R( θ ))( x ) v( t)) where the 3-D poston of n eyponts n the camera coordnate s x = [ X Y Z ] ( = 1,2, L, n), translaton vector of mappng robot s v (t), rotaton of that s θ (t), and R ( ) s a rotaton matrx. he egomoton, m ) = [ v( t) θ )], can be estmated by mnmzng the sum of Mahalanobs dstance between x + 1) and x ˆ + 1) as shown n the followng equatons: q ) = x + 1) xˆ + 1) Q ) = S( x )) + S( x + 1)) m * ) = arg mn( where S(x) s the estmated error covarance matrx of the trnocular stereo measurements x. he possble m(t) s lmted n the small range because tme slce s small enough. In ths research, the brute force search was employed for the robot egomoton. he correspondence that has the largest Mahalanobs dstance does not be used to estmatng egomoton. here are some stable eyponts that were not traced. hey are transformed to the world coordnate then ther correspondences are searched n the map. he egomoton s estmated agan usng these correspondences. B. Update 3-D Poston of Keyponts he 3-D poston of the eyponts s transformed from the camera coordnate nto the world coordnate. We suppose that the eyponts on the envronment do not move and deform. Kalman flter s employed for precsely estmatng ther postons. hs flterng can be represented by the trnocular stereo measurements y, corrected 3-D poston xˆ of the eyponts, and predcted poston ~ x of the eyponts as follows: x ~ 1 ˆ = x + P C W { y C ~ x } ~ x xˆ P = ( = M + C W C = P 1 M n 1 m ) = 1 ) q ) Q ) 1 where M s the predcted error covarance matrx, P s the corrected error covarance matrx, and C s a transformaton matrx represented as: 1 q ))

3 cosθ C= snθ 1 snθ cosθ ( x ( x cosθ z snθ + z snθ ) cosθ ) Fg. 3 he flow of self localzaton of worng robots he envronment map s bult by teratng the above process. Fnally those feature ponts are deleted whch are observed only once. he feature of the landmars changes accordng to the vew drecton. hen the feature vectors are regstered every 1 degree. Each feature pont on the map has a set of features: observaton poston (X o, Y o ) and 36 SIF eyponts. IV. SELF LOCALIZAION OF WORKING ROBOS WIH MONOCULAR VISION Once the robot wth trnocular vson bulds the envronment map, the worng robot self-localzes on the map from the 2-D mage cues wthout any 3-D sensors. he robot captures an mage at the current poston, and extracts SIF eyponts from the mage and maes correspondence to the eyponts regstered on the map. Self-localzaton s acheved by fndng the poston whch mnmzes the dfference between predcted eypont s postons and those n the actually observed mage (loo Fg. 3). A. Predcton the 2-Dposton of the Keyponts he poston of the feature ponts on the map s proected by the proecton. It predcts each eypont s poston n the mage to be observed. We assume that the robot poston s nown approxmately. hen the feature ponts are deselected that have not been observed from the drecton. he center of Fg. 3 shows the poston of the eyponts n the camera coordnate where for each eypont the surroundng mage s attached at the poston. he predcted 2-D poston of n eyponts u ˆ = [ xˆ yˆ ] ( = 1,2, L, n) n the mage coordnate s determned from the 3-D poston of eypont w = [X Y Z ] n the map as: uˆ = P( R( φ )( w h)) where h and φ are poston and drecton of the robot respectvely, P ( ) s the proecton of the 3-D poston. All the vewable ponts are too much to localze the poston n real tme. hen the proected feature ponts should be reduced. Nearer ponts have advantage to obtan translaton, and farther ponts have advantage to obtan rotaton. We segment the feature ponts based on vew drecton and dstance from robot as Fg. 4, and pc up a dstnctve feature pont n each regon. he dstnctve feature s defned by the hghest observaton frequency and the hghest contrast. B. Matchng and Self-Localzaton Let the 2-D poston of m eyponts n the nput mage be denote by u = [x y ] ( = 1,2,,m), correspondng par canddates should satsfy the followng constrants: x ) x y ) y o ) o 1) 15 1) 15 1) 2 ( f( t) f( t 1)) ( f( t) f( t 1)).25 Fg.4 Reducng the Proected Ponts Among those canddates, correspondences are decded as n the case of Sec.2.1. he robot poston p s estmated by mnmzng the mean square dstance between the actual and predcted poston:

4 Fg.5 eypont tracng result: he blac flled crcles represent the eyponts matchng to those of the prevous frame and the whte flled crcles represent ones that no matchng eypont s found. he lnes from the blac flled crcles represent the optcal flow of the eyponts (ther tal ends show the postons of the correspondng eyponts at the prevous frame). Fg.6 Keypont map: he map scale s 1 cm per pxel. he orgn of the map coordnate s located at the n- tal robot poston, the center bottom of the map. he estmated robot postons (the rght camera poston) and ts movng traectory are shown as the flled small crcle and the sold lnes. he short blac sold lnes on the flled crcle represent the vewng drectons of the robot. he large non-flled crcle on the traectory tal represents the robot sze (approxmately 6mm). he 43 eyponts observed at least twce s drawn n the map as the covarance elpsods. p* = arg mn p l= 1 ( u uˆ ) l where s the number of the correspondng eyponts. l V. EXPERIMENAL RESUL We show the expermental results of the eypont map generaton n an ndoor scene and the self-localzaton of the worng robot. We mplemented the mappng robot wth a omn drectonal moble robot, the trnocular camera ( 99.6 mm baselnes, 32x24 mage resoluton and 28 fps samplng rate) and a destop PC (Pentum4 2.8GHz, memory 1GB, OS Wndows XP Professonal Verson 22 SP2). Frst we employed the mappng robot and bult a eypont map of an offce room. hen we re-employed the robot enabled wth only one camera of the trnocular devce as the worng robot n order to smulate the localzaton by monocular vson. A. Matchng and Self-Localzaton Fg.s 5(a)-(f) show the eypont extracton results of the frst 6 frames of an mage sequence for map buldng. he number of extracted stable eyponts from the one frame of the mage sequence was 51 at least, 97 at most, and 67 n average. he egomoton between successve mage frames were estmated by a brute force search n the followng range: 2

5 ABLE 1 REDUCED POINS x[mm] z[mm] φ[deg] Mean error Varance Devaton ABLE 2 ALL POINS x[mm] z[mm] φ[deg] Mean error Varance Devaton Fg.8 Estmaton traectory of worng robot: he blac crosses represent the estmated traectory of the worng robot s poston. Fg.7 Keypont matchng: he left column shows nput mages, the rght column shows reconstructed scene appearance. he blac flled crcles represent the eyponts correspondng to the proected ey ponts n the map, the whte flled ones are those not correspondng to them. 1 X 1 every 1[ mm] 1 Z 15 every 1[ mm] 1 θ 1 every 1[deg] Fg. 6 shows the generated eypont map. It can be seen that the covarances of the regstered ey ponts near the robot traectory or those observed from varous drectons are small enough. B. Self-localzaton results usng Keypont Map he ntal robot poston at the frst frame was gven approxmately. For the followng frames, the estmaton result at the prevous frame was gven as the ntal estmate. he range of the search s set as follows: 1 X 1 every 5[ mm] Z 15 every 5[ mm] 1 θ 1 every 2[deg] Snce the mappng robot s left camera traectory s nown, the localzaton accuracy can be verfed usng the left camera mages used n the map buldng process. able 1 and able 2 compare the localzaton accuracy wth reduced ponts to the accuracy wth all ponts. hey have smlar devaton and ther requred tme are 1.73 [sec/frame] and 14.3 [sec/frame]. Reducng ponts s able to reduce the requred tme about 1%. he mean ratos of the number of the proected ponts to the number of the matched ponts are 63.51% and 32.37%. Fg. 7 shows the nput mages captured by the worng robot (the left column) and the correspondng scene models obtaned as the result of the eypont matchng between the mage and the map (the rght column) wth reducng ponts. Fg. 8 shows the self-localzaton results of the worng robot by monocular vson. he worng robot moved near the traectory of the mappng robot by human gudance. he estmated traectory of the worng robot s close to the traectory of the mappng robot. VI. CONCLUSION We have presented a SIF-based mappng method and selflocalzaton method for moble robots. Once a mappng robot wth the trnocular camera bulds the map for an unnown envronment, the worng robots wth only one 2-D camera can localze own poston by matchng the vsble SIF

6 eyponts to those on the map. he current localzaton method usng a smple brute force matchng s much tmeconsumng. he eypont matchng needs more rapd algorthm le the Best-Bn-Frst [5], and the poston searchng also needs le the steepest descent method. he mappng robot should have a strategy where to explore and what nd of eyponts to select and store nto the map for the accurate navgaton. he worng robot should also have a strategy where to move for no mssng of ts locaton. he developments of those strateges are the future wors. ACKNOWLEDGEMEN We acnowledge Satosh Yamaguch and Yutaa Yamada for many helpful dscussons durng the development. REFERENCES [1] Stephan Se, Davd Lowe, Jm Lttle, Moble Robot Localzaton and Mappng wth Uncertanty Usng Scale-Invarant Vsual Landmars, Internatonal Journal of Robotcs Research, Vol. 21, No. 8, 22, pp [2] Yoshro Negsh, Jun Mura, Yosha Shra, Map Generaton of a Moble Robot by Integratng Omndrectonal Stereo and Laser Range Fnder, Journal of the Robotcs Socety of Japan, Vol. 21, No.6, 23, pp [3] Xu G., S. su, M Asada, A Moton Stereo Method Based on Coarse to Fne Control Strategy, IEEE ransactons on Pattern Analyss and Machne Intellgence, Vol. 9, No. 2, 1987, pp [4] Andrew J. Davson, Ian D. Red, Ncholas D. Molton and Olver Stasse, MonoSLAM: Real-me Sngle Camera SLAM, IEEE ransactons on Pattern Analyss and Machne Intellgence, Vol. 29, No. 6, 27, pp [5] Davd G. Lowe, Dstnctve Image Features from Scale-Invarant Keyponts, publcaton n the Internatonal Journal of Computer Vson, 24. [6] Jeffrey S. Bes and Davd G. Lowe, Shape ndexng usng approxmate nearest-neghbour search n hghdmensonal spaces, In Conference on Computer Vson and Pattern Recognton, Puerto Rco, pp. 1-16, 1997.