Generalized-ICP. Aleksandr V. Segal Stanford University Dirk Haehnel Stanford University

Transcription

1 Generalzed-ICP Aleksandr V. Segal Stanford Unversty Emal: Drk Haehnel Stanford Unversty Emal: Sebastan hrun Stanford Unversty Emal: Abstract In ths paper we combne the Iteratve Closest Pont ( ICP ) and pont-to-plane ICP algorthms nto a sngle probablstc framework. We then use ths framework to model locally planar surface structure from both scans nstead of just the model scan as s typcally done wth the pont-to-plane method. hs can be thought of as plane-to-plane. he new approach s tested wth both smulated and real-world data and s shown to outperform both standard ICP and pont-to-plane. Furthermore, the new approach s shown to be more robust to ncorrect correspondences, and thus makes t easer to tune the maxmum match dstance parameter present n most varants of ICP. In addton to the demonstrated performance mprovement, the proposed model allows for more expressve probablstc models to be ncorporated nto the ICP framework. Whle mantanng the speed and smplcty of ICP, the Generalzed-ICP could also allow for the addton of outler terms, measurement nose, and other probablstc technques to ncrease robustness. I. INRODUCION Over the last decade, range mages have grown n popularty and found ncreasng applcatons n felds ncludng medcal magng, object modelng, and robotcs. Because of occluson and lmted sensor range, most of these applcatons requre accurate methods of combnng multple range mages nto a sngle model. Partcularly n moble robotcs, the avalablty of range sensors capable of quckly capturng an entre D scene has drastcally mproved the state of the art. A strkng llustraton of ths s the fact that vrtually all compettors n the DARPA Grand Challenge reled on fast-scannng laser range fnders as the prmary nput method for obstacle avodance, moton plannng, and mappng. Although GPS and IMUs are often used to calculate approxmate dsplacements, they are not accurate enough to relably produce precse postonng. In addton, there are many stuaton (tunnels, parkng garages, tall buldngs) whch obstruct GPS recepton and further decrease accuracy. o deal wth ths shortcomng, most applcatons rely on scan-matchng of range data to refne the localzaton. Despte such wde usage, the typcal approach to solvng the scan-matchng problem has remaned largely unchanged snce ts ntroducton. II. SCANMACHING Orgnally appled to scan-matchng n the early 9s, the ICP technque has had many varatons proposed over the past decade and a half. hree papers publshed around the same tme perod outlne what s stll consdered the state of the art soluton for scan-matchng. he most often cted analyss of the algorthm comes from Besl and McKay[]. [] drectly addresses regstraton of D shapes descrbed ether geometrcally or wth pont clouds. Chen and Medon[7] consdered the more specfc problem of algnng range data for object modelng. her approach takes advantage of the tendency of most range data to be locally planar and ntroduces the pont-to-plane varant of ICP. Zhang[] almost smultaneously descrbes ICP, but adds a robust method of outler rejecton n the correspondence selecton phase of the algorthm. wo more modern alternatves are Iteratve Dual Correspondence [] and Metrc-Based ICP [6]. IDC mproves the pont-matchng process by mantanng two sets of correspondences. MbICP s desgned to mprove convergence wth large ntal orentaton errors by explctly puttng a measure of rotatonal error as part of the dstance metrc to be mnmzed. he prmary advantages of most ICP based methods are smplcty and relatvely quck performance when mplemented wth kd-trees for closest-pont look up. he drawbacks nclude the mplct assumpton of full overlap of the shapes beng matched and the theoretcal requrement that the ponts are taken from a known geometrc surface rather than measured []. he frst assumpton s volated by partally overlapped scans (taken from dfferent locatons). he second causes problems because dfferent dscretzatons of the physcal surface make t mpossble to get exact overlap of the ndvdual ponts even after convergence. Pont-to-plane, as suggested n [7], solves the dscretzaton problem by not penalzng offsets along a surface. he full overlap assumpton s usually handled by settng a maxmum dstance threshold n the correspondence. Asde from pont-to-plane, most ICP varatons use a closed form soluton to teratvely compute the algnment from the correspondences. hs s typcally done wth [] or smlar technques based on cross-correlaton of the two data sets. Recently, there has been nterest n the use of generc non-lnear optmzaton technques nstead of the more specfc closed form approaches [9]. hese technques are advantageous n that they allow for more generc mnmzaton functons rather then just the sum of eucldean dstances. [9] uses non-lnear optmzaton wth robust statstcs to show a wder basn of convergence. We argue that among these, the probablstc technques are some of the best motvated due to the large amount of theoretcal work already n place to support them. [] apples a probablstc model by assumng the second scan s generated from the frst through a random process. [] Apples ray tracng technques to maxmze the probablty of algnment.

2 [8] bulds a set of compatble correspondences, and then maxmzes probablty of algnment over ths dstrbuton. [7] ntroduces a fully probablstc framework whch takes nto account a moton model and allows estmates of regstraton uncertanty. An nterestng aspect of the approach s that a sampled analog of the Generalzed Hough ransform s used to compute algnment wthout explct correspondences, takng both surface normals nto account for D data sets. here s also a large amount of lterature devoted to solvng the global algnment problem wth multple scans ([8] and many others). Many approaches to ths ([8] n partcular) use a par-wse matchng algorthm as a basc component. hs makes mprovements n parwse matchng applcable to the global algnment problem as well. Our approach falls somewhere between standard IPC and the fully probablstc models. It s based on usng MLE as the non-lnear optmzaton step, and computng dscrete correspondences usng kd-trees. It s unque n that t provdes symmetry and ncorporates the structural assumptons of [7]. Because closest pont look up s done wth eucldean dstance, however, kd-trees can be used to acheve fast performance on large pontclouds. hs s typcally not possble wth fully probablstc methods as these requre computng a MAP estmate over assgnments. In contrast to [8], we argue that the data should be assumed to be locally planar snce most envronments sampled for range data are pecewse smooth surfaces. By gvng the mnmzaton processes a probablstc nterpretaton, we show that s easy to extend the technque to nclude structural nformaton from both scans, rather then just one as s typcally done n pont-to-plane ICP. We show that ntroducng ths symmetry mproves accuracy and decreases dependence on parameters. Unlke the IDC [] and MbICP [6] algorthms, our approach s desgned to deal wth large D pontclouds. Even more fundamentally both of these approaches are somewhat orthogonal to our technque. Although MbICP suggests an alternatve dstance metrc (as do we), our metrc ams to take nto account structure rather then orentaton. Snce our technque does not rely on any partcular type (or number) of correspondences, t would lkely be mproved by ncorporatng a secondary set of correspondences as n IDC. A key dfference between our approach and [7] s the computatonal complexty nvolved. [7] s desgned to deal wth planar scan data the Generalzed Hough ransform suggested requres comparng every pont n one scan wth every pont n the other (or a proportonal number of comparsons n the case of samplng). Our approach works wth kd-trees for closest pont look up and thus requres O(n log(n) explct pont comparsons. It s not clear how to effcently generalze the approach n [7] to the datasets consdered n ths paper. Furthermore, there are phlosophcal dfferences n the models. hs paper proceeds by summarzng the ICP and pontto-plane algorthms, and then ntroducng Generalzed-ICP as a natural extenson of these two standard approaches. Expermental results are then presented whch hghlght the advantages of Generalzed-ICP. A. ICP he key concept of the standard ICP algorthm can be summarzed n two steps: ) compute correspondences between the two scans. ) compute a transformaton whch mnmzes dstance between correspondng ponts. Iteratvely repeatng these two steps typcally results n convergence to the desred transformaton. Because we are volatng the assumpton of full overlap, we are forced to add a maxmum matchng threshold d max. hs threshold accounts for the fact that some ponts wll not have any correspondence n the second scan (e.g. ponts whch are outsde the boundary of scan A). In most mplementatons of ICP, the choce of d max represents a trade off between convergence and accuracy. A low value results n bad convergence (the algorthm becomes short sghted ); a large value causes ncorrect correspondences to pull the fnal algnment away from the correct value. Standard ICP s lsted as Alg nput : wo pontclouds: A = {a }, B = {b } An ntal transformaton: output: he correct transformaton,, whch algns A and B ; whle not converged do for to N do m FndClosestPontInA( b ); f m b d max then w ; else w ; end end argmn { w b m }; end Algorthm : Standard ICP B. Pont-to-plane he pont-to-plane varant of ICP mproves performance by takng advantage of surface normal nformaton. Orgnally ntroduced by Chen and Medon[7], the technque has come nto wdespread use as a more robust and accurate varant of standard ICP when presented wth.d range data. Instead of mnmzng Σ b m, the pont-to-plane algorthm mnmzes error along the surface normal (.e. the projecton of ( b m ) onto the sub-space spanned by the surface normal). hs mprovement s mplemented by changng lne of Alg. as follows: argmn { where η s the surface normal at m. w η ( b m ) }

3 A. Dervaton III. GENERALIZED-ICP Generalzed-ICP s based on attachng a probablstc model to the mnmzaton step on lne of Alg.. he technque keeps the rest of the algorthm unchanged so as to reduce complexty and mantan speed. Notably, correspondences are stll computed wth the standard Eucldean dstance rather then a probablstc measure. hs s done to allow for the use of kd-trees n the look up of closest ponts and hence mantan the prncple advantages of ICP over other fully probablstc technques speed and smplcty. Snce only lne s relevant, we lmt the scope of the dervaton to ths context. o smplfy notaton, we assume that the closest pont look up has already been performed and that the two pont clouds, A = {a } =,...,N and B = {b } =,...,N, are ndexed accordng to ther correspondences (.e. a corresponds wth b ). For the purpose of ths secton, we also assume all correspondences wth m b > d max have been removed from the data. In the probablstc model we assume the exstence of an underlyng set of ponts, Â = {â } and ˆB = { ˆb }, whch generate A and B accordng to a N (â, C A) and b N ( ˆb, C B). In ths case, {CA } and {CB } are covarance matrces assocated wth the measured ponts. If we assume perfect correspondences (geometrcally consstent wth no errors due to occluson or samplng), and the correct transformaton,, we know that ˆb = â () For an arbtrary rgd transformaton,, we defne d () = b a, and consder the dstrbuton from whch d ( ) s drawn. Snce a and b are assumed to be drawn from ndependent Gaussans, d ( ) N ( ˆb ( )â, C B + ( )C A ( ) ) = N (, C B + ( )C A ( ) ) by applyng Eq. (). Now we use MLE to teratvely compute by settng = argmax p(d () ) = argmax log(p(d () )) he above can be smplfed to = argmn d () (C B + C A ) d () () hs defnes the key step of the Generalzed-ICP algorthm. he standard ICP algorthm can be seen as a specal case by settng C B = I C A = In ths case, () becomes = argmn = argmn d () () d d () () whch s exactly the standard ICP update formula. Wth the Generalzed-IPC framework n place, however, we have more freedom n modelng the stuaton; we are free to pck any set of covarances for {C A} and {CB }. As a motvatng example, we note that the pont-to-plane algorthm can also be thought of probablstcally. he update step n pont-to-plane ICP s performed as: = argmn { P d } () where P s the projecton onto the span of the surface normal at b. hs mnmzes the dstance of a from the plane defned by b and ts surface normal. Snce P s an orthogonal projecton matrx, P = P = P. hs means P d can be reformulated as a quadratc form: P d = (P d ) (P d ) = d P d Lookng at () n ths format, we get: = argmn { d P d } () Observng the smlarty between the above and (), t can be shown that pont-to-plane ICP s a lmtng case of Generalzed-ICP. In ths case C B = P (6) C A = (7) Strctly speakng P s non-nvertble snce t s rank defcent. However, f we approxmate P wth an nvertble Q, Generalzed-ICP approaches pont-to-plane as Q P. We can ntutvely nterpret ths lmtng behavor as b beng constraned along the plane normal vector wth nothng known about ts locaton nsde the plane tself. B. Applcaton: plane-to-plane In order to mprove performance relatve to pont-to-plane and ncrease the symmetry of the model, Generalzed-ICP can be used to take nto account surface nformaton from both scans. he most natural way to ncorporate ths addtonal structure s to nclude nformaton about the local surface of the second scan nto (7). hs captures the ntutve nature of the stuaton, but s not mathematcally feasble snce the matrces nvolved are sngular. Instead, we use the ntuton of pont-to-plane to motvate a probablstc model. he nsght of the pont-to-plane algorthm s that our pont cloud has more structure then an arbtrary set of ponts n -space; t s actually a collecton of surfaces sampled by a range-measurng sensor. hs means we are dealng wth

4 Fg.. llustraton of plane-to-plane a sampled -manfold n -space. Snce real-world surfaces are at least pece-wse dfferentable, we can assume that our dataset s locally planar. Furthermore, snce we are samplng the manfold from two dfferent perspectves, we wll not n general sample the exact same pont (.e. the correspondence wll never be exact). In essence, every measured pont only provdes a constrant along ts surface normal. o model ths structure, we consder each sampled pont to be dstrbuted wth hgh covarance along ts local plane, and very low covarance n the surface normal drecton. In the case of a pont wth e as ts surface normal, the covarance matrx becomes ɛ where ɛ s a small constant representng covarance along the normal. hs corresponds to knowng the poston along the normal wth very hgh confdence, but beng unsure about ts locaton n the plane. We model both a and b as beng drawn from ths sort of dstrbuton. Explctly, gven µ and ν the respectve normal vectors at b and a C B and C A are computed by rotatng the above covarance matrx so that the ɛ term represents uncertanty along the surface normal. Lettng R x denote one of the rotatons whch transform the bass vector e x, set C B = R µ C A = R ν ɛ ɛ R µ R ν he transformaton,, s then computed va (). Fg. provdes an llustraton of the effect of the algorthm n an extreme stuaton. In ths case all of the ponts along the vertcal secton of the green scan are ncorrectly assocated wth a sngle pont n the red scan. Because the surface orentatons are nconsstent, plane-to-plane wll automatcally dscount these matches: the fnal summed covarance matrx of each correspondence wll be sotropc and wll form a very small contrbuton to the objectve functon relatve to the thn and sharply defned correspondence covarance matrces. An alternatve vew of ths behavor s as a soft constrant for each correspondence. he nconsstent matches allow the red scanpont to move along the x-axs whle the green scan-ponts are free to move along the y-axs. he ncorrect correspondences thus form very weak and unnformatve constrants for the overall algnment. Computng the surface covarance matrces requres a surface normal assocated wth every pont n both scans. here are many technques for recoverng surface normals from pont clouds, and the accuracy of the normals naturally plays an mportant role n the performance of the algorthm. In our mplementaton, we used PCA on the covarance matrx of the closest ponts to each scan pont. In ths case the egenvector assocated wth the smallest egenvalue corresponds wth the surface normal. hs method s used to compute the normals for both pont-to-plane and Generalzed-ICP. For Generalzed-ICP, the rotaton matrces are constructed so that the ɛ component of the varance lnes up wth the surface normal. IV. RESULS We compare all three algorthms to test performance of the proposed technque. Although effcent closed form solutons exst for n standard ICP, we mplemented the mnmzaton wth conjugate gradents to smplfy comparson. Performance s analyzed n terms of convergence to the correct soluton after a known offset s ntroduced between the two scans. We lmt our tests to a maxmum of teratons for standard ICP, and teratons for the other two algorthms snce convergence was typcally acheved before ths pont (f at all). Both smulated (Fg. ) and real (Fg. ) data was used n order to demonstrate both theoretcal and practcal performance. he smulated data set also allowed tests to be performed on a wder range of envronments wth absolutely known ground truth. he outdoor smulated envronment dffers from the collected data prmarly n the amount of occluson presented, and n the more hlly features of the ground plane. he realworld outdoor tests also demonstrate performance wth more detaled features and more representatve measurement nose. Smulated data was generated by ray-tracng a SICK scanner mounted on a rotatng jont. wo D envronments were created to test performance aganst absolute ground truth both n the ndoor (Fg. (a)) and an outdoor (Fg. (b)) scenaro. he ndoor envronment was based on an offce hallway, whle the outdoor settng reflects a typcal landscape around a buldng. In both cases, we smulated a laser-scanner equpped robot travelng along a trajectory and takng measurements at fxed ponts along the path. Gaussan nose was added to make the tests more realstc. ests were also performed on real data from the logs of an nstrumented car. he logs ncluded data recorded by a roof-mounted Velodyne range fnder as the car made a loop through a suburban envronment and were annotated wth GPS and IMU data. hs made t possble to apply a parwse constrant-based SLAM technque to generate ground truth In our mplementaton we compute these transformatons by consderng the egen decomposton of the emprcal covarance of the closest ponts, ˆΣ = UDU. We then use U n place of the rotaton matrx (n effect replacng D wth dag(ɛ,, ) to get the fnal surface-algned matrx).

5 Fg.. Velodyne scans scan A s shown n green, scan B n red Fg.. (a) ndoor scene (b) outdoor scene Fg.. smulated D envronments (a) ndoor scene (b) outdoor scene ray-traced scans scan A s shown n green, scan B n red postonng. Although (standard) ICP tself was used n the parwse matchng to generate the ground truth, the spacng of scans used for the SLAM approach was an order of magntude smaller. In contrast, the scan pars used for testng were extracted wth much hgher spacng (-+ meters) n order to pose a much more challengng problem. hs s not a perfect method to generate ground truth, but we beleve t provdes a reasonable baselne to make comparsons between the algorthms. o measure performance, all algorthms were run on pars of scans from each of the three data sets. For each scan par, the ntal offset was set to the true offset wth a unformly generated error term added. he error term was set wthn ±.m and ± along all axes. Performance was measured by averagng postonng error over all scan pars for a partcular algorthm. In all cases tested, rotatonal error was neglgble. As mentoned before, selecton of d max plays an mportant role n the convergence of ICP. Fg. shows the average error for dfferent values of d max ; the plot shows average performance across all scan pars. Fg. 9 shows the averages for ndvdual scan pars based on deal values of d max ; t demonstrates the dstrbuton of error across the range of scan pars. In contrast to Fg., the large number off random ntal offsets averaged nto each data pont of Fg. 9 serves to sample the space of possble offsets. For Fg., the algorthms were run on each scan par wth randomly generated startng postons. For the plots n Fg. 9, each data pont was generated wth random ntal poses usng best-case values for d max. σ In all cases, error bars were computed as N. he plots n Fg. show that the proposed algorthm s more robust to choce of the matchng threshold and demonstrates better performance n general. hs s to be expected snce t more completely models the envronment and wll automatcally dscount many ncorrect matches based on the structure of the scene. In partcular, Fg. shows that n the smulated envronments, the accuracy of the algorthm s not senstve to overestmated values of d max. For the real data, Generalzed-ICP s stll shown to be less senstve due to the smaller slope of average error as d max. he dscrepancy between smulated and real data can be explaned by the dfference n ther respectve frequency profles. Whereas the smulated envronments only have hgh-level features modeled

6 Smulated Hallway wth, ponts, cm nose Smulated Outdoor wth, ponts, cm nose. Generalzed ICP Pont-to-plane ICP Standard ICP Generalzed ICP Pont-to-plane ICP Standard ICP max. match dstance [m].. (a) smulated ndoor... max. match dstance [m]. (b) smulated outdoor Velodyne Data wth, ponts. Generalzed ICP Pont-to-plane ICP Standard ICP max. match dstance [m]. (c) Velodyne data Fg.. (a) Intal algnment average error as a functon of dmax (b) Pont-to-plane Fg. 6. (a) Intal algnment Example of results for velodyne scan par # (b) Pont-to-plane Fg. 7. (c) Generalzed-ICP Example of results for velodyne scan par # (c) Generalzed-ICP

7 (a) scan par #, vew Fg. 8. (b) scan par #, vew (c) scan par #, vew Velodyne scan pars # and # shown n perspectve to llustrate scene complexty Smulated Hallway wth, ponts, cm nose Smulated Outdoor wth, ponts, cm nose.... (d) scan par #, vew Scan par # Generalzed ICP, dmax=.m Pont-to-plane ICP, dmax=.6m Standard ICP, dmax=.m Scan par # Generalzed ICP, dmax=.6m Pont-to-plane ICP, dmax=.m Standard ICP, dmax=.8m (a) smulated ndoor (b) smulated outdoor Velodyne Data wth, ponts Generalzed ICP, dmax=.m Scan par # Pont-to-plane ICP, dmax=.m Standard ICP, dmax=.m (c) Velodyne data # - #6 Velodyne Data wth, ponts Generalzed ICP, dmax=.m Scan par # Pont-to-plane ICP, dmax=.m Standard ICP, dmax=.m (d) Velodyne data #7 - # Fg. 9. average error wth deal values of dmax whch mnmze Fg.

8 by hand, the real world data contans much more detaled, hgh-frequency data. hs ncreases the chances of ncorrect correspondences whch share a common surface orentaton a stuaton whch s not taken nto account by our algorthm. Nonetheless, even when comparng worst-cast values of d max for Generalzed-ICP wth best-case values for pont-to-plane, Generalzed-ICP performs roughly as good. As mentoned n Secton II, the d max plays an mportant role n the performance of ICP. Settng a low value decreases the chance of convergence, but ncreases accuracy. Settng a value whch s too hgh ncreases the radus of convergence, but decreases accuracy snce more ncorrect correspondences are made. he algorthm proposed n ths paper heavly reduces the penalty of pckng a large value of d max by dscountng the effect of ncorrect correspondences. hs makes t easer to get good performance n a wde range of envronment wthout hand-pckng a value of d max for each one. In addton to the ncreased accuracy, the new algorthm gves equal consderaton to both scans when computng the transformaton. Fg. 6 and Fg. 7 show two stuatons where usng the structure of both scans removed local mnma whch were present wth pont-to-plane. hese represent top-down vews of velodyne scans recorded approxmately meters apart and algned. Fg. 8 shows some addtonal vews of the same scan pars to better llustrate the structure of the scene. he scans cover a range of 7- meters from the sensor n an outdoor envronment as seen from a car drvng on the road. Because ths mnmzaton s stll performed wthn the ICP framework, the approach combnes the speed and smplcty of the standard algorthm wth some of the advantages of fully probablstc technques such as EM. he theoretcal framework also allows standard robustness technques to be ncorporated. For example, the Gaussan kernel can be mxed wth a unform dstrbuton to model outlers. he Gaussan RVs can also be replaced by a dstrbuton whch takes nto account a certan amount of slack n the matchng to explctly model the nexact correspondences (by assgnng the dstrbuton of d () a constant densty on some regon around ). Although we have consdered some of these varatons, none of them have an obvous closed form whch s easly mnmzed. hs makes them too complex to nclude n the current work, but a good topc for future research. V. CONCLUSION In ths paper we have proposed a generalzaton of the ICP algorthm whch takes nto account the locally planar structure of both scans n a probablstc model. Most of the ICP framework s left unmodfed so as to mantan the speed and smplcty whch make ths class of algorthms popular n practce; the proposed generalzaton only deals wth the teratve computaton of the transformaton. We assume all measured ponts are drawn from Gaussans centered at the true ponts whch are assumed to be n perfect correspondence. MLE s then used to teratvely estmate transformaton for algnng the scans. In a range of both smulated and real-world experments, Generalzed-ICP was shown to ncrease accuracy. At the same tme, the use of structural nformaton from both scans decreased the nfluence of ncorrect correspondences. Consequently the choce of maxmum matchng dstance as a parameter for the correspondence phase becomes less crtcal to performance. hese modfcatons mantan the smplcty and speed of ICP, whle mprovng performance and removng the trade off typcally assocated wth parameter selecton. ACKNOWLEDGMEN hs research was supported n part under subcontract through Raytheon Sarcos LLC wth DARPA as prme sponsor, contract HR--C-7. REFERENCES [] P. Besl, N. McKay. A Method for Regstraton of -D Shapes, IEEE rans. on Pattern Analyss and Machne Intel., vol., no., pp. 9-6, 99. [] P. Bber, S. Fleck, W. Strasser. A Probablstc Framework for Robust and Accurate Matchng of Pont Clouds, Pattern Recognton, Lecture Notes n Computer Scence, vol. 7/, pp. 8-87,. [] N. Gelfan, L. Ikemoto, S. Rusnkewcz, M. Levoy. Geometrcally Stable Samplng for the ICP Algorthm, Fourth Internatonal Conference on -D Dgtal Imagng and Modelng, p. 6,. [] D. Haehnel, W. Burgard. Probablstc Matchng for D Scan Regstraton, Proc. of the VDI-Conference Robotk,. [] Z. Zhang. Iteratve Pont Matchng for Regstraton of Free-Form Curves, IRA Rapports de Recherche, Programme : Robotque, Image et Vson, no. 68, 99. [6] D. Hahnel, W. Burgard, S. hrun. Learnng compact D models of ndoor and outdoor envronments wth a moble robot, Robotcs and Autonomous Systems, vol., pp. -7,. [7] Y. Chen, G. Medon. Object Modelng by Regstraton of Multple Range Images, Proc. of the 99 IEEE Intl. Conf. on Robotcs and Automaton, pp. 7-79, 99. [8] L. Montesano, J. Mnguez, L. Montano. Probablstc Scan Matchng for Moton Estmaton n Unstructured Envronments, IEEE Intl. Conf. on. Intellgent Robots and Systems, pp. 99-,. [9] A. Ftzgbbon. Robust regstraton of D and D pont sets, Image and Vson Computng, vol., no. -, pp. -,. [] B. Horn. Closed-form soluton of absolute orentaton usng unt quaternons, Journal of the Optcal Socety of Amerca A, vol., pp. 69-6, 987. [] S. Rusnkewcz, M. Levoy. Effcent Varants of the ICP Algorthm, hrd Internatonal Conference on -D Dgtal Imagng and Modelng, p.,. [] G. Dalley, P. Flynn. Par-Wse Range Image Regstraton: A Study n Outler Classfcaton, Computer Vson and Image Understandng, vol. 87, pp. -,. [] S. Km, Y. Hwang, H. Hong, M. Cho. An Improved ICP Algorthm Based on the Sensor Projecton for Automatc D Regstraton, Lecture Notes n Computer Scence, vol. 97/ pp. 6-6,. [] J.-S. Gutmann, C. Schlegel, AMOS: comparson of scan matchng approaches for self-localzaton n ndoor envronments, eurobot, p.6, st Euromcro Workshop on Advanced Moble Robots (EUROBO), 996. [] F. Lu, E. Mlos. Robot Pose Estmaton n Unknown Envronments by Matchng D Range Scans, Journal of Intellgent Robotcs Systems 8: pp. 9-7, 997. [6] J. Mnguez, F. Lamraux, L. Montesano. Metrc-Based Scan Matchng Algorthms for Moble Robot Dsplacement Estmaton, Robotcs and Automaton, Proceedngs of the IEEE Internatonal Conference on, pp. 7-6,. [7] A. Cens, Scan matchng n a probablstc framework, Robotcs and Automaton, Proceedngs of the 6 IEEE Internatonal Conference on, pp. 9-96, 6. [8] K. Pull, Mutlvew Regstraton for Large Data Sets, -D Dgtal Imagng and Modelng, 999. Proceedngs. Second Internatonal Conference on, pp. 6-68, 999.