REGISTRATION OF 3D POINT CLOUD USING A HIERARCHICAL OBJECT BASED METHOD

Transcription

1 REGISTRATION OF 3D POINT CLOUD USING A HIERARCHICAL OBJECT BASED METHOD Mig. Li a,b, *, Tao. Wag b, Qigqua Li b a NERCMS, Natioal Egieerig Research Ceter of Multimedia Software, Computer School, WuHa Uiversity, Chia - Limig @whu.edu.c b Trasportatio Research Ceter (TRC), WuHa Uiversity, Chia, qqli@whu.edu.c Commissio TS 18, WG V/4 KEY WORDS: Poit cloud Registratio, Slam6D, Lidar, Smart Vehicle, Mobile Mappig ABSTRACT: Poit cloud registratio is a ievitable problem i may applicatios, such as modellig i large scale outdoor eviromet with the poit cloud captured by the movig scaer. How to put these poit cloud ito the same coordiate system fast ad efficietly is the bottleeck to accomplish the 3D modelig applicatios. I this paper, we propose a ew approach which cas register poit clouds automatically ad robustly without pose iformatio. This method makes a extesio of SLAM6D i two aspects: 1) The modified surf descriptor is used to fid correspodig poit pairs to estimate the iitial pose; 2) We use a cotiuous movig style to collect 3D Poit Data ad cosequetly take the velocity of the scaer ito accout. This extesio ca help us limit the accumulatio of error i sca matchig ad be used more widely i may fields. We also evaluated this method by our smart vehicle Smart-Ⅱ ad the results show that the proposed method is very fast ad robustly to solve the problems metioed above. 1. INTRODUCTION With the performace improved ad cost reduced, it s easier to acquire accurate ad dese poit cloud by Lidar over large scale eviromet for may applicatios such as dyamic map geeratio(alle, 2001), 3D recostructio(fruh, 2001). All of these applicatios wat to do the 3D poit clouds registratio fast ad efficietly as a prerequisite for subsequet processes. I this paper we tackle the problem of cosistetly aligig overlappig 3D poit cloud of Velodye HDL-64 Lidar ito a complete model without pose iformatio. A commo solutio to this ca be formulated as a optimizatio problem, that is, usig a iitial pose to solvig for the best rotatio ad traslatio parameters (6 DOF) betwee the datasets such that the distace betwee the overlappig areas of the datasets is miimal. Ufortuately, without a iitial pose estimate, the iterative algorithm is ot goig to work well. Moreover, the poit cloud is captured by HDL-64 laser scaer o movig vehicle i outdoor eviromets. As the scaer is movig at a speed of 36 km/s ad the scaig rate is 10 Hz, so the scaer moves about 1 meter to complete a circle sca. This offset caused by movig must be take ito accout to avoid the locatio error ad improve our ICP registratio result. Due to the oe frame sca of Velodye Lidar cas detect almost all directio of eviromet aroud it, there are eough iformatio to estimate the relevace of each scas of poit cloud. Ad as the speed about the movig vehicle is 30Km/H ad the scaer ruig at 10HZ, therefore the two frames scas are overlapped. It ca be foud may poit pairs to estimate the pose iformatio. Therefore, we propose a ew approach which cas register poit clouds automatically ad robustly i movig eviromets without pose iformatio. This method makes a extesio of 6D SLAM i two aspects: 1. The modified surf descriptor is used to fid correspodig poit pairs to estimate the iitial pose rather tha the odometry to approximately get the startig guess for ICP algorithm. 2. We use a cotiuous movig style to collect 3D Poit Data ad cosequetly take the velocity of the scaer ito accout. This extesio ca help us limit the accumulatio of error i sca matchig ad be used more widely i may fields. I this paper, we have evaluated the proposed method i urba eviromets by SmartV-II, a experimetal vehicle with the HDL-64 sesor mouted o the Chery Tiggo SUV. Ad the experimetal data was collected by it i campus of Wuha Uiversity. I order to evaluate the registratio performace of our method, we compared it with the classic ICP algorithm. This paper is orgaized as follows. I sectio 2, previous work is briefly summarized. The, sectio 3, describes the details of the proposed method, which is divided ito two stages: coarse matchig ad fie matchig. After this, experimetal results are show i sectio 4. For the more, the coclusio ad the possible improvemets i the future are addressed i sectio 5. Fially, the related ackowledgemet is aouced i the ed sectio. 2. PREVIOUS WORK The goal of registratio is to fid the relative positio ad orietatio of oe data set to aother ad the most famous method is kow as iterative closest poit (ICP) algorithm origially developed by Besl ad McKay (Besl, 1992). Sice ICP is a iterative descet algorithm, it requires a good iitial estimatio so as to coverge to the global miimum. Several extesios ad improvemets to the origial algorithm have bee proposed. 3D least squares matchig developed by Akca ad Grue (Akca, 2005) ca also be regarded to be part of this group of algorithms. The iterative algorithms obviously require prior kowledge o the pose of the data. Curretly o defiite compariso exists o the covergece radius of the iterative algorithms. * Correspodig author. (Mig LI, phoe: ; limig751218@whu.edu.c). 387

2 Besides, sice ICP matchig costs most of the time of registratio stage, improvig the rate of covergece is crucial to make registratio faster. To reduce the matchig time, effective features should be foud. The pricipal curvatures.(chua et al 1996) is used to costrai a heuristic search for correspodeces. The ivariats derived from the spi-image(johso, 2000), a histogram of distaces ad agles to earby surface poits, to perform recogitio ad registratio of 3D rage maps. Ivestigate Euclidea ivariat features(sharp et al,2002), ad poit wise correspodeces are chose as the closest poit with respect to a weighted liear combiatio of positioal ad feature distaces which make correspodeces correct more ofte tha correspodeces formed usig the positioal distace aloe. A emergig research topic is 6D SLAM(Nuchter 2004), that is, while mappig, the robot pose is represeted with six DoF. I previous work, Prof. Adreas used a 3D laser rage fider i a stop-sca-match-go-process to create a 3D map of the eviromet by mergig several 3D scas ito oe coordiate system. Ulike the idoor eviromets, our experimets were implemeted i urba area with a early flat groud. Ad istead of usig the commo 2D laser scaer which produces a polylie or a arc i oe sca, the HDL-64 (Velodye) laser scaer ca produce 64 times of it at a cycle like a surface laser scaer. Due to the oe frame sca of Velodye Lidar cas detect almost all directio of eviromet aroud it, there are eough iformatio to estimate the relevace of each scas of poit cloud. Therefore, we propose a ew approach which cas register poit clouds automatically ad robustly i movig eviromets without pose iformatio. 3. APPROACH To create a correct ad cosistet model, the scas have to be registered i oe commo coordiate system. If the vehicle carryig the 3D scaer were precisely localized, the registratio could be doe based directly o the pose iformatio. However, there are two reasos which make the registratio problem more complicated: 1. There is o GPS receiver to get eve the approximate pose iformatio i some applicatios such as ruig tuels. So, the geometric structure of overlappig 3D scas has to be cosidered to get a iitial guess. 2. The cotiuous movig of the scaer ca produce about 1 meter to complete a circle sca. It must be take the velocity of the scaer ito accout to limit the accumulatio of error. Fig.1 Registratio poit cloud o movig Therefore, the feature based coarse matchig ad 6Dslam based fie matchig are used to accomplish the 3D poit clouds registratio. 3.1 Feature Based Coarse Matchig The coarse 3D ( xy,, ) pose iformatio is get from feature-based registratio approaches which extract a modified surf features from the measured data ad attempt to match features i-betwee the separate scas. As usig the objects istead of oly geometric features i our approach to solve the coarse matchig problem, a segmetatio process is straightforward. First we project the raw data ito occupacy grids with each cell havig sizes, ad the calculate the variace of z-value with all poits fallig ito the same grid cell. As we all kow that the variace idicates the tedecy of dispersio of a variable, so if the variace of a grid is below a certai threshold the we treat this grid as groud the delete all poits i it. After the groud segmetatio, poit cloud was divided ito may separate objects. Whe usig the HDL-64 laser scaer, poit cloud acquired i oe sca becomes sparser with the rage grow farther. So we igore the object as log as it beyod a certai distace ad does ot have eough poits (i our approach is 30) to reduce the impact of the oise ad the iheret defect of the 3D poit cloud data. Oce we labeled the objects i successive scas, we the calculate the correlatios betwee two frames to determie the correspodece of these objects. That is to say, two objects correspod to each other whe they have the highest correlatio value. Our applicatio employs a modify SURF feature developed by Herbert Bay (2006) extractio ad key poit matchig based o those features DSlam with Vehicle Movig After the coarse matchig, we do a 6D ICP to calculate the six parameters from curret data set to previous data set. The ICP algorithm was developed by Besl ad McKay ad is usually used to register two give poit sets i a commo coordiate system. where N m Nd E( R, t) i, j mi ( Rd j t) (1) m i1 j1 N ad N, are the umber of poits i the model set d M ad data set D, respectively, ad i, jare the weights for a poit match. The weights are assiged as follows: i, j 1, if mi is the closest poit to d j, i, j 0 otherwise. This ca give us a approximate result like the stop-sca-go style i may previous researches. However, it s ot eough i our special cotiuous movig situatio. As the scaer is movig, we caot igore this movig distace measuremet error. The velocity betwee two scas is V (P 2 2 x, P ) (P P ) 1 x, y, 1 y, 01. (2) P ( P, P ) is the positio i time T ad P x P, P 1 1 ) i 1 time T. As oe circle sca costs 0.1s, we assume that the 1 scaer is movig liearly. The positio of P betwee P ad P ca be calculated by Equ 3:

3 T 360 S P,P' V T x S P,P' cosθ y S P,P' siθ (3) By usig a coarse correspodece process ad a fie process, our approach ca accomplish the 3D poit clouds registratio acquired by HDL-64 laser scaer i urba eviromet precisely, robustly ad efficietly. 4. EXPERIMENTS 4.1 Hardware Used i our Experimets Fig. 2 Sca lie features of 3D poit clouds Because the scaer is movig, the object A measured at positio P is regarded to be measured at positio P ad A seems to be moved from A to A cosequetly (see Fig. 2). After the coarse ICP process, we get the approximate pose iformatio of the scaer at positio P 1, P 2, P N, PN 1.The, we calculate the locatio iformatio of A by a data acquisitio method for HDL-64. So the true locatio of A ca be calculated by equatio 4.The we use this correct locatio iformatio as iput for the fie matchig step. PA PA' A' A x x x P A A' x, y y y P A A' y, (4) Mai processig process ca be demostrated by Fig.3. I the preprocess step, we have doe the segmetatio, surf feature detectio ad RANSAC to fid some correspodig poits ad calculated the iitial pose estimatio for ICP. I the coarse matchig step, a typical ICP iteratio algorithm was take to get the trasformatio ad rotatio iformatio betwee two scas. I the fie matchig step, we correct the poit coordiate accordig to the velocity ad rotatio agel of the scaer. To validate our framework, we have performed experimets of poit cloud registratio for real world datasets which capture by our SmartVII. a autoomous vehicle, with the Velodye HDL-64 sesor mouted o roof of the Chery Tiggo. It equipped with five laser ragefiders, three Itel computer systems ad a custom drive-by-wire iterface developed by Chery Cetral Research Lab. It s drivig decisios through a software that itegrates perceptio, plaig, avigatio ad cotrol. Iside the Velodye HDL-64 sesor, there are 64 lasers mouted o upper ad lower blocks with two groups of receivers. It scas aroud at 10Hz, compose 64 sca lies of differet size of circle, ad delivers 360 degree horizotal field of view ad 26.8 degree vertical field of view. This sesors ca providig more tha 1 millio poits per secod, Therefore, it ca detectio a almost all directio of eviromet aroud it ad providig more data poits per secod tha other desigs. The desity of this poit cloud is gradually thiig from the ceter to surroudig. With this sesor, the test data was collected by it i the FC09 Challege ad i the campus of Wuha Uiversity. Fig. 4 Iside the Our SmartV II 4.2 Test of Feature Based Coarse Matchig Fig. 3 Procedure of registratio 3D poit clouds The proposed modify SURF feature matchig algorithms have bee applied to a data set acquired at the FC09 i Xi A Chia, 2600 scas, ad each cotaiig 2160x64 rage data poits. After pre-processig step, data reductio ad feature detectio. The we use the surf feature matchig to calculate the iitial locatio iformatio ad record them i *.pose files. Due to the ature of the feature descriptor, however, a large part of the false matches also is caused by the symmetry ad self-similarity of the poit cloud structure. Repetitive elemets such as trees, buildig, eve the same occlusio poits by other sick sesor o the roof of Smart VII, ca cause false matches. To exclude these false matches from the registratio we have to segmet o use part of poit cloud such as groud. After the groud segmetatio, poit cloud was divided ito may separate objects. The RANSAC filterig scheme is used to delete other false matches, which radomly a sample of three poit pairs is draw from all Surf matches. By usig those algorithms, the result is quite good. 389

4 Fig.5 Two example of coarse matchig by surf descriptor. The first row are images of the straight course of FC 09 (NSFC Future Challege 2009 of Chia), which two cotiuous scas projected to the occupacy grids, the produce rage image, (a)(b) are matchig result by usig surf descriptor marchig directly. (c)(d) are matchig result by usig modified surf descriptor marchig. The secod row are image of the first curve of FC 09, Similarly, (e)(f) are matchig result by usig surf descriptor marchig directly. (g)(h) are matchig result by usig modified surf descriptor marchig. The Fig.5 is two example of coarse matchig by surf descriptor. Those images are two cotiuous scas projected to the occupacy grids, the produce rage image, The first row are image of the straight course of FC 09 (NSFC Future Challege 2009 of Chia), (a)(b) are matchig result by usig surf descriptor marchig directly. From them, you ca fid some marchig error just at the gree arrow. The same occlusio poits by other sick sesor o the roof of Smart VII i two sca were error marchig by surf descriptor. (c)(d) are matchig result by usig modified surf descriptor marchig. By usig groud segmet ad modify some parameters of surf descriptor, the result is quite good. I first example 67 key poits are extracted from the first image usig the SURF. Of those 7 are matched to correspodig key poits i the secod image based o the descriptor. Oly 6 are cofirmed as valid 3D correspodig tie poits usig groud segmetatio ad RANSAC. The secod row are image of the first curve of FC 09, Similarly, (e)(f) are matchig result by usig surf descriptor marchig directly. From them, you ca fid some marchig error just at the gree arrow. The same occlusio poits by other sick sesor o the roof of Smart VII i two sca were error marchig by surf descriptor. (g)(h) are matchig result by usig modified surf descriptor marchig. By usig groud segmet ad modify some parameters of surf descriptor, the result is quite good DSlam with Vehicle Movig After the coarse matchig step, we do a 6D ICP to calculate the six parameters from curret data set to previous data set. Due to sesor s architecture of Velodye HDL-64, the poit cloud of oe sca will be sparse from the ceter to the surroudig gradually. The poits reductio will be very useful optios for the poit cloud registratio. Figure 6 shows the slam6d with vehicle movig, (a)(b) are the map of the FC 09 ad the real scee. (c) is the raw data of 50 scas. (d) is the coarse matchig step result ad (e) is the fie matchig step result, red rectagle ad purple rectagle describe the differeces betwee them. (f) represets oe sca ad (g)is the fial fie matchig result. From those figure, it ca be see by usig a coarse correspodece process ad a fie process, our approach ca accomplish the 3D poit clouds registratio acquired by HDL-64 laser scaer i urba eviromet precisely, robustly ad efficietly. 390

5 Fig 6. Slam6D with Poit cloud of Velodye HDL-64 Lidar 5. CONCLUSION We preset a ew approach for cosistetly aligig overlappig 3D poit cloud of Velodye HDL-64 Lidar ito a complete model without pose iformatio. This method makes a extesio of 6D SLAM i two aspects: The modified surf descriptor is used to fid correspodig poit pairs to estimate the iitial pose. Ad take the velocity of the scaer ito accout. This extesio ca help us limit the accumulatio of error i sca matchig ad be used more widely i may fields. The, we have evaluated the proposed method i urba eviromets by SmartV-II, a experimetal vehicle with the HDL-64 sesor mouted o the Chery Tiggo SUV. Ad the experimetal data was collected by it i FC 09 (NSFC Future Challege 2009 of Chia). As the more scas are registered, the matchig errors will be accumulated. If the umber of scas is more tha 100, the quality of registratio is decreased obviously. Therefore, this may be our ext research topic. ACKNOWLEDGMENT The authors would like to thak Adreas Nüchter for fruitful discussios ad Qigzhou Mao for collectig the data sets. The paper supported by Natioal Natural Sciece Foudatio of Chia (Key Program ). 391

6 REFERENCES A.Johso, Surface ladmark selectio ad matchig i atural terrai, i: Proceedigs of IEEE Coferece o Computer Visio ad Patter Recogitio, vol.2, Hilto Head Islad, SC, USA, 2000, pp C.Chua, R.Jarvis, 3D free form surface registratio ad object recogitio, It. J.Comput. Visio17 (1) (1996) C.Fruh ad A.Zakhor. 3D Model Geeratio for Cities Usig Aerial Photographs ad Groud Level Laser Scas. I Proc. CVPR, Hawai, USA, December 2001 D.Akca ad A. Grue. A flexible mathematical model for matchig of 3d surfaces ad attributes. Proceedigs of SPIE - The Iteratioal Society for Optical Egieerig, 5665: , G.C.Sharp, S.W.Lee, ICP registratio usig ivariat features, IEEE Tras. Patter Aal. Mach. Itell. 24 (1) (2002) Nuchter, A., Surma, H., Ligema, K., Hertzberg, J., & Thru, S. (2004). 6D SLAM with a applicatio i autoomous mie mappig (pp ). I Proceedigs of the IEEE Iteratioal Coferece o Robotics ad Automatio (ICRA 04), New Orleas, LA. P.Alle, I.Stamos, A.Gueorguiev, E.Gold, ad P.Blaer. AVENUE: Automated Site Modellig i Urba Eviromets. I Proc.3DIM, Caada, May P.Besl ad N.McKay. A method for Registratio of 3 D Shapes. IEEE Trasactios o PAMI, 14(2): , February