Simultaneous Localisation and Map-Building Using Active Vision

Transcription

1 IEEE rnsctions on Pttern Anlysis nd Mchine Intelligence Vol 4, No 7, pp 85-88, Simultneous Loclistion nd Mp-Building Using Active Vision Andrew J. Dvison nd Dvid W. Murry Abstrct An ctive pproch to sensing cn provide the focused mesurement cpbility over wide field of view which llows correctly formulted Simultneous Loclistion nd Mp-Building (SLAM) to be implemented with vision, permitting repetble long-term loclistion using only nturlly occurring, utomticlly-detected fetures. In this pper we present the first emple of generl system for utonomous loclistion using ctive vision, enbled here by high-performnce stereo hed, ddressing such issues s uncertinty-bsed mesurement selection, utomtic mp-mintennce nd gol-directed steering. We present vried rel-time eperiments in comple environment. Keywords Active Vision, Simultneous Loclistion nd Mp-Building, Mobile Robots. I. INRODUCION Incrementl building nd mintining of mps for immedite use by nvigting robot hs been shown to rely on detiled knowledge of the cross-coupling between running estimtes of the loctions of robot nd mpped fetures []. Without this informtion, fetures which re re-detected fter period of neglect re treted s new, nd the entire structure suffers progressive error ccumultion which depends on the distnce trvelled, not on distnce from the strting position in the fiducil coordinte frme. It becomes impossible to build persistent mps for long-term use, s pprent in erlier nvigtion reserch [] [3] [4] [5] [] [7]. For emple, Figure 5() of ref [7], shows tht the strt nd end of n ctully closed pth re recovered s different loctions. Storing nd mintining coupling informtion proves to be computtionlly epensive, in turn imposing the need to use only sprse sets of fetures. his runs counter to the emphsis of recent reserch into visul reconstruction, where lrge numbers of fetures over mny imges re used in btch mode to obtin ccurte, dense, visully relistic reconstructions for multimedi pplictions rther thn robotic tsks (eg. [8] [9]). Although btch methods provide the most ccurte nd robust reconstructions, the volume of clcultion required for ech cmer loction grows depending on the totl length of the trjectory. Rel-time pplictions on the other hnd require updtes to be clculble in time bounded by constnt step intervl: it is stisfying this crucil constrint which permits ll-importnt interction with the mp dt s it is cquired. So lthough visul sensing is the most informtion-rich modlity for nvigtion in everydy environments, recent d- he uthors re with the Robotics Reserch Group, Deprtment of Engineering Science University of Oford Oford OX 3PJ, UK. MPEG video illustrting spects of this work is vilble t he Scene Librry, open-source C++ softwre for simultneous loclistion nd mpbuilding which evolved from the work described in this pper, is vilble t jd/scene/ vnces in simultneous loclistion nd mp building (SLAM) for mobile robots hve been mde using sonr nd lser rnge sensing to build mps in D, nd hve been lrgely overlooked in the vision literture. Durrnt-Whyte nd collegues (e.g. []) hve implemented systems using wide rnge of vehicles nd sensor types, nd re currently working on wys to ese the computtionl burden of SLAM. Chong nd Kleemn [] chieved impressive results using dvnced trcking sonr nd ccurte odometry combined with submpping strtegy. hrun et l. [] hve produced some of the best known demonstrtions of robot nvigtion in rel environments (in museum for emple) using lser rnge-finders nd some vision. Cstellnos [3] lso used lser rnge finder nd mpping strtegy clled the SPmp. Leonrd nd collegues [4], working primrily with underwter robots nd sonr sensors, hve recently proposed new submpping ides, breking lrge re into smller regions for more efficient mp-building. In this pper, we describe the first ppliction of ctive vision to rel-time, sequentil mp-building within SLAM frmework, building on our erlier work reported in [5]. We show tht ctive visul sensing is idelly suited to the eploittion of sprse lndmrk informtion required in robot mp-building. Using cmers with the bility both to fite nd to chnge fition over wide ngulr rnge ensures tht persistent fetures re-detected fter lengthy neglect cn lso be re-mtched, even if the re is pssed through long different trjectory or in different direction. his is key to reducing the effect of motion drift from the fiducil coordinte frme: the drift now depends on the distnce from the origin, not the totl distnce trvelled. No doubt ctive sensing will be implemented electroniclly by choosing to process only subwindow from high-resolution omni-directionl dt. At present however full resolution multiple sensor cmers (fly-eyes) re epensive to construct nd mosicing still reserch problem. On the other hnd fish-eye lenses nd ctdioptric mirrors [] hve the disdvntge of vrible nd sometimes low ngulr resolution. In this work, we use gile electro-mechnicl stereo hed with known forwrd kinemtics, four degrees of movement freedom nd full odometry permitting the loctions of the cmers with respect to the robot to be known ccurtely t ll times nd their loction to be controlled in n closed-loop sense. While n ctive hed combines wide field of view with high sensing resolution, it lso introduces the interesting penlty tht finite time is required to re-fite the cmer, time in which further mesurements might hve mde of the previously fited scene point. Selective sensing is the essence of the ctive pproch, nd in mp-building there is much more to be gined by mking observtions of some prts of the robot s surroundings thn others:

2 : ; R φ W W Position(k+) Scene Feture hr hl hg Hed Centre nl R Coordinte Frme Crried With Robot R K s R d Hed Centre Position(k) L s H pl cl W Fied World Coordinte Frme W R () Coordinte Frmes (b) Motion Model (c) Active Hed Model Fig.. () he robot s loction in the world coordinte frme is specified by the coordintes. (b) he vehicle s motion geometry. (c) Hed geometry: the hed center is t height verticlly bove the ground plne. the two pper well-mtched. Here we consider only how ctive vision cn provide robot with ccurte loclistion; but this could be just one prt of robot s overll tsk. In [7], one of us described system where ttention is divided between loclistion nd inspection. Regrdless of the simplicity or compleity of the tsk, rigorous sttisticl frmework is necessry if prudent seril selection of fition point is to be mde. Although the computtionl compleity is high (in EKF-bsed SLAM, proportionl to, where is the number of mpped fetures), rel-time implementtion is fesible on modest hrdwre, even without the vrious SLAM short-cut methods which hve recently ppered [4] [8] []. he rest of the pper is orgnised s follows. In Section II we introduce the SLAM problem nd discuss some of the points relevnt to our implementtion. We present the imge processing pproch nd ctive hed control strtegies involved in identifying nd locting nturl scene fetures in Section III, nd Section IV describes n eperiment using contrived scene fetures to verify loclistion nd mp-building performnce ginst ground-truth. We continue in Section V by discussing the dditionl sensing nd processing tools, in prticulr ctive feture selection, which re necessry in fully utonomous nvigtion, nd in Section VI give results from fully utomtic eperiment. In Section VII we look t supplementing SLAM with smll mount of prior knowledge, nd in Section VIII bring ll these elements together in finl eperiment in gol-directed nvigtion. II. SLAM USING ACIVE VISION Sequentil loclistion nd mp-build bsed on the etended Klmn Filter (EKF) is now incresingly well understood [][3][][9][] nd in this section we wish only to estblish some bckground nd nottion. Detiled epressions for the kinemtics of our prticulr vehicle nd ctive hed cn be found in [5]. A. he Stte Vector nd its Covrince In order tht informtion from motion models, vision nd other sensors cn be combined to produce relible estimtes, sequentil loclistion nd mp-building unvoidbly [] involves the propgtion through time of probbility density functions (PDF s) representing not only uncertin estimtes of the position of the robot nd mpped fetures individully, but coupling informtion on how these estimtes relte to ech other. he pproch tken in this pper nd in most other work on SLAM is to propgte first-order pproimtions to these probbility distributions in the frmework of the EKF, implicitly ssuming tht ll PDF s re Gussin in shpe. Geometricl non-linerity in the motion nd mesurement processes in most SLAM pplictions men tht this ssumption is poor one, but the EKF hs been widely demonstrted not to be bdly ffected by these problems. More significnt is the EKF s inbility to represent the multi-modl PDF s resulting from imperfect dt ssocition (mismtches). he prticle filtering pproches which hve recently come to the fore in visul trcking reserch offer solution to these problems, but in their current form re inpplicble to the SLAM problem due to their huge growth in computtionl compleity with stte dimension [] in SLAM, the stte consists of coupled estimtes of the positions of robot nd mny fetures, nd it is impossible to spn spce of this stte-dimension with number of prticles which would be mngeble in rel-time; however, some uthors [] re investigting the use of prticle filters in robot loclistion. In the first-order uncertinty propgtion frmework, the overll stte of the system is vector which cn be prtitioned into the stte of the robot nd the sttes of entries in the mp of its surroundings. he stte vector is ccompnied by covrince mtri which cn lso be prtitioned s follows: #" $ <>@ %&% '%&(*) '%+(-,./. (*)% (*)()3'(*)4(-,5./. (, % (, ( )3 (, (,5./. 7 In this pper the robot stte is just ground plne position nd orienttion 879 " " nd ech feture stte is 3D position A9 B C " D but stte vector is not limited to pure position estimtes: other feture nd robot ttributes (such s velocity or the positions of redundnt joints) cn be included (eg [7]).

3 W O O " W I U ~ ~ c < < ~ 9 9 U ~ U < < 9 9 n š B. Coordinte Frmes nd Initilistion When the robot moves in surroundings which re initilly unknown, the choice of world coordinte frme is rbitrry: only reltive loctions re significnt. Indeed, it is possible to do wy with world coordinte frme ltogether nd estimte the loctions of fetures in frme fied to the robot: motions of the robot simply pper s bckwrds motion of fetures. However, in most pplictions there will be interction with informtion from other frmes of reference often in the form of known wy-points through which the robot is required to move (even in cse so simple s tht in which the robot must return to its strting position). A world coordinte frme is essentil to interct with informtion of this kind nd, s there is little computtionl penlty in including n eplicit robot stte, we lwys do so (Figure ()). In typicl nvigtion scenrios (such s tht of the eperiments of Sections IV nd VI) where there is no prior knowledge of the environment, the world coordinte frme cn be defined with its origin t the robot s strting position, nd the initil uncertinty relting to the robot s position in E%&% is eroed. If there is prior knowledge of some feture loctions (s in the eperiment of Section VII, these cn be inserted eplicitly into the mp t initilistion nd this informtion will effectively define the world coordinte frme. he robot s strting position reltive to these fetures must lso be input, nd both robot nd feture positions ssigned suitble initil covrince vlues. C. Process Model emporl updting using n EKF requires prediction of the stte nd covrince fter robot movement during possibly vrible period FHGJI. &K IL M IN O 9+ &K I M IN "JP I " FHG I K IL M IN K I M IN "QSR VUXW K IL *M IN K I M IN I Here, O4 is function of the current robot stte estimte, the period, nd control inputs P, which for our robot re steering ngle nd wheel velocity (Figure b): the robot s motion in ech time step is modelled s circulr trjectory with rdius Y determined by wheel geometry nd steering ngle (see [9] for detils). he full stte trnsition Jcobin is denoted Z+\ Z+[ nd is the process noise, I]^ O_ PH` where is the digonl covrince mtri of ` P. Process noise ccounts essentilly for unmodelled effects in the vehicle motion such s wheel slippge. D. Mesurement Model Any sensor which is ble to mesure the loction of fied fetures in the scene reltive to the robot cn contribute loclistion informtion, nd it is wise in implementtion to seprte the detils of the sensors (nd indeed the robot) from the lgorithms used to build nd process mps []. O_ P he key to our ctive pproch is the bility we gin from our probbilistic stte representtion to predict the vlue of ny mesurement, nd lso clculte the uncertinty epected in this mesurement in the form of the innovtion covrince b. Eplicitly, our mesurement model is: porqs/t where } d~ % ( dc>e cgf * {ƒ ƒ@ l ihkjmln hkjvl n hkjmln oo qs/u qsw 7 o qs/y orq{s B ; C ˆ B ƒ@ > ; l * ƒ " D > : D > : is the Crtesin vector from the hed centre to feture R (epressed in the robot-centred coordinte frme). is the length of vector nd /Š Œ % ~ is its projection onto the ;S: plne. ~ is the inter-oculr seprtion of the ctive hed, nd ˆ is the height bove the ground plne of the hed centre. he innovtion covrince b is clculted s: b '%&%Ž VU '%+( s U ( s ( s U Here '( s %Ž is the mesurement noise covrince mtri, defined shortly. Clculting b before mking mesurements llows us to form serch region in mesurement spce for ech feture, t chosen number of stndrd devitions (providing utomtic gting nd minimising serch computtion). We will see lter tht b lso provides the bsis for utomtic mesurement selection. In our work, mesurement of feture in the mp involves the stereo hed (sketched in Figure (c)) using this prediction to turn to fite the epected position of the feture, crry out stereo imge serch of sie determined by the innovtion covrince (see Section III-B), nd then use its mtched imge coordintes in combintion with the hed s known odometry nd forwrd kinemtics to produce mesurement of the position of the feture reltive to the robot. For filtering, mesurements re prmeterised in terms of the pn, elevtion nd (symmetric) vergence ngles c e f of n idelised ctive hed ble to mesure the positions of the fetures t perfect fition: by idelised, we men n ctive hed which does not hve the smll offsets between es possessed by our hed. In imge mesurements, we epect to detect fetures to n ccurcy of H piel, which for t the centre of the imge in the cmers used is n ngulr uncertinty of bout + rd. Compred with this, ngulr errors introduced by the ctive hed, whose es hve repetbilities two orders of mgnitude smller, re negligible. he dvntge of the idelised hed prmeteristion is tht when we mp the uncertinty coming from imge mesurements into this spce, the mesurement noise covrince is very closely digonl nd constnt nd cn be p-

4 U F b n F b n c c b n proimted by: ce c f F F F c e c}f In fct in our system F. his is preferble to prmeterising mesurements in the Crtesin spce of the reltive loction of feture nd robot, since in tht cse the mesurement noise covrince would depend on the mesurement in non-liner fshion (in prticulr the uncertinty in depth increses rpidly with feture distnce) nd this could led to bised estimtes. () (b) E. Updting nd Mintining the Mp Once mesurement of feture hs been returned, the Klmn gin œ cn then be clculted nd the filter updte performed in the usul wy []: ž ž f4ÿ fÿ œ %&% '(*)4% / œ9 / œ bvœ U %&( s '()4( s '(k,@( s Since in our mesurement model the mesurement noise digonl, this updte cn be seprted in implementtion into seprte, sequentil updtes for ech sclr component of the mesurement (tht is to sy tht we perform the bove updte three times, once for ech ngulr component c e f of the mesurement;, nd b become sclr in these steps): this is computtionlly dvntgeous. Initilising New Feture. When n unknown feture is observed for the first time, vector mesurement ž is obtined of its position reltive to the hed, nd its stte initilised ccordingly using the inverse ž 9 " }ž of the mesurement model. Jcobins Z v nd Z v q re clculted nd used to updte the totl stte vector Z+\m ndz+ covrince: ž 'ž f4ÿ f4ÿ where. ž ) Z %+% %+( %&% Z+\m '(*)_% '(*)4(*) (*)% Z / Z \ Z v Z \m '%+% Z v Z+\m '%+(*) Z v Z \m '%+(-, ª ª«ž U '%+% ž ž ž ž ž Deleting Feture. A similr Jcobin clcultion shows tht deleting feture from the stte vector nd covrince mtri is simple cse of ecising the rows nd columns which is (c) (e) (f) Fig.. (), (b): Feture detection. Rogue fetures likely to deleted s nonsttionry rise from depth discontinuities nd speculrities. (c), (b): Ellipticl serch regions generted for fetures; the sie of the ellipse depends on the uncertinty in the reltive position of the robot nd feture. (e), (f): wo emples of successful feture mtching close to the limits of visibility constrints. contin it. For emple, where the second of three known fetures is deleted, the prts removed re delineted s follows: š " (d) '%&% '%&() '%+(-, '%&(k '(*)4%5'(*)4(*) '()(-, '(*)4(- (, % (, ( ) (, (, (, '(k J(-, III. DEECION AND MACHING OF SCENE FEAURES Repetble loclistion in prticulr re requires tht relible, persistent fetures in the environment cn be found nd re-found over long periods of time. his differs perhps from the more common use of visul fetures in structure from motion, where they re often treted s trnsient entities to be mtched over few frmes nd then discrded. When the gol of mpping is loclistion, it is importnt to remember tht motion drift

5 will occur unless reference cn be mde to fetures fter periods of neglect. he visul lndmrks we will use should be fetures which re esily distinguishble from their surroundings, robustly ssocited with the scene geometry, viewpoint invrint nd seldom occluded. In this work, we ssume the fetures to be sttionry points. Since when nvigting in unknown res nothing is known in dvnce bout the scene, we do not ttempt to serch purposively for fetures in certin loctions which would be good sites for lndmrks: there is no gurntee tht nything will be visible in these sites which will mke good lndmrk. Rther, feture cquisition tkes plce s dt-driven process: the robot points its cmers in rther rbitrry directions nd cquires fetures if regions of imge interest re found. his rther rough collection of fetures is then refined nturlly through the mp mintennce steps described in Section V-C into lndmrk set which spns the robot s re of opertion. A. Acquiring 3D fetures Fetures re detected using the Hrris corner detector [3] s pplied by Shi nd omsi [4] to reltively lrge piel ptches ( & r rther thn the usul ] for corner detection). Products of the sptil grdients % nd *( of the smoothed imge irrdince re verged over the ptch nd if both eigenvlues of the mtri %v *% *%v *( %v *( *( *(²± re lrge, the ptch is corner-like. o cquire new feture t the current hed position, the detector is run over the left imge, finding predetermined number of the most slient non-overlpping regions. For the strongest feture, n epipolr line is constructed in the right imge (vi the known hed geometry), nd bnd round the line serched for stereo mtch. If good mtch is found, the two pirs of imge coordintes 9 ³> "@µ nd 9 ³ "@µ llow the feture s 3D loction in the robot-centered coordinte frme to be clculted. he hed is driven to fite the feture, enforcing symmetric left nd right hed vergence ngles to remove redundncy, the feture re-mesured, nd the process iterted to given tolernce. Mking mesurements t fition reduces dependency on knowledge of the cmer focl lengths. he imge ptch intensity vlues of the new feture re sved, so tht ppernce mtching is possible lter, nd the feture is inserted into the mp with uncertinty derived s in Section II. Note tht this uncertinty depends only on the geometricl loction of the feture, nd not on its imge chrcteristics: we ssume tht imge mtching (see Section III-B) hs constnt uncertinty in imge spce; tht is to sy tht how ccurtely prticulr feture cn be locted in the imge does not depend on its ppernce. In our work, s in [4], no ttempt is mde to discern good or bd fetures, such s those corresponding to reflections or lying t depth discontinuities (such s those seen in the rther pthologicl emples of Figure (, b)), or those which re frequently occluded, t the detection stge: the strtegy used is to ccept or reject fetures depending on how well they cn be trcked once the robot hs strted moving. Ptches which do not ctully correspond to sttionry, point fetures will quickly Scene Feture Fig. 3. he epected visibility of feture is clculted bsed on the difference in distnce nd ngle between the viewpoint from which it ws initilly seen nd tht from which the current mesurement is to be mde. look very different from new viewpoint, or will not pper in the position epected from the vehicle motion model, nd thus mtching will fil (this is lso the cse with frequently occluded fetures which re soon hidden behind other objects. hese fetures cn then be deleted from the mp, s will become clerer in our discussion of eperiments lter: while the initil choice of fetures is unplnned nd rndom, the best fetures survive for long periods nd become persistent lndmrks. B. Serching For nd Mtching Fetures Applying the feture detection lgorithm to the entire imge is required only to find new fetures. Since we propgte full informtion bout the uncertinty present in the mp, whenever mesurement is required of prticulr feture, regions cn be generted in the left nd right imges within which the feture should lie with some desired probbility (usully 3 stndrd devitions from the men). ypicl serch ellipses re shown in Figure (c,d). Mtching within these regions is then chieved by bruteforce correltion serch, using normlised sum-of-squreddifferences, for the best mtch to the sved feture ptch within the (usully reltively smll) regions defined by the serch ellipses in both left nd right imges. A consistency check is then pplied between the two imge loctions found (tking ccount of the epipolr coupling between the two mesurements): this gives some robustness ginst mismtches. Normlised sum-ofsqured-differences gives the mtching firly lrge degree of robustness with respect to chnging light conditions, nd in eperiments hs ment tht the sme fetures could be mtched well over the durtion of eperiments of mny minutes or few hours, though we hve not tested the durbility of mtching under etreme chnges (from nturl to rtificil lighting, for emple). Figures (e,f) show mtches obtined of some fetures, giving n impression of the surprising rnge of viewpoints which cn be mtched successfully using the lrge ptch representtion of fetures. However, clerly mtching cn only be epected to succeed for moderte robot motions, since the ptch representtion is intrinsiclly viewpoint-vrint fetures look different when viewed from new distnces or ngles (to void drift, we do not updte feture templtes fter mtching). herefore, we hve defined criterion for epected mtchbility bsed on the difference between the viewpoint from which the feture ws initilly seen nd new viewpoint. Figure 3 shows simplified horig β h

6 M M M Á < cut-through of the sitution: }ķ¹ º» is the vector from the hed centre to the feture when it ws initilised, nd is tht from the hed centre t new vehicle position. he feture is epected to be visible if the length rtio M is close enough to ) nd the ngle dif- is close enough to (in prctice less thn Ç È in mgnitude); the mtches shown in Figures (e,f) re close to these limits of viewpoint chnge. In our loclistion lgorithm, we re in position to estimte both of these vectors before mesurement is mde, nd so ttempts re mde only to mesure fetures which re epected to be visible. he result is region of the robot s movement spce defined for ech feture from which it should be ble to be seen. A feture which fils to mtch regulrly within this region should be deleted from the mp, since the filures must be due to it being n essentilly bd feture in one of the senses discussed bove rther thn due to simple viewpoint chnge. v¼¾½/ À (in prctice between something like Á  nd ference à 9@9.rķ¹ º» kä 9JÅ ÆÅ/Å ķ¹ º»'Å J {ƒ n C. Filure Modes wo filure modes were observed in our EKF-bsed SLAM system. he first rises from filure of dt ssocition: mismtches re likely to hppen when robot nd feture uncertinty grow nd serch regions (Figures (c,d)) become very lrge (for instnce, of width in the region of piels rther thn the more norml piels). In this sitution, there is chnce tht n imge region of similr ppernce to mpped feture is incorrectly identified, nd this filure cnnot be identified by norml mesurement gting. In this work, we did not implement multiple hypothesis frmework, nd therefore single mismtch could prove to be ftl to the loclistion process. However, mismtches were ctully very rre: firstly, the lrge sie of imge ptch used to represent feture ment tht mtching gve very few flse-positives within the uncertintybounded serch regions (which implicitly impose the eplicit consistency checks, bsed on multi-focl tensor for emple, included in most structure from motion systems). More importntly though, the ctive mesurement selection nd mpmngement pproches used ment tht t ll times ttempts were mde to keep uncertinty in the consistency of the mp to minimum. In norml opertion, imge serch regions were smll, nd the chnce of mismtches low. For this reson, long periods of error-free loclistion were possible. Nevertheless, in future systems there is cler need for n eplicit pproch to multiple hypotheses. he second, much rrer, filure mode rose from nonlinerities. When uncertinty in the mp is lrge, mesurements with lrge innovtion my led to unpredictble EKF updtes due to the unmodelled non-linerity in the system. IV. SYSEM VERIFICAION AGAINS GROUND RUH o evlute the loclistion nd mp-building ccurcy of the system in controlled environment, the lbortory floor ws mrked with grid (to enble mnul ground-truth robot position mesurements), nd rtificil scene fetures were set up in known positions eqully spced in line long the bench top (Fig. 4()). he robot s motion ws controlled interctively in this eperiment by humn opertor, who lso mnully in- () Eperimentl Setup (c) Return Journey (b) Outwrd Journey (d) Feture Re-found Fig. 4. Eperiment with rtificilly introduced fetures. Eperimentl rrngement. Estimted positions of the robot ( Ê Ë É ) nd fetures (É ÌÍ ) in grey, long with ÎÏ ellipses for the point covrinces ÐÑ Í Ñ Í, re shown superimposed on the true positions (from mnul mesurement) in blck s the robot moved forwrd nd bck. he feture spcing ws 4cm, nd the robot moved bout 5m from its origin. Feture lbels - show the order they were trcked in. (As ever with stereo, the mjor is of the uncertinty ellipse lies long the Cyclopen direction nd so here the hed ws viewing on verge perpendiculr to the direction of trvel.) dicted (by highlighting imge interest regions vi the mouse) which fetures the robot should initilise into its mp. Strting from the grid origin with no prior knowledge of the loctions of scene fetures, the robot ws driven nominlly stright forwrd. Every second feture in the line ws fited nd trcked for short while on this outwrd journey, the robot stopping t frequent intervls so tht mnul ground-truth mesurements could be mde of its position nd orienttion using n on-bord lser pointer. he recovered vlues nd uncertinties in the positions of fetures 5 re shown in grey in Fig. 4(b), superimposed on the mesured ground truth in blck. he effects of drift re pprent, nd the uncertinties hve incresed stedily. he robot ws then reversed bck down the corridor, nd mde to fite upon the lternte fetures it hd not used previously. he im ws tht it should return to its origin while lwys trcking only recently cquired fetures, s would be cse in looped movement round rectngulr lyout of corridors for emple. As epected, the nd uncertinty continued to increse (Figure 4(c)), nd by its return to the nominl origin the filter estimted the robot s position s : v m, ; Ò Ó m, {Ô rd, wheres the true position ws : 7 m, ; Ò Ó < m, Ò Ó rd. At this stge, following one more movement step, the robot

7 Þ â Þ Þ Ä Ö ws llowed to re-fite feture, which it hd seen much erlier t the strt of the eperiment. As cn be seen in Fig. 4(d), drift nd uncertinty re immeditely reduced, both in the robot stte nd scene geometry, prticulrly ner the re-fited feture. he estimted position of : Õ Ö m, ; Õ mç < m, vô rd ws now much closer to the true position : ÖvØ m, ; Ù Ó < m, Ù v rd. he robot stte covrince %&% reduced shrply fter refition from Ú3Û&Ü ÛÛ Û&Ü ÛÛ Û&Ü ÛÛ ÚäÛ&Ü ÛÛ Û&Ü ÛÛÛ-âèÛ&Ü ÛÛ Û&Ü ÛÛ ÎÝßÞ Û&Ü ÛÛ Ýà ÎáßÞ Û&Ü Û-â Ýà Û&Ü Û Îá Û&Ü Û á&ãpþ âäû&ü ÛÛã@Î ã@î à&ã åaæ Û&Ü ÛÛÛ-âèÛ&Ü ã@áçþ ÛÛÛé Û&Ü ÛÛÛ-â ã@á Û&Ü ÛÛ Û&Ü ÛÛÛ-âèÛ&Ü ÛÛ ã@áçþ ã@êëå It cn be seen tht resonble degree of uncertinty still remins: this is due to the fct tht single mesurement, even of feture with very smll position uncertinty, does not fully constrin the robot s position estimte further re-fitions of other fetures providing complementry informtion will llow the robot s position to be relly locked-down (s will be eplined in more detil in Section V-A). By mintining full covrince informtion, uncertinty grows s function of ctul distnce from known position here the origin, where the coordinte frme ws defined t the robot s strting position not s function of the totl distnce trvelled by the robot from the known point. he drift still seen in the uncertinty in distnt fetures is fundmentl limittion of ny mp-building sitution involving the use of sensors with limited rnge: the loctions of these fetures reltive to the world coordinte frme must be estimted by implicit compounding of mny noisy mesurements nd uncertin robot motions. V. OOLS FOR AUONOMOUS NAVIGAION he previous eperiment ws contrived in tht the robot ws instructed which fetures to fite, nd how to nvigte. In this section we describe tools which combine to permit utonomous ctive SLAM, s will be demonstrted in the eperiments presented lter. First, in Sections V-A nd V-B, is method for performing the criticl role of ctively choosing which feture to fite upon t ech stge of nvigtion, both without nd with considertion of the time penlty involved with re-fition using mechnicl device. Net, in Section V-C we consider the mintennce of feture set, nd finlly in Section V-D discuss how to inject n element of gol-direction into the robot s progress. A. Active Feture Selection In our SLAM work, the gol is to build mp of fetures which ids loclistion rther thn n end result in itself. Nevertheless, in the combined nd coupled estimtion of robot nd feture loctions which this involves, estimtion of the robot position is not intrinsiclly more importnt thn tht of the feture positions: iming to optimise robot position uncertinty through ctive choices is misleding, since it is the overll integrity nd consistency of the mp nd the robot s position within it which is the criticl fctor. We hve lredy seen in the preceding eperiment tht robot position uncertinty reltive to world frme will increse with distnce trvelled from the origin of tht frme. It is the mutul, reltive uncertinty between fetures nd robot which is key. Our feture selection strtegy chieves this by mking mesurement t the currently visible feture in the mp whose position is hrdest to predict, n ide used in the re of ctive eplortion of surfce shpe by White nd Ferrie [5]. he vlidity of this principle seems cler: there is little utility in mking mesurement whose result is esy to forecst, wheres much is to be gined by mking mesurement whose result is uncertin nd revels something new. he principle cn lso be understood in terms of informtion theory, since mesurement which reduces widely spred prior probbility distribution to more peked posterior distribution hs high informtion content. Our pproch to mpping is ctive not in the sense of White nd Ferrie, who ctully control the movement of cmer in order to optimise its utility in sensing surfce shpe, in tht we do not choose to lter the robot s pth to improve mp estimtes. Rther, ssuming tht the robot trjectory is given, or provided by some other gol-driven process, we im to control the ctive hed s movement nd sensing on short-term tcticl bsis, mking choice between selection of currently visible fetures: which immedite feture mesurement is the best use of the resources vilble o evlute cndidte mesurements, we clculte predicted mesurements nd innovtion covrinces b for ll visible fetures (where feture visibility is clculted s in Section III- B). In mesurement spce, the sie of the ellipsoid represented by ech b is normlised mesure of the uncertinty in the estimted reltive position of the feture nd the robot, nd we wish to choose feture with the lrgest uncertinty. o produce sclr decision criterion, the volume ì>í in spce of the ellipsoid t the îï Ö{ð level is clculted for c er ech f visible feture (n importnt point here is tht in our implementtion the mesurement noise in the three mesurement components is c}er f ñ multiple of the identity mtri). Computing the eigenvlues š of b yields the volume š ì í A9òÇvó î ñ ñ ñ š We use this mesure ì í s our score function for compring cndidte mesurements: mesurement with high ì í is hrd to predict nd therefore dvntgeous to mke. We do not propose here tht ìsí is the optiml choice of criterion from n informtion-theoretic point of view nevertheless, we believe tht it will give results for mesurement comprison which re lmost identicl to n optiml criterion. he importnt point is tht since it is evluted in mesurement spce where the mesurement noise is constnt, its vlue reflects how much new informtion is to be obtined from mesurement nd does not priori fvour fetures which re for emple close or fr from the robot. An illustrtive emple is shown in Figure 5. With the robot t the origin, five well-spced fetures were initilised, nd the robot driven forwrd nd bckwrds while fiting on feture (chosen rbitrrily). he sitution t the end of this motion is shown in Figure 5(), t which time the five ì í vlues were

8 š Ä Ö F F š F c n š U U () (b) Fite (c) Fite (d) Fite Fig. 5. Selecting between fetures fter long period trcking one (ground-truth quntities in blck, estimtes in grey): in () the robot stops fter trcking feture. In (b), (c) nd (d), the estimted stte is updted fter further mesurements of fetures, nd respectively. he lrge improvement in the estimted robot stte in (c) nd (d) shows the vlue of mking mesurements of multiple fetures. evluted s: ìëí}9ò " "kó'"köe" Ç ô9ò vç " Ç{ " õó{ö " Ç ØE" Ç{ 3 + According to our criterion, there is little merit in mking nother mesurement of feture, nd feture should be fited insted, rther thn, 3, or 4. Note here tht ì í, being clculted in mesurement spce, does not necessrily fvour those fetures such s which hve lrge uncertinty in the world coordinte frme. Figures 5(b, c nd d) show the situtions which result if feture, or is fited for the net mesurement. Clerly mking the etr mesurement of feture in (b) does little to improve the robot position estimtion which hs drifted long the direction mbiguous to mesurements of tht feture. Using fetures or in (c) nd (d), however, show significnt improvements in robot loclistion: visully there is little to choose between these two, but the robot stte covrince fter fiting feture is smller: '%&%$ & (if fited) Ö vô Ö vô õó Ç Ø Ö Ø + '%&%H + + { Ö 4 (if fited) Ö Ó & + Ø he qulities of the ì í bove criterion become cler when we consider the cse of compring fetures immeditely fter they hve been initilised into the mp; this is sitution we will often fce s the robot moves into new re nd stops to find new fetures. In this cse if the just-initilised fetures re compred for immedite re-mesurement, we find tht they ll hve ectly the sme vlue of ì í : ì í K/ø ù_úsn i9 Çvó 9¾û Ó î š c e c}f his is n initilly surprising but desirble chrcteristic of ì í : wht hs hppened is tht in initilistion, one unit of mesurement noise hs been injected into the estimte of the position of ech feture reltive to the robot. When the innovtion covrince for re-mesurement is clculted, it hs vlue which is simply this plus one more unit of mesurement noise. We hve proven tht the ìsí criterion hs no priori fvouritism towrds fetures in certin positions. o split these identicl vlues, we need to use dditionl informtion: in this cse, the future heding direction of the robot. We predict the robot s position in smll mount of time, nd then evlute ì í for the new fetures bsed on this. he result is tht we cn choose the feture which we epect to give the most informtion bout the robot s future movement. In relity, wht hppens is tht the criterion will choose feture which will be viewed from significntly different spect from the future robot position: when we consider the elongted shpe of the mesurement noise in our system in Crtesin spce, it will choose feture where from the new position we re ble to mke mesurement whose covrince ellipse overlps minimlly with the feture s world uncertinty (typiclly by crossing it t lrge ngle). his feture provides the best informtion for reducing future motion uncertinty. B. Mesurement Selection During Motion he strtegy developed so fr considers mesurement choice when the robot is sttionry; however, it is not suitble for mking ctive choices ctully while the robot is moving, since it ll but demnds chnge in fition t every opportunity given to do so. his impertive to switch rises becuse mesuring one point feture does not fully constrin the robot s motion uncertinty is lwys growing in one direction or nother, but predominntly orthogonl to the current fition direction. his mens tht switches in fition re likely to be through round Ø È which my tke severl ms. In fition switching during motion, we must consider this time dely s penlty, since it could otherwise be spent in mking different mesurements. We first require bsis for deciding whether one estimted stte is better thn nother. Remembering tht totl mp integrity is wht is importnt, we suggest tht the highest ì>í found for ll visible fetures, ì í9 ü, is good indictor. If ì í 9òü is high, there is mesurement j ý which needs to be mde j ý urgently, indicting tht the stte estimte is poor. Conversely, if ìsí}9òü is low the reltive positions of ll visible fetures re known well. jmý he steps then followed re:. Clculte the number of mesurements which would be lost during sccde ( rpid re-direction of fition direction) to ech of the visible fetures. his is done by estimting the time which ech hed is would need to move to the correct position, tking the lrgest (usully the pn time since this is is the slowest), nd dividing by the inter-mesurement time intervl ( ms).. Identify ÿþ, the highest : this is the number of mesurements lost in the lrgest sccde vilble. 3. For ech feture R, mke n estimte of the stte fter þ of mking filter prediction steps followed by Hþ steps if n immedite sccde to it is initited. his consists

9 9e-5 Innovtion Covrince Volume 8e-5 7e-5 e-5 5e-5 4e-5 3e-5 Feture Feture Feture Feture 3 Innovtion Covrince Volume.7e-5 5.9e-5 5.e-5 4.3e-5 3.5e-5 Feture Feture Feture Feture 3 e e Step Number () () (b) (b) Step Number Fig.. he innovtion covrince volume vlues nd fition switching () s the robot moves forwrd in the region of 4 newly-initilised fetures shown in (). Ech line, representing one feture, drops shrply s tht feture is mesured nd its uncertinty decreses. Lter in the run (e.g. ner step 9) etended fition on one feture becomes preferble to rpid switching. A generl downwrd trend shows continuous improvement in estimtes. Prts (b) nd (b) show the sme for longer second run, where the geometry is better known from the strt. Now low vlues for ll fetures re mintined predominntly by long periods trcking one feture. Chnges in behvior re seen when feture goes out of view t step, feture t step 47 nd finlly fetures nd 3 t step 7, fter which ll vlues grow without bound. simulted prediction/mesurement updtes. A mesurement is simulted by updting the stte s if the feture hd been found in ectly the predicted position (it is the chnge in covrince which is importnt here rther thn the ctul estimte). An estimted stte fter the sme number of steps is lso clculted for continued trcking of the currently selected feture. 4. For ech of these estimted sttes, ì í 9òü is evluted. he sccde providing the lowest ì>í9òü is jmý chosen for ction; or trcking stys with the current feture j ý if tht ì>í}9 ü is lowest. j ý Figure shows n eperiment into continuous fition switching: four fetures were initilised, nd () shows the robot s trjectory s it strted to move forwrd, choosing which fetures to fite on s described bove. In (), the vlues obtined from stright ì í comprison of the four fetures t ech time step re plotted. he four lines show how uncertinties in the positions of the fetures reltive to the robot vry with time. As would be hoped, there is generl downwrd trend from the initil stte (where ll the fetures hve ì í ì í Kø+ù_ú N s eplined erlier), showing tht the positions re becoming more nd more certin. In the erly stges of the motion, fition switches s rpidly s possible between the four fetures: only one mesurement t time is mde of ech feture before ttention is shifted to nother. In the grph of Figure (), mesurement of prticulr feture ppers s shrp drop in its ì í vlue. While feture is being neglected, its ì í grdully creeps up gin. his is becuse the newly-initilised fetures hve lrge nd uncoupled uncertinties: their reltive loctions re not well known, nd mesuring one does not do much to improve the estimte of nother s position. After while, the feture sttes become more coupled: round step 4, cler ig-gs in the rising curves of neglected fetures show tht the uncertinties in their positions reltive to the robot re slightly reduced when mesurement is mde of nother feture. At round step 8, the first cler sitution is seen where it becomes preferble to fite one feture for n etended period: feture is trcked for bout steps. his feture is very close to the robot, nd the robot is moving towrds it: mesurements of it provide the best informtion on the robot s motion. Since the loctions of the other fetures re becoming better known, their positions reltive to the robot re constrined quite well by these repeted mesurements (only gentle rise in the lines for fetures, nd 3 is seen during this time). Feture ctully goes out of the robot s view t step (the robot hving moved too close to it, violting one of the visibility criteri), nd behvior returns to quite rpid switching between the other fetures. he robot ws stopped t the end of this run with stte estimtes intct. It ws then driven bck to ner the origin in step-by-step fshion, mking further dense mesurements of ll of the fetures long the wy. he result ws tht once it ws bck t its strting point, feture estimtes hd been very well estblished. It ws from this point tht second continuous switching run ws initited: the trjectory nd the now ccurtely estimted feture positions re shown in Figure (b), nd grph of the feture comprison in (b). his second grph is drmticlly different from the first: in the erly stges, low ì í vlues for ll the fetures re now mintined by etended periods of trcking one feture (feture gin). he strong coupling now estblished between feture estimtes mens tht if the robot position reltive to one cn be well estimted, s is the cse when the nicely plced feture is trcked, its position reltive to the others will be s well. here is the occsionl jump to nother feture, ppering s spikes in the trces t round steps 7 nd 9. Just fter step, feture goes out of view, nd period of rpid switching occurs. None of the remining fetures on its own provides especilly good overll robot position informtion, nd it is necessry to mesure them in turn. Feture goes out of view (due to too lrge chnge in viewing ngle) t step 47. After this, only the distnt fetures nd 3 remin for mesurements. It is noticeble tht throughout the grph these two hve been locked together in their ì í vlues: mesurements of them provide very similr informtion

10 due to their proimity, nd there is little need to switch ttention between them. hese fetures finlly go out of view t bout step 7, leving the robot to nvigte with odometry only. A further eperiment ws performed to investigte the effect of using hed with lower performnce. Softwre velocity limits were introduced, incresing the hed s time to complete sccdes by some 3%. Runs were mde with both fst nd slow performnces. wo distnt fetures (fetures nd 3 in the previous eperiment) were initilised from the origin nd the robot drove stright forwrd, switching ttention between them. he results were s one would nticipte. he fst hed ws ble to keep the errors on both points of similr sie, nd continued to switch fition t constnt rte throughout the run. he slow hed ws less ble to keep the error rtio constnt nd, lter in the run when the feture estimtes were well coupled, the rte of switching fell. he lrger penlty of slower sccdes ment tht it ws worthwhile trcking one feture for longer. C. Automtic Mp Growing nd Pruning Our mp-mintennce criterion ims to keep the number of relible fetures visible from ny robot loction close to vlue determined by the specifics of robot nd sensor, the required loclistion ccurcy nd the computing power vilble: in this work, the vlue two ws chosen, becuse mesurements of two widely-spced fetures re enough to produce fullyconstrined robot position estimted. Fetures re dded to the mp if the number visible in the re the robot is pssing through is less thn this threshold: the robot stops to detect nd initilise new fetures in rbitrrily chosen, widely-spced viewing directions. his criterion ws imposed with efficiency in mind it is not desirble to increse the number of fetures nd dd to the computtionl compleity of filtering without good reson nd the gin in loclistion ccurcy from dding more fetures thn this minimum would not be gret. However, in future work it my be useful to ensure tht one or two fetures more thn the minimum re lwys visible to ensure tht dding new fetures does not hppen too lte nd the robot is not ever left in position with less thn the minimum vilble. A feture is deleted from the mp if, fter predetermined number of detection nd mtching ttempts when the feture should be visible, more thn fied proportion (in our work 5%) re filures. his is the criterion which prunes the bd fetures discussed in Section III-A. In our current implementtion, there is no rule in plce to ensure tht the scene objects corresponding previously deleted fetures (which re of interest to the feture detection lgorithm despite their unsuitbility s long-term lndmrks) re not cquired gin in the future, but in prctice this ws rre due to the fct tht the robot rrely psses long ectly the sme route twice. It should be noted tht degree of clutter in the scene cn be delt with even if it sometimes occludes lndmrks. As long s clutter does not too closely resemble prticulr lndmrk, nd does not occlude it too often from viewing positions within the lndmrk s region of epected visibility, ttempted mesurements while the lndmrk is occluded will simply fil nd not led to filter updte. he sme cn be sid for moving clutter, such s people moving round the robot, who sometimes Fig. 7. Imge sequence obtined from continuous fition trcking of feture while following n voidnce pth generted by biologiclly-inspired control lw. occlude lndmrks few missed mesurements re not big issue. Problems only rise if mismtches occur due to similrity in ppernce between clutter nd lndmrks, nd this cn potentilly led to ctstrophic filure. he correct opertion of the system relies on the fct tht in most scenes very similr objects do not commonly pper in close enough vicinity to lie within single imge serch region (nd specil steps would need to be tken to enble the system to work in scenes with lot of repeted teture). D. Gol-directed nvigtion he purpose of this pper is to build mp which ids loclistion rther thn one dense enough to be useful for identifying free spce. Nevertheless, this loclistion method could form prt of complete system, where n dditionl module (visul or otherwise) could perform this role nd communicte with the loclistion system to lbel some of its fetures with contetul informtion, such s this is feture t the left-hnd side of n obstcle. In n erlier pper [] we showed how fition could be used to steer vehicle towrds nd then round fited wypoint nd then on to the net wypoint. he method produces steering outputs similr to those of humn drivers [7]. In Figure 7 we show n imge sequence obtined from one of the robot s cmers in period of fition trcking of certin mp feture, nd the pth followed by the robot during such mneuvre. Section VIII shows how this type of behviour cn be incorported into the mpping system. VI. AUOMAIC POSIION-BASED NAVIGAION With utomtic feture-selection, mp mintennce nd goldirected steering, the robot is in position to perform utonomous position-bsed nvigtion. A trjectory is specified s sequence of wypoints in the world coordinte frme through which the robot is desired to pss. he robot moves in steps of pproimtely two seconds durtion. Before ech step, the feture selection lgorithm of the previous section chooses the best

11 " b c d e 5 4 Fig. 8. Frmes from video of the robot nvigting utonomously up nd down the corridor where the ctive hed cn be seen fiting on vrious fetures, nd fited views from one of its cmers of some of the first 5 fetures initilised. he gridded floor ws n id to mnul ground-truth mesurements nd ws not used by the vision system () () () (3) (44) (77) (4) (79) Fig. 9. Numbered steps in utonomous nvigtion up nd down corridor. Grey shows the estimted loctions of the robot nd fetures, nd blck (where mesured) the true robot position. he furthest fetures lie t ê m. feture to trck during the movement, nd this feture is trcked continuously during movement (t rte of 5H, mking mesurements nd the sme number of filter prediction/updte steps per movement step). he robot stops for short period between movement steps to mke gross fition chnge to nother feture. he breks in movement re lso used to utomticlly dd fetures to or delete them from the mp s necessry. As the robot drives, mking mesurements of the chosen feture nd updting the loclistion filter, the steering ngle is continuously set to the pproprite vlue to rech the net wypoint. In the follow eperiment, the instructions given to the robot were to hed in sequence from its strting point t 9 : ; 9ò " gin (in metre units). his eperiment ws designed to prove gin the system s bility to return to previously visited re nd recognise it s such, but now using mp which ws generted nd mintined completely utomticlly. (he etr wypoint 9ò " Ç ws specified merely to ensure tht the robot turned in wy which did not sng its umbilicl cble.) he robot s progress is shown in Figure 8, long with views from the left cmer of some of the first 5 fetures inserted into the mp, which itself is shown t vrious stges in Figure 9. On the outwrd journey the sequence of fetures fited in the erly stges of the run (up to step ()) ws,,,,, to the wypoints 9 " Ç, 9ò ", nd finlly bck to 9ò "

12 ; " ", 3, 5, 4, 7,, 8, 3,, 8, 7, 3, 7, 8, 3, 9 we see frequent switching between certin set of fetures until some go out of visibility nd it is necessry to find new ones. Fetures 4 nd 5 did not survive pst very erly mesurement ttempts nd do not pper in Figure 9. Others, such s, nd 4 proved to be very durble, being esy to see nd mtch from ll positions from which they re epected to be visible. It cn be seen tht mny of the best fetures found lie ner the ends of the corridor, prticulrly the lrge number found on the furthest wll ( 5, etc.). he ctive pproch relly comes into its own during shrp turns such s tht mde round step (44), where using the full rnge of the pn is fetures such s these could be fited while the robot mde turn of 8È. he ngle of turn cn be estimted ccurtely t time when wheel odometry dt is prticulrly unrelible. At step (77) the robot hd reched the finl wypoint nd returned to its strting point. he robot successfully re-mtched originl fetures on its return journey, in prticulr feture. he robot s true position on the grid compred with the estimted position ws ( Ù9 : < being given in metre nd rdin units): çi9ò v " Ó"ÖE { " ]i9ò & " Ö " Ó' ØvØ o verify the usefulness of the mp generted, the eperiment ws continued by commnding the robot to repet the round trip. In these further runs, the system needed to do little mp mintennce of course ll mesurements dd to the ccurcy of the mp, but there ws little need to dd to or delete from the set of fetures stored becuse the eisting set covered the re to be trversed well. At (, ), step (4) the veridicl nd estimted positions were i9 {Ô " Ó " Ó " ]ô9¾ Ô Ö'" Ó " Ó nd on return to the origin, fter totl trip of 4m, i9ò ö" ö{" Ö Ö " A9 +Ô " v " A plesing spect of the feture choice criterion described erlier is its inbuilt pressure to crete tightly known nd globlly consistent mps. Becuse uncertinty in the robot s position reltive to erlier-seen fetures epnds during the period of neglect, the criterion mkes them prime cndidtes for fition s soon s they become visible gin; re-registrtion with the originl world coordinte frme, in which the loctions of these erly fetures is known well, hppens s mtter of course. ÖE v VII. INCORPORAING SPARSE PRIOR KNOWLEDGE he fundmentl limittion of SLAM tht s the robot moves further from its fudicil strting point, position estimtes reltive to the world frme become incresingly uncertin, cn be mitigted in mny rel ppliction domins if there re some visul lndmrks which re in positions known in dvnce. Idelly, they would be distributed uniformly round the mpped re. hey must lso be visully distinguishble from other fetures which could, within the growing uncertinty bounds, be mistken for them: however, this cn more esily be chieved with these hnd-picked fetures then those detected utonomously by (7) (37) (55) (98) Fig.. Automtic position-bsed nvigtion with 3 known fetures (, nd ). High loclistion ccurcy cn now be chieved over wider rnge of robot movement. the robot. here hve mny pproches to robot loclistion using lndmrks in known loctions: when mp is given in dvnce, the loclistion problem becomes reltively simple [4]. Here however we wish to show tht smll number of nturl visul lndmrks (smll in the sense tht there re not enough to permit good loclistion using only these lndmrks) cn be esily integrted into the SLAM frmework to improve loclistion. he lndmrk s known loction is initilised into the estimted stte vector s the coordintes of feture R t the strt of the run (i.e., s though it hd lredy been observed) nd its covrince ( s ( s is set with ll elements equl to ero, long with the cross-covrinces between the feture stte nd tht of the robot nd other fetures. In prediction nd mesurement updtes, the filter hndles these perfectly known lndmrks just like ny other feture. Note however tht uncertinty in lndmrk s reltive position will grow s the robot moves before observing it, nd so the ì í criterion will, s ever, mke the lndmrk desirble to look t. When there re perfectly known fetures in the mp, it is these which define the world coordinte frme, rther thn the rbitrry definition of this frme t the robot s strting position used before. herefore, in this eperiment the robot s position ws initilised with strting uncertinty not equl to ero: n ssessment ws mde of the uncertinty in robot loction nd orienttion reltive to the known lndmrks (with stndrd devition of the order of few centimetres nd degrees) nd this formed the initil %+%. Note too tht s well s perfectly known lndmrks, it would be strightforwrd to introduce lndmrks in prtilly known positions (i.e. with some uncertinty) into this frmework. An eperiment ws conducted where the robot mde movement similr to tht in the utonomous nvigtion eperiment presented erlier, but now with 3 known fetures inserted the mp before it set out. hese ly to one side of the corridor, nd re lbelled s, nd in the pictures of Figure showing the progress of the eperiment. In just the sme wy tht in the previous eperiment the utomtic feture-choice criterion selected fetures not mesured for long time whenever possible,

13 " " " " " 3 Window y Approimte Position of Light () (b) Fig.. A virtul reflected feture: 3 is reflection in window of n overhed light. Its position in the mp lies outside of the lbortory, but it still cts s stble lndmrk. in this eperiment the known fetures were selected s soon s they becme visible, showing the drift which ws occurring in the robot s estimtion reltive to the world frme. he benefit of the known fetures ws to improve world-frme loclistion ccurcy when the robot ws long wy from its origin. At step (37), when the robot ws t it frthest distnce from the origin, its ground-truth loction ws mesured. he true nd estimted loctions were ]ô9¾ Ô Ö'" E " E " A9 Ô " " Ó nd the covrince mtri n order of mgnitude smller thn tht chieved erlier. It cn lso be seen tht the nturl fetures initilised close to the lndmrk re now more certin: the fetures t the fr end of the corridor (high : ) in Figure hve much smller ellipses thn those in Figure 9. A lterl slice through 3D mp recovered in this eperiment (Figure ()) revels curiosity the use of virtul reflected feture. he eperiment ws crried out t night under rtificil lighting, nd s the robot returned to its strting position it inserted the reflection of one of the ceiling lights into the mp s feture 3. Lndmrk is Obstcle A t 9 : ; A9¾ m " m Lndmrk is Obstcle B t 9 : ; A9 ö v " Ó' &. Go forwrd to wypoint 9 : ; A9 Ó' ".. Steer round Obstcle A, keeping to the left. 3. Steer round Obstcle B, keeping to the right. 4. Go forwrd to wypoint 9 : ; A9 Ô " Ö. 5. Stop. In this eperiment steering round the known obstcles took plce on positionl bsis the robot steered so s to void the known obstcles bsed on its current position estimte, even before it hd first mesured them. he utomtic feture-selection criterion decided when it ws necessry ctully to mesure the known fetures, nd in the eperiments this proved to be s soon s they becme visible, in order to lock the robot position estimte down to the world frme. he results re shown in Figure, where the estimted trjectory generted is pictured net to stills from video of the robot. he point when first mesurement of known feture is mde cn be clerly seen in Figure s smll kink in the robot trjectory: ctully mesuring the feture corrected the robot s drifting position estimte nd ment tht the steering ngle ws chnged slightly to correct the pproch. After this, the obstcle feture ws fited on only when it gin becme the best mesurement to mke. Otherwise, ttention ws pid to improving the mp of utomticlly-cquired fetures. IX. CONCLUSIONS VIII. ADDING CONEX O A MAP Well-locted visul lndmrks spred through the scene llow the robot to remin true to the world coordinte frme over wider re, mking nvigtion by specifying wypoints vible. But it is lso likely tht fetures, whether those supplied to the robot mnully or detected utomticlly, lso hve contetul mening, nd cn hve lbels ttched such s feture is point on the edge of n obstcle region or... is the door jmb. his informtion could be ttched by humn opertor or supplied by nother visul process. o illustrte the use of ll the techniques developed in this pper for utonomous loclistion nd nvigtion while mpbuilding, the loctions of just two lndmrks t the corners of ig-g pth were given to the robot, long with instructions to steer to the left of the first nd to the right of the second on its wy to finl loction using the following pln: We hve shown tht n ctive pproch is the device which permits vision to be used effectively in simultneous loclistion nd mp-building for mobile robots, nd presented fully utonomous rel-time implementtion. Our use here of ctive vision for nvigtion differs fundmentlly from tht eplored by Sndini nd coworkers [8] [9] [3] whose emphsis ws on n ctive pproch to recovering free spce by computing time to contct from the evolution of disprity nd motion prll. heir representtion ws dense rther thn sprse. he pproch here lso differs from our erlier work where we utilised n ctive hed for nvigtion tsks such s steering round corners nd long winding rods []. Our results indicte tht ctive fition hs prt to ply not only in short-term or tcticl nvigtion tsks, but lso in strtegic tsks where informed visul serch is required. From this position, visul nvigtion reserch cn join with tht progressing using other sensor types nd move towrds solving the remining problems in the burgeoning field of se-

14 Fig.. he estimted trjectory nd frmes cut from video s the robot nvigted utonomously round two known lndmrks nd out of the lbortory door. he nvigtion knew the loctions of fetures nd s prior knowledge, long with informtion on their sttus s obstcles. quentil mp-building. It is lso hoped tht by introducing the problems of robot mp-building to reserchers in visul reconstruction, insights cn be gined into the methodology which will be needed to construct structure from motion systems which cn operte in rel time, the first emples [3] of which hve just strted to pper. REFERENCES [] R. Smith, M. Self, nd P. Cheesemn, A stochstic mp for uncertin sptil reltionships, in 4th Interntionl Symposium on Robotics Reserch, 987. [] C. G. Hrris nd J. M. Pike, 3D positionl integrtion from imge sequences, in Proc. 3rd Alvey Vision Conference, Cmbridge, 987, pp [3] N. Ayche, Artificil Vision for Mobile Robots: Stereo Vision nd Multisensory Perception, MI Press, Cmbridge MA, 99. [4] H. F. Durrnt-Whyte, Where m I A tutoril on mobile vehicle loclition, Industril Robot, vol., no., pp., 994. [5] C. G. Hrris, Geometry from visul motion, in Active Vision, A. Blke nd A. Yuille, Eds. MI Press, Cmbridge, MA, 99. [] P. A. Berdsley, I. D. Reid, A. Zissermn, nd D. W. Murry, Active visul nvigtion using non-metric structure, in Proceedings of the 5th Interntionl Conference on Computer Vision, Boston. 995, pp. 58 5, IEEE Computer Society Press. [7] J.-Y. Bouget nd P. Peron, Visul nvigtion using single cmer, in ICCV5, Los Almitos, CA, 995, pp. 45 5, IEEE Computer Society Press. [8] M. Pollefeys, R. Koch, nd L. Vn Gool, Self-clibrtion nd metric reconstruction in spite of vrying nd unknown internl cmer prmeters, in Proceedings of the th Interntionl Conference on Computer Vision, Bomby, 998, pp [9] P. H. S. orr, A. W. Fitgibbon, nd A. Zissermn, Mintining multiple motion model hypotheses over mny views to recover mtching nd structure, in Proceedings of the th Interntionl Conference on Computer Vision, Bomby, 998, pp [] H. F. Durrnt-Whyte, M. W. M. G. Dissnyke, nd P. W. Gibbens, owrd deployments of lrge scle simultneous loclistion nd mp building (SLAM) systems, in Proceedings of the 9th Interntionl Symposium of Robotics Reserch, Snowbird, Uth, 999, pp. 7. [] K. S. Chong nd L. Kleemn, Feture-bsed mpping in rel, lrge scle environments using n ultrsonic rry, Interntionl Journl of Robotics Reserch, vol. 8, no., pp. 3 9, Jnury 999. [] S. hrun, D. Fo, nd W. Burgrd, A probbilistic pproch to concurrent mpping nd loclition for mobile robots, Mchine Lerning, vol. 3, 998. [3] J. A. Cstellnos, Mobile Robot Loclition nd Mp Building: A Multisensor Fusion Approch, Ph.D. thesis, Universidd de Zrgo, Spin, 998. [4] J. J. Leonrd nd H. J. S. Feder, A computtionlly efficient method for lrge-scle concurrent mpping nd loclition, in Robotics Reserch. Springer Verlg,. [5] A. J. Dvison nd D. W. Murry, Mobile robot loclistion using ctive vision, in Proceedings of the 5th Europen Conference on Computer Vision, Freiburg, 998, pp [] S. K. Nyr, Ctdioptric omnidirectionl cmer, in Proceedings of the IEEE Conference on Computer Vision nd Pttern Recognition, 997. [7] A. J. Dvison nd N. Kit, Active visul loclistion for cooperting inspection robots, in In Proceedings of the IEEE/RSJ Conference on Intelligent Robots nd Systems,. [8] J. G. H. Knight, A. J. Dvison, nd I. D. Reid, Constnt time SLAM using postponement, in Proceedings of the IEEE/RSJ Conference on Intelligent Robots nd Systems,.