FastSLAM with Stereo Vision

Transcription

1 FasSLAM wih Sereo Vision Wikus Brink Elecronic Sysems Lab Elecrical and Elecronic Engineering Sellenbosch Universiy Corné E. van Daalen Elecronic Sysems Lab Elecrical and Elecronic Engineering Sellenbosch Universiy Willie Brink Applied Mahemaics Deparmen of Mahemaical Sciences Sellenbosch Universiy Absrac We consider he problem of performing simulaneous localizaion and mapping (SLAM) wih a sereo vision sensor, where image feaures are mached and riangulaed for use as landmarks. We explain how we obain landmark measuremens from image feaures, and describe hem wih a Gaussian noise model for use wih a Rao-Blackwellized paricle filer-based SLAM algorihm called FasSLAM. This algorihm uses paricles o describe uncerainy in robo pose, and Gaussian disribuions o describe landmark posiion esimaes. Simulaion and experimenal resuls indicae ha FasSLAM is well suied for visionbased SLAM, because of an inheren robusness o landmark mismaches, and we achieve accuracies ha are comparable o oher sae-of-he-ar sysems. I. INTRODUCTION Simulaneous localizaion and mapping (SLAM) is a rapidly growing par of he auonomous navigaion field. SLAM aemps o solve he problem of esimaing a mobile robo s posiion in an unknown environmen while building a map of he environmen a he same ime. This is a challenging problem since an accurae map is necessary for localizaion and accurae localizaion is necessary for mapping. Mos SLAM algorihms use a probabilisic landmark-based map raher han a dense map. If landmarks in he map can be measured, relaive o he robo, and racked over ime he pose of he robo and he locaions of he landmarks can be esimaed in an opimal manner. Iniial implemenaions made use of he exended Kalman filer (EKF), bu displayed several shorcomings such as quadraic complexiy and sensiiviy o incorrec feaure racking [1] []. The paricle filer can be used o overcome hese limiaions. However, because of he high dimensionaliy of he problem he paricle filer canno be used direcly. Insead, he Rao-Blackwellized paricle filer [3] is used. This filer esimaes some saes wih paricles and ohers wih EKFs. In he case of SLAM paricles are used for he pose of he robo and an EKF for each landmark. This mehod is called FasSLAM and has shown promising resuls in he lieraure [4] [5]. Sereo vision is an aracive sensor o use wih SLAM as i can provide a large amoun of 3D informaion a every ime sep. Exracing ha informaion reliably can, however, be challenging. Powerful algorihms such as SIFT [6] or SURF [7] have been used o solve his problem by exracing salien feaures from images. These algorihms can be employed o rack feaures over muliple images so ha landmarks for SLAM can be idenified. In his paper we aemp o solve he D SLAM problem by using FasSLAM and image feaures (he 3D exension is concepually he same). We begin wih a brief descripion of how we obain measuremens of landmarks wih a Gaussian noise model. A deailed descripion of he FasSLAM algorihm is given, followed by some simulaions where we compare FasSLAM wih he popular EKF SLAM algorihm []. We provide experimenal resuls from our sysem on an oudoor daase and measure accuracy agains differenial GPS ground ruh. II. IMAGE FEATURES AND STEREO GEOMETRY In his secion we discuss a mehod of finding feaures in images, riangulaing hese feaures for use as landmarks and approximaing he noise associaed wih each measuremen of a landmark. This characerizaion of he sereo vision sensor is imporan for accurae opimal esimaion. Since his secion is similar o previous work, he explanaion will be brief. For a more in deph discussion refer o [8] and [9]. A. Feaure deecion and maching In order o idenify landmarks we op for one of wo popular feaure deecion algorihms: he scale-invarian feaure ransform (SIFT) [6] or speeded-up robus feaures (SURF) [7]. Noe ha since we perform SLAM in D we discard he verical coordinaes of image feaures. A every ime sep we search for feaure maches in a synchronized pair of recified sereo images. We model each mach as a measuremen wih Gaussian noise: xl z im = + N (, N ), (1) x R where x L and x R are he image coordinaes of he feaure in he lef and righ images. By N (, N ) we mean a sample drawn from he normal disribuion wih zero mean and covariance marix N (he same noaion is used hroughou he res of his paper). We describe he noise covariance in Equaion 1 by σ N = xl σx, () R wih σ xl and σ xr he sandard deviaions in pixels of he mach measuremen, which we obain hrough esing.

2 c L (x L, y L ) x r (x R, y R ) c R (a) camera geomery Fig. 1. z r X w ψ (x, y ) (b) robo geomery The geomery of our sysem. We can hen mach he descripors of a new measuremen wih he descripors of feaures already found a previous ime seps, o arrive a puaive landmark correspondences. B. Sereo geomery of calibraed images Now ha we have sereo image feaures ha can be racked over ime, we conver hem ino D landmarks. Figure 1(a) depics he geomery of a pair of sereo cameras wih camera cenres a c L and c R, where he image planes have been recified, and a landmark T x r z r observed a image coordinaes (x L, y L ) in he lef image and (x R, y R ) in he righ image. As menioned we are working in D, so he feaures are effecively projeced ono he X r Y r plane. Wih he geomery of he sereo camera pair, he landmark locaion in meres can be calculaed in robo coordinaes as fb xr x = L x R y (x r L p x)b + N (, Q x L x R b ), (3) where b is he baseline (disance beween c L and c R ), f he focal lengh and p x and p y he x- and y-offse of he principal poin, all obained from an offline calibraion process. Q is he noise covariance marix of he measuremen. Noe ha we differeniae beween robo coordinaes (subscrip r) and world coordinaes (subscrip w) as indicaed in Figure 1(b), where x, y and ψ are he robo s posiion and orienaion in world coordinaes a ime. We know ha a ransformaion from N o Q is possible if we have a linear sysem and, since Equaion 3 is no linear, we use a firs order Taylor approximaion o find he ransformaion marix ] W = [ xr x L x L x R x R I hen follows ha Q can be approximaed as Y w X r x r Y r. (4) Q = W N W T. (5) This approximaion is performed o mainain a Gaussian noise model, which is necessary for FasSLAM. We use his noise model and he riangulaed locaions of landmarks o find ouliers in puaive correspondences beween new measuremens and hose already in he map, according o he RANSAC-based probabilisic mehod discussed in [9]. From Figure 1(b) we see ha he robo pose can be described wih he sae vecor x = x y, (6) ψ wih x and y he locaion of he robo and ψ is orienaion. We define he roaion marix cos(ψ ) sin(ψ R = ). (7) sin(ψ ) cos(ψ ) In order o perform SLAM we need o esablish a relaionship beween robo and world coordinaes. We denoe he locaion of a landmark i in he map corresponding wih measuremen j a ime as xw xr m i, = and z j, =. (8) y w The measuremen z j, will always be as he robo observes he landmark in robo coordinaes, and he landmark s locaion m i, will always be given in world coordinaes. The ransformaion beween robo and world coordinaes is given by he measuremen equaion z j, = h(x, m i, ) = R T xw x, (9) y w y or inversely, m i, = h 1 (x, z j, ) = R [ xr ] + [ x y ]. (1) Exacly which measuremen corresponds o which landmark in he map, as mached wih he feaure descripors and confirmed wih he oulier deecion scheme, is sored in a correspondence vecor c. III. MOTION MODEL Now ha we have esablished a measuremen equaion, we need o derive a moion model for our robo so ha we can perform SLAM. We use he velociy moion model. A every ime sep he conroller of he robo will give i a forward and angular velociy, [ v ψ] u = + N (, M ), (11) wih v he forward ranslaional speed and ψ he angular velociy. To characerize he uncerainy we add zero mean Gaussian noise wih covariance marix α1 v M = + α ψ α 3 v + α 4 ψ, (1) as is common pracice [1]. The α parameers are robo and environmen specific, and have o be esimaed wih pracical esing and some degree of guesswork.

3 To updae he robo saes wih he conrol inpu we define he moion equaion as x = g(x 1, u ) = x 1 vt cos( y 1 + R ψt ) 1 vt sin( ψt ), ψ 1 ψt (13) wih T he sample period of he sysem. Alhough his is an approximaion, he accuracy los due o he approximaion is far smaller han he effec of expeced noise in he conrol inpu u. IV. SLAM WITH THE RAO-BLACKWELLIZED PARTICLE FILTER The paricle filer can be used o approximae any disribuion, and i is ofen uilized o accuraely esimae non- Gaussian sysems. A major drawback of he paricle filer, however, is ha wih high dimensional problems a large number of paricles is needed o describe he disribuion sufficienly. The Rao-Blackwellized paricle filer has been developed o overcome his problem [3]. This filer uses paricles o describe some saes and Gaussian disribuions o represen all oher saes. In order o uilize i we need o facorize he SLAM problem as p(x, m z 1:, u 1: ) = p(x z 1:, u 1: ) n p(m i z 1:, u 1: ). (14) Wih his facorizaion we describe he required poserior as a produc of n + 1 probabiliies. If we suppose ha he exac locaion of he robo is known, i is reasonable o assume ha he landmark posiions are independen from one anoher and can herefore be esimaed independenly. Naurally, we do no know he robo s locaion, bu his independence can be uilized when we use paricles o esimae he robo posiion. I can even be shown ha he above facorizaion is exac and no an approximaion [4]. FasSLAM uses a paricle filer o compue he poserior over robo saes, p(x z 1:, u 1: ), and a separae EKF for every landmark in he map o obain p(m i z 1:, u 1: ). Wha his means is ha, insead of only one filer, we facor he problem ino 1 + nm filers, where m is he number of paricles. The large number of filers may seem excessive, bu because of he low dimensionaliy of each individual filer he algorihm is remarkably efficien. We define every paricle o have a sae vecor for he robo saes, and a mean vecor and covariance marix for every landmark, as Y [k] = x [k] i=1, m [k] 1,, Σ[k] [k] 1,,..., m n,, Σ [k] n,, (15) wih x [k] he robo locaion and orienaion for paricle k, and m [k] i,, i, Σ[k] he i-h landmark s Gaussian mean and covariance. The FasSLAM algorihm, as i is execued a every ime sep, is given below in Algorihm 1. We proceed wih a sep by sep explanaion. Algorihm 1 FasSLAM(Y 1, u, z, c ) 1: for all paricles k {1,,..., m} do : x [k] p(x x [k] 1, u ) 3: for all observed landmarks z i, do 4: j = c i, 5: if landmark j has never been seen hen 6: m [k] j, = h 1 (x [k], z i, ) 7: H j = J h (m [k] j, ) 8: Σ [k] j, = (H 1 j 9: else 1: ẑ = h(x [k], m [k] j, ) 11: H j = J h (m [k] j, ) )Q i (H 1 1: Q = HΣ [k] j, 1 HT + Q i 13: K = Σ [k] j, 1 HT j Q 1 j ) T 14: m [k] j, = m[k] j, 1 + K(z i, ẑ) 15: Σ [k] j, = (I KH j)σ [k] j, 1 16: w [k] = w [k] f(q, z i,, ẑ) 17: end if 18: end for 19: for all oher landmarks j c do : m [k] j, = m[k] j, 1 1: Σ [k] j, = Σ[k] j, 1 : end for 3: end for 4: for all k {1,,..., m} do 5: draw random paricle k wih probabiliy w [k] 6: include x [k], m [k] 1,, Σ[k] [k] 1,,..., m n,, Σ [k] n, in Y 7: end for 8: reurn Y Lines 1 and : As wih a normal paricle filer, he FasSLAM algorihm begins by enering a loop over all he paricles. The conrol inpu is used o sample a new robo pose for every paricle according o he uncerainy in he moion model. We add random noise drawn from a zero mean Gaussian disribuion wih a covariance of M, given in Equaion 1, o he conrol inpu and use he moion equaion g, given in Equaion 13, o find he new locaion and orienaion of each paricle. Lines 3 and 4: For every paricle we ener a loop over all he measured landmarks. For every ieraion he algorihm can do one of wo hings: add a new landmark, or updae

4 an old landmark. The index of an old landmark in he map is given by he correspondence vecor. Lines 5 o 8: A new landmark is added o he map using he measuremen equaion h, given in Equaion 9, o calculae is locaion in world coordinaes. Since we wan o use an EKF o esimae each landmark we have o linearize he measuremen model by using a firs order Taylor approximaion wih he Jacobian J h (x, m j, ) = x w x w z r x w y w y w z r y w z w z w z r z w. (16) Wih his Jacobian we ransform he uncerainy in measuremen o an uncerainy in world coordinaes. Lines 9 o 15: If a landmark has been observed before, we use he normal EKF equaions o updae is sae vecor and covariance. The sae esimae is calculaed by using he measuremen model. The measuremen model is hen linearized wih a Jacobian similar o he one used for new landmarks. Line 16: Once he landmark has been updaed by using he measuremen we have o calculae is effec on he weighing of he paricle in quesion. As wih a normal paricle filer he imporance weigh is given by w [k] = arge disribuion proposal disribuion. (17) The weighing funcion used in he algorihm can be shown [4] o be f(q, z i,, ẑ) = Q 1 e 1 (zi, ẑ)t Q 1 (z i, ẑ). (18) I is no necessary o updae he weigh for new landmarks as hey will be he same for all paricles, and herefore have no overall effec. Lines 19 o : If a previously observed feaure has no been observed a he curren ime sep is sae vecor and uncerainy will remain unchanged. All unobserved landmarks are herefore essenially ignored. This propery of he algorihm is especially useful when a large map is mainained, as he number of unseen landmarks in he map does no impac he execuion ime. Lines 4 o 7: Resampling is done by drawing paricles wih a probabiliy proporional o heir normalized weighs. Paricles wih low weighs will be more likely o perish while paricles wih high weighs will be copied and used a he nex ime sep. Line 8: Finally he updaed and resampled paricles are reurned o be used a he nex ime sep. A powerful possibiliy emerging from he use of paricles is ha of muliple hypohesis racking. Wha i enails is ha, since paricles represen possible pahs ha he robo could have aken, we can calculae landmark correspondences for each paricle separaely. Because of he expensive naure of calculaing feaure maches we decide agains his procedure and, insead, calculae one correspondence vecor for all he paricles. I is, however, imporan o noe ha he algorihm creaes his possibiliy and fuure exensions can explore his feaure. V. SIMULATION In order o es our SLAM sysems we creaed a simulaion environmen ha provides a realisic represenaion of he real world while faciliaing a quaniaive evaluaion of he performance of he sysem. A. Simulaion environmen We creaed he environmen wih he aim of simulaing he real world wihou i being unnecessarily complicaed. We oped for a roue hrough a corridor-like environmen wih landmarks on he walls. Alhough hese landmarks are more srucured han hey ypically would be in a real world siuaion, he srucure should no influence he resul significanly and should have he benefi of being easy o evaluae visually. In order o creae a conrol inpu we supply waypoins for he simulaed robo o follow. A each ime sep a simple gain conroller generaes an inpu command ha seers i owards he nex waypoin. This conrol inpu is sored for use in he SLAM simulaions bu, before he robo execues he command, we add some Gaussian noise o simulae he uncerainy ha we know exiss in his process (in oher words, we add process noise o he conrol inpu). The robo s acual moion from he noisy conrol is used as a ground ruh rajecory and o generae he measuremens. As he robo moves hrough he environmen, landmarks in he robo s field of view are included in he measuremen a every ime sep. Because feaure deecors will someimes see a landmark a one ime sep and no a he nex, even if i is in he field of view, we add a probabiliy ha a landmark will be seen. We projec he landmarks ono he image planes of wo cameras fixed on he robo and hen add Gaussian noise o he pixel coordinaes. Each landmark is assigned a unique scalar o be used as a descripor. By changing or mixing hese descripors in a measuremen we can simulae feaure mismaches and invesigae heir effec on he accuracy of he SLAM sysem. B. Simulaion resuls The simulaion environmen and he roue and map as esimaed by FasSLAM, using 5 paricles, is depiced in Figure. A every ime sep each landmark has a 4% chance of being observed, bu if i is observed, maching is done wihou error. When we display he roue esimaed by FasSLAM, we use a weighed average of he paricles a every ime sep. In order o evaluae he accuracy we compare i o resuls obained from anoher popular SLAM algorihm, namely EKF SLAM [8]. Resuls of he wo algorihms are consisenly similar in his simulaion, even wih varied noise parameers. The experimen described above shows ha i is possible o achieve accurae resuls using 5 paricles wih FasSLAM. To furher invesigae he relaionship beween he number

5 1 1 8 error (m) Xr (m) ime (s) Fig. 3. The effec of differen numbers of paricles on he Euclidean error of he roue esimaed by FasSLAM. Xr (m) (a) Y r (m) simulaion wihou landmark mismaches of randomness inroduced by he pose sampling sep of he algorihm. Resuls of hese experimens are shown in Figure 3. We see ha wih FasSLAM in D, 5 paricles is a good number o use as we do no lose much accuracy in comparison o using a larger number of paricles. In order o es he effec of landmark mismaches on he accuracy of FasSLAM we performed a simulaion wih such mismaches. The EKF SLAM algorihm is noorious for is inabiliy o handle his kind of error [1] [8] and our simulaion confirms his. Wih only six landmark mismaches over hree ime seps he EKF becomes unsable. Wih he same mismaches FasSLAM remains sable and inroduces only a small degree of drif. This is a major pracical advanage of he algorihm. These resuls are also depiced in Figure. Wih hese simulaions we can esablish, in a conrolled environmen, ha FasSLAM achieves accuracy similar o EKF SLAM and is robus o landmarks mismaches. The following secion describes our pracical ess and resuls. 8 7 VI. EXPERIMENTAL RESULTS The final sep in our invesigaion and developmen of a FasSLAM sysem ha uses sereo vision as a sensor is o es DGPS anenna (b) Y r (m) simulaion wih landmark mismaches Fig.. The roue and map from a simulaion of FasSLAM, compared o EKF SLAM and ground ruh (op). The boom panel depics an enlarged secion of a simulaion wih landmark mismaches. The roues calculaed wih EKF SLAM are shown in magena, he ground ruh roue in red and he environmen walls in green. The esimaed roues from FasSLAM are depiced in blue and he esimaed landmark posiions as black dos wih corresponding confidence ellipses in cyan. Trajecories are shown wih markers on every enh ime sep. Fireflies Sync uni Lapop of paricles and accuracy we ran several simulaions, each wih a differen number of paricles. For every such number we ran he es imes in an aemp o remove he effec Fig. 4. Tes plaform. Pioneer

6 Fig. 5. Sample frames (capured by he lef camera) of he daases used in our experimens. he complee sysem wih real world daases. A. Experimenal seup and daases A real world daase should ideally consis of a se of images capured by wo synchronized and calibraed cameras, a conrol inpu and independenly obained ground ruh locaion informaion ha can be used o evaluae he performance of algorihms. In order o capure such daases we mouned a sereo camera se on a Pioneer 3-AT from Mobile Robos. We programmed he robo o execue a command given o i by a human using a joysick conroller. A every ime sep we sore he forward and roaional velociies so ha hey can be used as conrol inpu by he SLAM algorihms. Our sereo camera rig consiss of wo Poin Grey Firefly MV cameras wih a synchronizaion uni we developed. Ground ruh daa is recorded wih a DGPS (accurae o abou 5 cm) mouned on he robo. Noe ha his ground ruh daa is no used in our SLAM sysem, and is employed merely for evaluaing resuls. Figure 4 shows a picure of our es plaform, indicaing he various componens. When we work in a real world scenario we should expec problems such as bad lighing, uncluered scenes (ha give very few feaures), and a fair amoun of shaking. We ried o capure realisic daases ha included hese problems o a degree. Two daases were capured on he roof of he Elecrical and Elecronic Engineering building in Sellenbosch. The roof is a suiable environmen o es D SLAM algorihms, since i is more or less fla. Apar from background rees moving in he wind i is also compleely saic. The firs of he wo roof daases includes a fair amoun of maneuvering around wo obsacles over a disance of abou 45 meres. The second daase comprises of a slow urn, a fairly long sraigh secion, a hree poin urn wih some reversing, and anoher sraigh secion. The robo covered abou 7 meres. Noe ha urning increases he process noise subsanially because of wheel slippage. A few frames of he daases capured by one of he cameras are shown in Figure 5. B. Experimenal resuls We show he resuls obained from wo experimens. The firs was done using SURF feaures on he firs daase, and he second using SIFT feaures on he second daase. These resuls are depiced in Figure 7 wih corresponding locaion errors in Figure 6. We see ha he Euclidean error from he firs experimen grows over ime. Drif is somehing ha will be presen wih any localizaion sysem ha does no employ absolue measuremens (like GPS). In our work we aemp o limi his drif as much as possible. We see ha boh SIFT and SURF can be used o obain accurae resuls. Alhough we have no way of measuring he accuracies of he esimaed maps, we can observe some srucure and large quaniies of landmarks locaed on he obsacles around which he robo moved. VII. CONCLUSIONS In his paper we invesigaed he use of he FasSLAM algorihm wih landmarks originaing from sereo image feaures. We explained how image feaures can be used as landmarks, wih associaed uncerainies in he form of Gaussian disribuions. A measuremen funcion convers feaures relaive o he robo o landmarks in world coordinaes and hese landmarks are hen mached over ime, and ouliers are idenified and rejeced. The FasSLAM algorihm hen uses a paricle filer o mainain he robo saes, and for each paricle a se of separae EKFs o esimae landmark locaions. We esed he sysem in a conrolled simulaion environmen, and found ha FasSLAM can be as accurae as EKF SLAM (when landmark maches are unconaminaed) bu has he advanage of being largely unaffeced by landmark error (m) error (m) ime (s) ime (s) Fig. 6. The Euclidean error over ime, as measured agains DGPS, of he FasSLAM sysem using SURF feaures on he firs daase (op) and SIFT feaures on he second daase (boom).

7 5 Yw (m) Xw (m) 8 6 Yw (m) Xw (m) Fig. 7. Esimaed roues (in blue saring a he origin) and maps from he FasSLAM algorihm using SURF feaures on he firs oudoor roof daases (op) and SIFT feaures on he second (boom) wih he DGPS ground ruh in red. Markers are placed a every enh ime sep of he roues. The landmarks ha we show, as black dos wih cyan confidence ellipses, are hose ha were observed on muliple ime seps, i.e. hose ha conribued o he accuracy of he roue esimaion. mismaches. This advanage of FasSLAM is significan, paricularly when sereo feaures are used as landmarks, due o he unavoidable possibiliy of mismaches occurring. The problem of mismaches is inheren o image feaures, ha ofen exhibi ambiguous characerisics, and we mus herefore be able o rely on he SLAM sysem o remain sable in spie of such errors. We also esed our complee FasSLAM sysem on daa capured by a real robo. The accuracies achieved wih eiher SIFT or SURF feaures are comparable o oher sae-of-he-ar sysems [1] [11]. We conclude ha, because of is accuracy and robusness, FasSLAM can be a very effecive algorihm o use wih measuremens from a sereo vision sensor. R EFERENCES [1] S. Thrun, W. Burgard, and D. Fox, Probabilisic Roboics. MIT Press, 6. [] H. Durran-Whye and T. Bailey, Simulaneous localizaion and mapping (SLAM): Par I, IEEE Roboics and Auomaion Magazine, vol. 13, no., pp , 6. [3] A. Douce, J. de Freias, K. Murphy, and S. Russel, Rao-Blackwellized paricle filering for dynamic Bayesian neworks, Conference on Uncerainy in Arificial Inelligence, pp ,. [4] M. Monemerlo, S. Thrun, D. Koller, and B. Wegbrei, FasSLAM: A facored soluion o he simulaneous localizaion and mapping problem, Proceedings of he AAAI Naional Conference on Arificial Inelligence,. [5] G. Grisei, C. Sachniss, and W. Burgard, Improved echniques for grid mapping wih Rao-Blackwellized paricle filers, IEEE Transacions on Roboics, vol. 3, no. 1, pp , 7. [6] D. Lowe, Objec recogniion from local scale invarian feaures, IEEE Inernaional Conference on Compuer Vision, pp , [7] H. Bay, A. Ess, T. Tuyelaars, and L. van Gool, Speeded-up robus feaures (SURF), Compuer Vision and Image Undersanding, vol. 11, no. 3, pp , 8. [8] W. Brink, C. van Daalen, and W. Brink, Sereo vision as a sensor for EKF SLAM, nd Annual Symposium of he Paern Recogniion Associaion of Souh Africa, pp. 19 4, 11. [9], Probabilisic oulier removal for robus landmark idenificaion in sereo vision based SLAM, IEEE/RSJ Inernaional Conference on Inelligen Robos and Sysems, pp. 8 87, 1. [1] G. Dubbelman, W. van der Mark, and F. Groen, Accurae and robus ego-moion esimaion using expecaion maximizaion, IEEE/RSJ Inernaional Conference on Inelligen Robos and Sysems, pp , 8. [11] J. Civera, O. Grasa, A. Davison, and J. Moniel, 1-poin RANSAC for EKF-based srucure from moion, IEEE/RSJ Inernaional Conference on Inelligen Robos and Sysems, pp , 9.