Planar Odometry from a Radial Laser Scanner. A Range Flow-based Approach

Transcription

1 Plana Odomety fom a Radial Lase Scanne. A Range Flow-based Appoach Maiano Jaimez, Javie G. Monoy and Javie Gonzalez-Jimenez Abstact In this pape we pesent a fast and pecise method to estimate the plana motion of a lida fom consecutive ange scans. Fo evey scanned point we fomulate the ange flow constaint equation in tems of the senso velocity, and minimize a obust function of the esulting geometic constaints to obtain the motion estimate. Convesely to taditional appoaches, this method does not seach fo coespondences but pefoms dense scan alignment based on the scan gadients, in the fashion of dense 3D visual odomety. The minimization poblem is solved in a coase-to-fine scheme to cope with lage displacements, and a smooth filte based on the covaiance of the estimate is employed to handle uncetainty in unconstaint scenaios (e.g. coidos). Simulated and eal expeiments have been pefomed to compae ou appoach with two pominent scan matches and with wheel odomety. Quantitative and qualitative esults demonstate the supeio pefomance of ou appoach which, along with its vey low computational cost (.9 milliseconds on a single CPU coe), makes it suitable fo those obotic applications that equie plana odomety. Fo this pupose, we also povide the code so that the obotics community can benefit fom it. I. INTRODUCTION Odomety is an essential component fo obot localization. It is commonly solved though thee majo techniques that ae based on inetial devices, wheel encodes o visual odomety (eithe by featue tacking o by dense image alignment). Inetial measuement units (IMUs) ae ideal to estimate spatial oientation but accumulate too much tanslational eo ove time due to thei inability to cancel the gavitational component of the measuement []. Odomety based on encondes has extensively been used to povide fast motion estimates fo wheeled o legged obots, though this appoach is pone to being inaccuate due to wheel/leg slippage and the impeciseness of the kinematic obot models. Last, vision-based methods ae aguably the most flexible and poweful solution to the motion estimation poblem because they can be adapted to wok with diffeent types of obots (wheeled, legged, aeial) and configuations (D-3D motion). Ou poposal hee elies on lase scans and has the advantage ove the afoementioned methods to be independent of the vehicle type of locomotion as well as vey fast and pecise, as suppoted by expeimental validation. Thus, it tuns out to be paticulaly suitable fo those (vey common) cases whee the obot aleady uses a lase ange scanne fo mapping, obstacle avoidance o localization. Ou appoach, named RFO (Range Flow-based D Odomety), builds This wok has been funded by the Spanish Govenment unde poject DPI R and the gant pogam FPI-MICINN. All authos belong to the MAPIR goup of the depatment of System Engineeing and Automation, Univesity of Malaga. {maianojt, jgmonoy, javiegonzalez}@uma.es upon [] and epesents the appaent motion of any point obseved by the senso as a function of the velocity of the senso, assuming that the envionment is static. Thus, evey point defines a geometic esidual which can be minimized within a dense fomulation to obtain the lida motion. To ovecome the assumption of a motionless envionment (i.e. to handle moving objects), we compute the Cauchy M-estimato of the geometic esiduals, a moe obust estimate than taditional choices like the L o L noms. Futhemoe, we solve this estimation poblem within a coase-to-fine scheme, which povides fine esults and allows the method to cope with lage motions. We have conducted a vaied set of expeiments to compae ou method against point-to-line iteative closest point (PL- ICP) [3] and the pola scan matching appoach () [4]. Fistly, thei pefomances ae evaluated at diffeent scan ates on simulated scenaios whee the gound tuth is available. Secondly, qualitative esults ae shown fo a eal expeiment whee D maps ae built by concatenating the scanned points accoding to the odomety motion estimates of each method. Thidly, we devise a eal expeiment to evaluate how obust the methods ae against the pesence of noise and moving objects. Oveall, esults show that RFO is significantly moe pecise fo both tanslations and otations, and pesents the lowest untime ( times faste than and times faste than ). Besides analyzing the esults pesented heein, we encouage the eade to watch the demonstation video which, togethe with the available code, can be found at: II. RELATED WORK Although low-cost RGB-D cameas have ecently favoed the tansition to 3D odomety, localization and mapping stategies, it is a matte of fact that a fai numbe of mobile obots move on plana sufaces and ely on lase scannes to navigate. In this context, vey successful esults have been achieved in the fields of D Localization [5][6] and SLAM [7], and many algoithms have been poposed to solve the geneal scan matching poblem [8][3][9]. In this pape we focus on pue D odomety, which can be egaded as a paticula case of scan matching, whee the scans to be aligned ae taken consecutively and ae nomally close to each othe. Taditionally, ICP [9] o some of its vaiants have been applied to solve the egistation poblem between consecutive scans. A vey successful appoach was poposed by Censi

2 [3], whee a point-to-line metic is used instead of the pointto-point oiginal metic of ICP. Futhemoe, the autho pesented an implementation which an one ode of magnitude faste than existing ICP vaiants, and was moe pecise and efficient than the pionee point-to-segment wok in []. Moe ecently, Genealized-ICP [] showed an impoved pefomance ove pevious ICP vesions, but has been mostly used fo the egistation of 3D point clouds. Fo this family of methods, accuacy depends on evey paticula vesion and implementation, yet they all shae the same weakness: they ae computationally expensive. Altenatively, othe methods wee specifically designed to solve the D scan matching poblem. Gonzalez&Gutieez [] fomulated the velocity constaint equation, an adaptation of the optical flow constaint fo ange scans, and poposed to estimate the lida motion by imposing this estiction fo evey point obseved in the scans. Howeve, thei method was only tested with simple simulated scenaios and povided modest esults. Diosi&Kleeman pesented the Pola Scan Matching appoach [8], whee the tanslation and otation between two scans ae altenately estimated until convegence. Convesely to ICP, this method avoids seaching fo coespondences by simply matching points with the same beaing, which leads to a highe computational pefomance. This appoach was subsequently extended and futhe evaluated in [4]. A diffeent method poposed by Olson [] ties to find the igid tansfomation that maximizes the pobability of having obseved the latest scan given the pevious one. Additional infomation is used (contol inputs o wheel odomety) to ease the method convegence and two diffeent implementations, GPU and multi-esolution CPU, ae pesented. A thoough evaluation is pefomed in tems of computational pefomance but, supisingly, no esults fo the method s accuacy ae pesented. Moe ecently, othe appoaches have built upon the afoementioned woks. It is the case of [3] and [4], which fuse lase odomety (the Olson s lase odomety [] and pointto-line ICP [3], espectively) with steeo vision to pefom autonomous navigation with UAVs. Futhemoe, the wok of Pomeleau et al. [5] pesents a fast implementation and a though evaluation of some ICP vaiants on eal-wold D and 3D data sets. III. LIDAR VELOCITY AND D RANGE FLOW In this section we descibe how the D velocity of a lase scanne can be estimated fom the appaent motion that it obseves, assuming that the envionment is static and igid. Let R(t, α) be a ange scan whee t is the time and α [, N) R is the scan coodinate, being N the size of the scan. The position of any point P with espect to the local efeence fame attached to the senso is given by its pola coodinates (, θ) (see Fig. ). Povided that P is visible fom the lida, it will be obseved at a scan coodinate α that is diectly elated to the angula coodinate of P : α = N F OV θ + N = k α θ + N () Fig.. Top view epesentation of a lase scan that intesects with a cetain object. An obseved point P moves with espect to the scanne to P afte an inteval of time t. whee F OV is the scanne field of view. Similaly to the optical flow constaint equation, a linea constaint can be deived fom the geneal expession of geometic consistency of two scan pais. Assuming the diffeentiability of R, the ange of any point at the second scan can be expessed as the Taylo expansion R(t + t, α + α) = R(t, α) + R (t, α) t t + R α (t, α) α + O( t, α ) () whee t is the time lapse between consecutive scans and α epesents the change in the scan coodinate of the point consideed. Neglecting the second/highe ode tems, and dividing by t, we can obtain a simple expession that elates the scan gadients with the change of the ange and the scan coodinate of a point duing the inteval [t, t + t]: with R t R α t + R α t R = R(t + t, α + α) R(t, α), R t = R (t, α), t R α = R (t, α). α If we conside that ṙ = R/ t and α = α/ t ae the aveage velocities of a point in ange and scan coodinates duing the inteval [t, t + t], we obtain: (3) ṙ R t + R α α = R t + R α k α θ (4) Equation (4) was fistly intoduced by Gonzalez&Gutieez [] and subsequently genealized and named as the ange flow constaint equation in [6]. In ode to descibe the velocities of all points with espect to the same vecto basis, we tansfom the adial and azimuthal velocities (ṙ, θ) to a catesian epesentation (ẋ, ẏ), as shown in Fig. : ṙ = ẋ cos θ + ẏ sin θ (5) θ = ẏ cos θ ẋ sin θ (6) As a last step, we need to impose that evey appaent motion is actually caused by the lida tanslation and/o otation. In

3 othe wods, we assume that evey point moves with espect to the senso as if it was pat of a igid body whose velocity is the same but opposite in sign to that of the senso: (ẋ ) ( ) vx,s + y ω = s (7) ẏ v y,s x ω s being ξ s = (v x,s, v y,s, ω s ) a D twist (senso velocity) and (x, y) the catesian coodinates of P. If the catesian velocities (5) (6) ae substituted in (4) and the igidity hypothesis (7) is imposed, we can tansfom the ange flow constaint equation into a constaint fo the lida velocity: ( cos θ + R ) ( αk α sin θ v x,s + sin θ R ) αk α cos θ v y,s + (x sin θ y cos θ R α k α ) ω s + R t = (8) As a esult, evey scanned point imposes a estiction to the senso motion and, theefoe, 3 linealy independent estictions would theoetically suffice to estimate it. IV. VELOCITY ESTIMATION In pactice, the lida motion cannot be estimated with only thee independent estictions because, in geneal, (8) is inexact due to the noise of the ange measuements, the eos made by the linea appoximation (3) o the pesence of moving object (non-static envionment). Theefoe, we popose a dense fomulation in which all the points of the scan contibute to the motion estimate. Fo evey point, we define the geometic esidual ρ(ξ) as the evaluation of the ange flow constaint (8) fo a given twist ξ: ρ(ξ) = R t + (x sin θ y cos θ R α k α ) ω (9) ( + cos θ + R ) ( αk α sin θ v x + sin θ R ) αk α cos θ v y To obtain an accuate estimate, the senso motion is computed by minimizing all the geometic esiduals within a obust function F : ξ M = ag min ξ N F (ρ i (ξ)) () i= ( ( F (ρ) = k ρ ) ) ln + k () The function F is the Cauchy M-estimato, and k is an adjustable paamete. Convesely to the moe common choices of the L o L noms, this function educes the elevance of those points with vey high esiduals, and epesents an effective and automatic way to deal with outlies. The optimization poblem is solved with Iteatively Reweighted Least Squaes (IRLS), whee the weights associated to the Cauchy M-estimato ae: w(ρ) = + ( ) ρ () k With IRLS, the system is iteatively solved by ecomputing the esiduals and subsequently the weights until convegence. A. Pe-weighting stategy As peviously mentioned, thee ae some factos that can ende (8) inaccuate, mainly the unfulfillment of the igidity hypothesis (7) and the deviations fom the linea appoximation made in (3). Although the Cauchy M-estimato can alleviate thei effect on the oveall motion estimate, it does not eliminate it completely. The pesence of moving objects is had to detect befoe solving the motion and, theefoe, we ely on the Cauchy M-estimato to downweight them duing the minimization pocess. On the othe hand, deviations fom the linea appoximation adopted in (3) can be detected befoehand, which helps to acceleate convegence in () and also leads to moe accuate esults. To this pupose, we popose a pe-weighting stategy to downweight the esiduals of those points whee the ange function is nonlinea o even non-diffeentiable. We call it pe-weighting because it is applied befoe the minimization poblem () is solved. In ode to quantify the eo associated to the lineaization of (), we expand the Taylo seies to the second ode: ṙ = R t + R α α + R o ( t, α) + O( t, α) R o ( t, α) = t ( Rtt + R tα α + R αα α ) (3) It can be noticed that, neglecting highe ode tems, the second ode deivatives in R o ( t, α) can be used to detect the deviations fom lineaity. One special case is the second ode deivative with espect to time (R tt ), which cannot be computed in a coase-to-fine scheme because the waped images ae timeless and, theefoe, the concept of second tempoal deivative makes no sense (coase-to-fine is descibed in Section V). Moeove, it is impotant to detect those egions of the scans whee the ange function is not only nonlinea but non-diffeentiable. These egions ae mainly the edges of the diffeent objects obseved, and ae typically chaacteized by vey high values of the fist ode deivatives (R t and/o R α ). To penalize these two effects, nonlineaities and discontinuities, we define the following pe-weighting function fo each scanned point: w = ɛ + R α + t R ( t + K d Rαα + t R ) tα (4) The paamete K d maks the elative impotance of fist ode and second ode deivatives, and ɛ is a small constant to avoid the singula case. Thus, we initially compute a pe-weighted set of esiduals ρ w i (ξ) = w i ρ i (ξ) i {,...N} (5) which ae subsequently minimized accoding to () (). Although we do not show compaisons in the pape, this stategy povides bette esults than standad IRLS minimization without pe-weighting and conveges faste (appoximately by a facto of ). V. COARSE-TO-FINE SCHEME AND SCAN WARPING The lineaization pesented in (3) holds eithe fo small displacements between consecutive scans o at aeas with

4 constant ange gadients (which, in the case of a lida, would occu fo a vey unusual geomety: an Achimedean spial). To ovecome this limitation, we estimate motion in a coaseto-fine scheme, whee the coase levels povide a ough estimate which is impoved subsequently in fine levels. The coase-to-fine scheme was intoduced by Battiti et al. [7] to solve the optical flow poblem fo lage displacements, and has commonly been adopted eve since [8][9]. Let R, R be two consecutive lase scans. Initially, two Gaussian pyamids ae to be ceated by successively downsampling (typically by ) the oiginal scans R, R. Nomally, a Gaussian mask is applied to downsample RGB o gayscale images but, in the case of ange data, a standad Gaussian filte is not the best choice since it ceates atifacts on the filteed scans. As an altenative, we employ a bilateal filte [] that does not mix distant points which ae likely to belong to diffeent objects of the scene. Once the pyamids ae built, the velocity estimation poblem is iteatively solved fom the coasest to the finest level. At evey tansition to a fine level, one of the two input scans must be waped against the othe accoding to the oveall velocity estimated in pevious levels (ξ p ). This waping pocess is always divided into two steps and, in ou fomulation, is applied ove the second scan R. Fistly, evey point P obseved in R is spatially tansfomed using the igid body motion associated to the twist ξ p : x w x y w = eˆξ p y, ˆξ ω p v x,p p = t ω p v y,p (6) Secondly, the tansfomed points must be epojected onto R to build the waped scan R w so that: R w (α w ) = (x w + (y w ), (7) ( ) y α w w = k α actan x w + N (8) Seveal points could be waped to the same coodinate α w, in which case the closest one is peseved (the othes would be occluded). If ξ p is conveging to the eal velocity, then the waped scan R w will be consideably close to the fist scan R than the oiginal R, which allows us to apply the linea appoximation in () with a fine esolution. VI. IMPLEMENTATION Ou algoithm pays special attention to the computation of the ange gadients. Nomally, a fixed discete fomula is employed to appoximate eithe scan o image gadients. In the case of ange data, this stategy leads to vey high values of the gadients at the object bodes, which do not epesent the eal gadients ove those objects. As an altenative, we make use of an adaptive fomula that egads the geomety of the envionment. This fomula weights fowad and backwad deivatives in the scan with the D distances between contiguous obsevations (points): R α (α) = d(α + ) R α (α) + d(α) R α + (α) (9) d(α + ) + d(α) Rα = R(α) R(α ), R α + = R(α + ) R(α) d(α) = ((x(α) x(α ), y(α) y(α )) Thus, the closest neighbou is always contibuting moe to the gadient computation while distant points baely affect it. In the case that both neighbous ae appoximately equidistant, the pesented fomula is equivalent to a centeed finite diffeence appoximation. Moe details about the gadient computation can be found in [9]. Last, it is impotant to emak that thee ae some geometic configuations of the envionment fom which the senso motion cannot be ecoveed. The most common case aises when the lida only obseves a wall. Unde this cicumstance, the motion paallel to the wall is undetemined and theefoe the solve would povide an abitay solution fo it (not only ou method but any appoach based puely on geomety). In ode to mitigate this poblem, we apply a lowpass filte in the eigenspace of the velocity ξ which woks as explained next. Fist, the eigenvalues of the covaiance matix Σ R 3 3 of the IRLS solution ae analyzed to detect which motion (o combinations of motions) ae undetemined and which ae pefectly constained. In the space of the eigenvectos, the velocity ξ t M povided by () is weighted with that of the pevious time inteval ξ t to obtain the new filteed velocity ξ t : [( + k l )I + k e E] ξ t = ξ t M + (k l I + k e E)ξ t () whee E is a diagonal matix containing the eigenvalues and k l, k e ae paametes of the filte. Concetely, k l imposes a constant weighting between the solution fom the solve and the pevious estimate while k e defines how the eigenvalues affect the final estimate. These paametes ae set to the following values: k l =.5e (l ), k e = 5 3 e (l ) () whee l is the pyamid level that anges fom (coasest) to the numbe of levels consideed. Please efe to [9] fo a moe detailed explanation on this filte and how it is applied. VII. EXPERIMENTS This section is composed of a set of thee diffeent expeiments. The two fist expeiments addess the evaluation of the poposed RFO algoithm and its compaison with othe appoaches in simulated and eal envionments, espectively. The thid expeiment is caied out to analyze the obustness of the motion estimates against noise and the pesence of moving objects. Fo compaison, two state-of-the-at scan matches ae selected: Point-to-Line ICP () [3], and Pola Scan Matching () [4]. In both cases, we use the implementations that thei authos published online. Fo quantitative evaluation, the elative pose eos as descibed in [] will be consideed. Both tanslational and otational deviations pe second will be evaluated with the oot mean

5 TABLE I SIMULATED EXPERIMENT - TRANSLATIONAL AND ROTATIONAL DEVIATIONS PER SECOND, AND EXECUTION TIMES. Scen. Scen. Scen. 3 Tanslational RMSE (cm/s) Rotational RMSE (deg/s) Runtime (ms) Scan ate (Hz) RFO RFO RFO squaed eo (RMSE), which coesponds to a pefomance measue independent of the expeiment duation. Fo eal expeiments, a Hokuyo URG-4LX-UG lase scanne mounted on a Giaff mobile obot [] is used to gathe the lase scans at a maximum fequency of Hz. Fo the case of simulated expeiments, the lase chaacteistics have been imitated (ay numbe = 68, FOV = 4, max distance = 5.5 m). Moeove, a Gaussian noise with σ = cm is added to the simulated scans to make them moe ealistic. A. Compaison in a Simulated Envionment In this expeiment, the compaed methods estimate the plana motion of a lase scanne that moves in a simulated envionment. We use the pecise gound tuth available in simulation to pefom a quantitative evaluation of the diffeent appoaches. The simulated envionment is divided into thee distinct scenaios: a oom containing only objects fomed by staight line segments (Scen. ), a oom that contains only cuved obstacles and cuved walls (Scen. ) and a staight coido with scatteed small objects (Scen. 3). Duing the expeiment, the lida tavels along a pedefined path, coveing a distance of metes at an aveage speed of.398 m/s. Futhemoe, fou diffeent scan ates (, 5, and Hz) ae tested to analyze the influence of the fequency of execution in the odomety estimates. Table I depicts the elative pose eos in the fom of tanslational End Stat GT RFO 3 Fig.. Simulated envionment and the best path as estimated by each Numbes indicates the diffeent scenaios of the envionment. and otational deviations pe second, togethe with thei untimes fo the thee compaed methods. Fig. plots the simulated envionment with the best estimated tajectoy of each method. That is, fom all the execution ates, only the one with oveall minimum eo is plotted fo qualitative assessment. As can be noticed, RFO exceeds the othe two appoaches fo all the scenaios in the expeiment, poviding much moe accuate estimates. pesents elatively good estimates fo the oom scenaios, but it dastically fails at the coido (specially fo tanslations). On the othe hand, pesents much highe elative eos in geneal, and concetely at the second scenaio whee only cuved objects can be found. Futhemoe, it pesents impotant poblems at naow places such as doos. It is inteesting to notice that the best esults ae not obtained with the highest fequency. Expeiments at Hz povide wose esults than those at 5 o Hz, which indicates that data ovesampling leads to eo accumulation. On the othe hand, a too little fequency implies that consecutive scans ae too sepaate and moe difficult to align (as occus at Hz). Thus, the optimal fequency is not always the highest available one and depends on the aveage (o maximum) linea and angula speeds of the lida. An altenative and helpful way to compae these methods is to calculate thei RMS eos pe segment length, as descibed in [3]. Fig. 3 depicts these aveage tanslational eos as a pecentage of the segment length consideed, and computed independently fo the thee scenes of the expeiment. It can be seen that ou appoach is in all cases supeio to the othe two methods, being always unde % RMSE fo all thee scenes. is the second best candidate, having aound 5% RMSE, except fo the long coido (Scen. 3) whee it completely fails. Finally, fom the computational point of view, the last columns on Table I show the untimes in milliseconds measued on an AMD Phenom II X6 35T CPU at.6 GHz. Oveall, RFO takes less than ms, followed by with.85 ms and with moe than 9 ms. Taking this into account, the pesented appoach not only povides moe accuate estimates but it is also much faste than its competitos.

6 Tanslation RMSE (%) Scen. Scen. Scen. 3 RFO 5 RFO RFO Path Length (m) Fig. 3. Tanslation eos aveaged ove all sub-sequences of a given length fo the thee scenes of the simulated expeiment. B. Real Expeiment To validate the esults obtained in the simulated expeiments, we employ a eal mobile obot equipped with a Hokuyo lase scanne which navigates in an office-like envionment. Making use of a mobile obot allows us to include the odomety estimates fom the onboad encodes (a pai of low-cost AMT-v incemental encodes fom CUI Inc.), but pevents us fom pefoming a quantitative compaison given the lack of a pecise gound tuth. Theefoe, in this section the diffeent methods ae compaed just qualitatively by plotting D maps built puely fom the odomety pose estimates. In othe wods, fo each method we pesent maps built as a concatenation of D point clouds along thei estimated tajectoies, without esoting to global consistency o any othe mapping stategy. The path coveed by the obot duing this expeiment is oughly 49 metes long, and is tavelled at an aveage speed of.535 m/s (max speed of.6 m/s). Fo all methods, we set the scan ate at Hz. Fig. 4 plots the maps built fom the tajectoy estimates of the diffeent appoaches. As a efeence, we plot the map built fom the accuate localization povided by [6], which does not compute odomety but finds the pose of the obot within a peviously built map. As can be seen, the map deived fom ou odomety estimation is noticeably close to the efeence map than any of the othes. povides the second best estimation afte RFO, failing mostly in the coido aeas (see Section VII- A), which esults in a shotening of the map and ovelapping of scan points in such aeas. and the encode-based maps follow the compaison, being the latte the wost of all of them, with diffeence. C. Robustness Against Noise and Non-static Envionments Finally, we analyze how noise and moving objects affect the motion estimation of the poposed method, i.e., when the assumption of a static envionment is violated. Theefoe, this section is composed of two expeiments. The fist one aims to evaluate the dift of the compaed methods caused by the noise of the measuements. To this end, a eal expeiment is conducted whee a lida woking at Hz is kept still in a static envionment fo a time lapse of thee minutes. Unde this setup, since the only eo involved is the senso noise, the outcome epesents how noise affects the motion estimates of the diffeent methods. Table II shows the elative deviations pe second of the compaed methods. We have also consideed a simplified vesion of ou appoach (RFO- NC), whee we emove the Cauchy M-estimato and simply minimize the squaed esiduals (see ()). Fom these esults we can conclude that both RFO and ae equally good at tanslations, being maginally wose than the nonobust RFO-NC, while, at otations, is slightly supeio than the othes. Then, a second expeiment is conducted in the same scenaio but, in this case, seveal moving objects ae intoduced. Duing the expeiment two pesons ae walking aound the obot, opening and closing a doo and displacing a cadboad box (see Fig. 5). The eade is encouaged to watch the demonstation video whee the expeiment is shown in detail ( TABLE II TRANSLATIONAL AND ROTATIONAL DEVIATIONS PER SECOND: ROBUSTNESS AGAINST NOISE AND MOVING OBJECTS. Static Expeiment RMSE RFO RFO-NC Tanslation (cm/s) Rotation (deg/s) Moving Objects Expeiment RMSE RFO RFO-NC Tanslation (cm/s) Rotation (deg/s) As can be seen in the second pat of Table II, is the most obust method in such situations, followed by the poposed RFO. It is impotant to notice that, although is between two and thee times bette than RFO, the magnitude of the eos is still petty small fo both methods, unlike the estimates, which show impotant tanslational and otational difts. Finally, compaing the two vesions of ou appoach, it can be noticed that unde the pesence of moving objects, the Cauchy M-estimato povides esults that ae 5% moe accuate than those obtained with standad quadatic minimization. VIII. CONCLUSIONS We have pesented a novel appoach named RFO to estimate the plana motion of a lida by imposing the ange flow constaint equation on consecutive scan pais. Extensive expeiments have been caied out to demonstate the accuacy of ou method, and compaisons with point-to-line ICP, Pola Scan Matching and the standad wheel odomety have been pefomed unde diffeent scenaios and fame ates. Results show that RFO povides the most accuate estimates fo both tanslations and otations, except fo nonstatic envionments, whee is slightly supeio. With a epoted untime of baely millisecond, plana motion can be easily estimated with almost no computational cost, which makes this method attactive fo many obotic applications

7 Refeence RFO Encode m Fig. 4. Maps built as a concatenation of D point clouds along the estimated tajectoies fo diffeent methods. The efeence map is built using the accuate localization of a paticle filte-based appoach. Tajectoies ae shown in ed and the scanned points in blue. Fig. 5. Fist ow: Sequence of images taken duing the second expeiment descibed in the section VII-B. Second ow: 3D epesentation of the scans and the obot at those paticula instants, whee the non-static points ae shown in ed. that ae computationally demanding and equie eal-time pefomance. Fo futue wok, we plan to analyze the effect of small deviations fom plana motion, which might be useful if this method is applied to estimate the motion of a quadcopte o a vehicle with stong dynamics. R EFERENCES [] O. J. Woodman, An intoduction to inetial navigation, Univesity of Cambidge, Compute Laboatoy, Tech. Rep. UCAMCL-TR-696, vol. 4, 7. [] J. Gonzalez-Jimenez and R. Gutieez, Diect motion estimation fom a ange scan sequence, Jounal of Robotic Systems, vol. 6, no., pp. 73 8, 999. [3] A. Censi, An ICP vaiant using a point-to-line metic, in IEEE Int. Confeence on Robotics and Automation (ICRA), 8, pp [4] A. Diosi and L. Kleeman, Fast lase scan matching using pola coodinates, The Intenational Jounal of Robotics Reseach, vol. 6, no., pp. 5 53, 7. [5] S. Thun, D. Fox, W. Bugad, and F. Dellaet, Robust Monte Calo localization fo mobile obots, Atificial intelligence, vol. 8, no., pp. 99 4,. [6] J. L. Blanco, J. Gonzalez-Jimenez, and J. A. Fenandez-Madigal, Optimal filteing fo non-paametic obsevation models: applications to localization and SLAM, The Intenational Jounal of Robotics Reseach, vol. 9, no. 4, pp , dec. [7] H. Duant-Whyte and T. Bailey, Simultaneous localization and mapping: pat I, IEEE Robotics & Automation Magazine, vol. 3, no., pp. 99, 6. [8] A. Diosi and L. Kleeman, Lase scan matching in pola coodinates with application to SLAM, in IEEE Int. Confeence on Intelligent Robots and Systems (IROS), 5, pp [9] P. J. Besl and N. D. McKay, Method fo egistation of 3-D shapes, in Robotics-DL tentative, 99, pp [] J. Gonzalez, A. Stentz, and A. Olleo, A mobile obot iconic position estimato using a adial lase scanne, Jounal of Intelligent and Robotic Systems, vol. 3, no., pp. 6 79, 995. [] A. Segal, D. Haehnel, and S. Thun, Genealized-ICP, in Poc. of Robotics: Science and Systems (RSS), 9. [] E. B. Olson, Real-time coelative scan matching, in IEEE Int. Confeence on Robotics and Automation (ICRA), 9, pp [3] M. Achtelik, A. Bachach, R. He, S. Pentice, and N. Roy, Steeo vision and lase odomety fo autonomous helicoptes in GPS-denied indoo envionments, in Unmanned Systems Technology XI, SPIE, 9, pp [4] T. Tomic, K. Schmid, P. Lutz, A. Domel, M. Kassecke, E. Mai, I. L. Gixa, F. Ruess, M. Suppa, and D. Buschka, Towad a fully autonomous UAV: Reseach platfom fo indoo and outdoo uban seach and escue, IEEE Robotics & Automation Magazine, vol. 9, no. 3, pp ,. [5] F. Pomeleau, F. Colas, R. Siegwat, and S. Magnenat, Compaing ICP vaiants on eal-wold data sets, Autonomous Robots, vol. 34, no. 3, pp , 3. [6] H. Spies, B. Jahne, and J. L. Baon, Range flow estimation, Compute Vision and Image Undestanding, vol. 85, no. 3, pp. 9 3,. [7] R. Battiti, E. Amaldi, and C. Koch, Computing optical flow acoss multiple scales: An adaptive coase-to-fine stategy, Intenational Jounal of Compute Vision, vol. 6, no., pp , 99. [8] T. Box, A. Buhn, N. Papenbeg, and J. Weicket, High accuacy optical flow estimation based on a theoy fo waping, in Poc. Euopean Confeence on Compute Vision (ECCV), 4, pp [9] M. Jaimez and J. Gonzalez-Jimenez, Fast visual odomety fo 3-D ange sensos, IEEE Tansactions on Robotics, vol. 3, no. 4, pp. 89 8, 5. [] M. Elad, On the oigin of the bilateal filte and ways to impove it, IEEE Tansactions on Image Pocessing, vol., no., pp. 4 5,. [] J. Stum, N. Engelhad, F. Endes, W. Bugad, and D. Cemes, A benchmak fo the evaluation of RGB-D SLAM systems, in IEEE Int. Confeence on Intelligent Robot Systems (IROS),. [] J. Gonzalez-Jimenez, C. Galindo, and J. R. Ruiz-Samiento, Technical impovements of the Giaff telepesence obot based on uses evaluation, in IEEE RO-MAN: The st IEEE Intenational Symposium on Robot and Human Inteactive Communication,. [3] A. Geige, P. Lenz, and R. Utasun, Ae we eady fo autonomous diving? the KITTI vision benchmak suite, in Poc. Int. Confeence on Compute Vision (ICCV),, pp