3D Laser Scan Registration of Dual-Robot System Using Vision

Transcription

1 3D Laser Scan Regisraion of Dual-Robo Sysem Using Vision Ravi Kaushik, Jizhong Xiao*, William Morris and Zhigang Zhu Absrac This paper presens a novel echnique o regiser a se of wo 3D laser scans obained from a ground robo and a wall-climbing robo which operaes on he ceilings o consruc a complee map of he indoor environmen. Tradiional laser scan regisraion mehods like he Ieraive Closes Poin (ICP algorihm will no converge o a global minimum wihou a good iniial esimae of he ransformaion marix. Our echnique uses an overhead camera on he wall-climbing robo o keep line of sigh wih he ground robo and solves he Perspecive Three Poin (P3P Problem o obain he ransformaion marix beween he wall-climbing robo and he ground robo, which serves as a good iniial esimae for he ICP algorihm o furher refine he ransformaion marix. We propose a novel paricle filer algorihm o idenify he real pose of he wall-climbing robo ou of up o four possible soluions o P3P problem using Gruner s soluion. The iniial esimaion ensures convergence of he ICP algorihm o a global minimum a all imes. The simulaion and experimenal resuls indicae ha he resuling composie laser map is accurae. In addiion, he vision-based approach increases he efficiency by reducing he number of ieraions of he ICP algorihm. 3 I. INTRODUCTION D laser scan regisraion is a widely sudied research opic in roboics communiy [1-3]. Relaive pose beween wo laser scans has been radiionally compued using he ICP algorihm and is varians [1, 4]. However, his algorihm diverges from he global minimum wihou a good iniial esimae of he ransformaion marix and is compuaionally expensive due o large number of ieraions. I is especially rue in he case of large roaion and ranslaion beween wo laser scans. In his paper, we address he problem of compuing he ransformaion marix using a vision-based algorihm, which provides a good iniial esimae for he ICP algorihm o regiser he laser scans. We sudy he scenario of a dual-robo sysem consising of Manuscrip received March 1 s, 009. This work was suppored in par by he U.S. Army Research Office under gran W911NF , and he U.S. Naional Science Foundaion under grans CNS , CNS and IIS Ravi Kaushik is wih he Dep. of Compuer Science, The Graduae Cener, The Ciy Universiy of New York (CUNY, 365 Fifh Avenue, New York, NY ( rkaushik@gc.cuny.edu *Jizhong Xiao is wih he Dep. of Elecrical Engineering, CUNY Ciy College and Dep. of Compuer Science, CUNY Graduae Cener, 138 h Sree and Conven Avenue, New York, NY (Corresponding Auhor, jxiao@ccny.cuny.edu Ph: 1-( William Morris is wih he Dep. of Elecrical Engineering, CUNY Ciy College, 138 h Sree and Conven Avenue, New York, NY ( morris@ee.ccny.cuny.edu Zhigang Zhu is wih he Dep. of Compuer Science, CUNY Ciy College and CUNY Graduae Cener, 138 h Sree and Conven Avenue, New York, NY 10031, ( zhu@cs.ccny.cuny.edu a ground robo and a wall-climbing robo [5] ha operaes on he ceiling. The wall-climbing robo, also known as Ciy-Climber, adops a novel adhesive mechanism based on aerodynamic aracion o he surface. I is capable of operaing on all kinds of surfaces, be i smooh or rough and efforlessly ransi beween walls, ceiling and floor. The advanages include vanage viewpoin from he ceiling for surveillance and inspecion. Boh robos are equipped wih a 3D laser scanner and he wall-climbing robo has a perspecive camera wih op-down view of he ground robo. The laser scanner on he wall-climbing robo augmens he laser scan daa provided by he ground robo and ogeher, hey form a complee laser poin cloud map. The complee 3D map consiss of all surfaces in he environmen, including hose surfaces (e.g., he furniure ops ha are impossible o deec using he laser scanner on he ground robo. We inroduce a vision-based algorihm ha compues he geomeric relaionship (i.e., he ransformaion marix beween he wall-climbing robo and he ground robo. The overhead camera on he wall-climbing robo capures D images of he 3 blinking LED lighs (conrol poins ha are placed on he ground robo. Given he image locaion and he 3D locaion of he conrol poins wih respec o he ground robo coordinae frame, we can esimae he pose of he overhead camera by solving he P3P problem. The P3P problem [6, 7] also known as Locaion Deerminaion Problem (LDP or Camera Pose Esimaion problem provides he relaive pose of he camera wih respec o he hree conrol poins. To dae, all aemps o solve he P3P problem have resuled in muliple soluions, ou of which only one soluion is valid [8]. Earlier, we presened an algorihm ha deermines a valid soluion o he P3P problem when he wall-climbing robo moves in linear rajecory [9]. In his paper, we presen a novel paricle filer algorihm ha probabilisically deermines he valid soluion o he P3P problem when wall-climbing robo moves wih an arbirary rajecory. The error analysis of he vision-based algorihm reveals ha he soluion o he P3P problem is bound o have errors if noisy images are considered. However, he resul from he P3P problem really provides good iniial esimae for he ICP algorihm o furher refine he accuracy of he ransformaion marix. In such a way, he ICP algorihm is guaraneed o converge ino he global minimum and he number of ieraions o regiser he laser scans is reduced compared o ICP algorihm wih no iniial ransformaion inpu. Oher scan maching algorihms can be used for refining he ransformaion marix such as he Normal Disribuion Transform, which requires iniial posiion by

2 preview or odomery [], Angle Hisogram [10]. I is known ha hese algorihms also fail o converge o a global minimum when here is a large roaion and ranslaion ransformaion beween wo scans, which is our case beween he ground robo and he wall-climbing robo (one scan invered wih respec o each oher if he wall-climbing robo is on he ceiling. Secion II inroduces he vision-based algorihm o deermine he rough iniial esimaion of he ransformaion marix beween he wo robos. Firs, we presen he approach used o solve he P3P problem. Then, we inroduce a paricle filer algorihm ha is used o idenify which soluion is he valid soluion o he P3P problem. Secion III presens he error analysis of he vision-based algorihm. Secion IV presens a mehod for denoising of he scan poin cloud and laser scan regisraion using he ICP algorihm wih iniial esimae of he ransformaion marix. The simulaion and experimenal resuls are discussed in Secion V. The conclusion is drawn in secion VI. II. VISION-BASED ALGORITHM A. Robo Coordinae Frames 3D Laser Scanner Z W Fig. 1A P 1 Y G P X G Z G poining upward Y W X W P 3 Camera The hree blinking lighs (P 1,P,P 3 on he ground robo form an equilaeral riangle wih side lengh of m. We se he camera coordinae frame (X-Y-Z on he cener of he overhead camera on he wall-climbing robo. The vision-based algorihm compues he relaive pose of he overhead camera (X-Y-Z wih respec o a coordinae frame se a P 1 (X 1 -Y 1 -Y 1 as shown in Fig.. The camera and laser scanner on he wall-climbing robo are close by and he axes of he wo coordinae frames align in parallel wih zero orienaion displacemen. Also noice ha he (X 1 -Y 1 -Z 1 coordinae frame and (X G -Y G -Z G coordinae frame on ground robo are displaced by a small ranslaion. The ICP algorihm has he capabiliy o deal wih hese small mismaches. This also eliminaes he need for laser-camera calibraion. B. P3P Problem and Gruner s soluion We obain he soluion o he P3P problem, which is he pose esimae of he camera coordinae frame (X w -Y w -Z w wih respec o a reference coordinae frame (X G -Y G -Z G. The conrol poins are spaially known wih respec o he reference coordinae frame. The image locaion of each of hese hree poins is obained from an image snapsho acquired by he camera. The spaial locaion of he conrol poins and is image locaions form he inpu o he Gruner s algorihm (which solves he P3P problem in addiion o camera calibraion [11] parameers, namely focal lengh, principal poin and scale facor. Gruner s soluion [6] o P3P problem leads o solving a fourh order polynomial 4 3 A 4v A3v Av A1v A0 0 (1 From Eqn. 1, we obain up o four soluions, which are lengh of he sides of he erahedron ( s1, s, s3 in Fig. formed by connecing he cener of perspecive (CP of he camera and he hree conrol poins. Afer we compue he lengh of he sides, given he hree poin locaions wih respec o ground robo coordinae frame, we can deermine he posiion of CP (O and he camera s orienaion as described in [7]. A his sage, we have up o four ses of poses of he camera, of which only one of hem is a valid soluion. Fig. 1B Fig.1A. Wall climbing robo and is coordinae frame fixed on he laser scanner. Fig. 1B shows ground mobile robo and is coordinae frame fixed on he laser scanner and he blinking LED lighs (P1, P, P3. For laser scan regisraion, we se he coordinae frames of wall-climbing robo (X w -Y w -Z w and he ground robo (X G -Y G -Z G on heir laser scanners respecively as shown in Fig. 1. Fig.. Terahedron formed by connecing he cener of perspecive of camera (O and he hree conrol poins (P 1, P, P 3 are spaially known wih respec o a reference coordinae sysem (In our case, he ground robo frame.

3 C. Paricle Filer Algorihm o Idenify Valid Soluion The number of soluions obained by solving he P3P problem using Gruner s algorihm always resuls in more han one soluion and up o four soluions. The algorihm presened in his secion ackles he issue of obaining he valid soluion which represens he real pose of he camera. A firs insance, he wall-climbing robo acquires firs se of images of he hree conrol poins (blinking LED lighs on he ground robo. The image is hen processed (image subracion algorihm idenifies image locaions of he hree blinking LED lighs o locae hree conrol poins. Afer solving he P3P problem using he pixel informaion of he hree conrol poins, we obain muliple soluions (i.e., he pose of he overhead camera o he P3P problem. Nex, he wall-climbing robo is commanded o move o a new posiion in any arbirary direcion and he odomery records he new posiion wih respec o is previous pose. A he second insance (ime afer wall-climbing robo moves o a new posiion, he wall-climbing robo capures anoher se of images of he ground robo and is processed o obain second se of soluions by applying he P3P problem. A his ime, we have wo ses of soluions o P3P problem and odomery daa of he wall-climbing robo. We develop an algorihm inspired by paricle filer algorihm [1] o deermine he valid soluion a wo insances from he wo ses of muliple soluions (Refer o Algorihm. 1 Prior Disribuion A firs insance (-1, muliple soluions o P3P problem are considered as paricles and each of hem is assigned an equal weigh. If he number of soluions obained is m, each of he paricles is assigned an equal weigh of 1/m. A his insance, we don know which one is he valid soluion. Predicion sage A second insance (, he predicion sage incorporaes he odomery daa from he wall-climbing robo and predics he new locaion of CP for all paricles from is previous pose a ime -1. The odomery moion model of he wall-climbing robo follows he probabilisic approach where he new predicion samples are drawn from he disribuion px u,x 1, where u is he conrol inpu (odomery, x 1 and x are he pose of he wall-climbing robo a ime -1 and respecively. 3 Odomery moion model Typical odomery measuremens are influenced by roaional error and ranslaional error [13]. The odomery measuremens are represened in he inernal robo coordinae frame. The conrol inpu is given by u x 1 x, y, z ( x x ',y',z' The hypohesized pose of he robo a ime -1 and is given by x 1 (x, y,z and x (x',y',z'. The algorihm for compuing he new samples using he poserior disribuion px u,x 1 is in Algorihm 1. The inpus o he algorihm are odomery inpu u and previous pose x 1. In algorihm 1, he lines 1 and compue he measured roaion and ranslaion of he wall-climbing robo on he ceiling using odomery inpu. Lines 3 and 4 add random noise o he odomery based on experimenal (sysemic error evaluaion. 1 and are decided afer measuring sysemic error (assumed as Gaussian of he odomery. Lines 5, 6 & 7 oupu he new posiion of he wall-climbing robo s camera cener. Noice in line 7 ha he z coordinae remains he same since he wall-climbing robo is assumed o move only on a D plane (ceiling. The new sampled paricles are denoed by x. [m] Algorihm 1 Odomery Sample Moion Model u, 1. arcan( y' y, x' x T ˆ ( x' x ( y' y. 3. sample( 1 T 4. ˆ T T sample( 1 T 5. x x ˆ ˆT.cos 6. y y ˆ ˆT.sin 7. z' z 8. reurn ( x ', y', z' ( x 1 Algorihm Unique Soluion o P3P ( X 1, u, z 1. X 0 (Empy se of new paricles Iniialize paricles weigh (a ime T =0 [. w m ] 1 1 m while (Wall-climbing robo seps do for j = 1 o M (a ime T = 3. x p( x u, x 1 (Sae ransiion model [ n] 4. w p( z x (Sensor model-compue imporance facor 5. [m w ] [m w [m 1 w ] (Merge he weighs 6. w w m (Normalize he weighs j w j1 7. X X x, w (Add o new se end for j j [m] 8. reurn X, x x, where w max( w (Imporance sampling: pick he paricle wih max weigh

4 4 Updae Sage A ime, he wall-climbing robo generaes a new se of [n muliple soluions ( z ] using Gruner s algorihm o P3P. n is he number of soluions a ime. The weigh of each paricle is obained based on he proximiy of he prediced paricle. We choose he radial basis funcion (RBF kernel as weighing funcion (Eqn. 3, ofen used in non-parameric esimaion echniques. The kernel generaes a non-negaive weigh (ranging from 0 o 1, and is dependen on he Euclidean disance, a measure of proximiy beween he predicion paricles x [m] [n and he new esimae z ] from he camera sensor. w m [n K(z ],x [m] exp z [n ] [m] x (3 Fig.3 provides a simulaion resul of he paricle filer algorihm o deermine he valid soluion o P3P problem where he wall-climbing robo moves along a sinusoidal rajecory. A four insances, he pose of he camera is calculaed by solving he P3P problem and applies he paricle filer algorihm o idenify, which is he valid soluion a each insance. III. ERROR ANALYSIS OF VISION-BASED ALGORITHM In his secion, we consider errors ha affec he vision-based algorihm s performance. There are wo kinds of errors in he inpu ha can affec he final soluion: one is he pixel error in he images, which is usually Gaussian noise, and he oher error is in he locaions of he hree conrol poins. The second error simply canno be oleraed as he Gruner s algorihm does no perform well wihou accurae measuremen of he disance beween he conrol poins. The pixel error sems from he Gaussian noise added o images. We simulae he pixel error by adding Gaussian random noise o he D image locaions of hree conrol poins. We use Roo Mean Square (RMS esimaion error o represen he difference beween he rue posiion and he esimaed posiion, given by X ˆ X 1/ es E (4 N where X ˆ ( x ˆ, y ˆ, z ˆ is he esimaed posiion and X (x,y,z is he rue posiion, N is he number of simulaion runs (N = The able 4 below provides RMS esimaion error for varying pixel error and focal lengh of he camera. Table 4 indicaes ha he focal lengh is inversely proporional o he RMS esimaion error and pixel error is direcly proporional o he RMS esimaion error. Focal Lengh Esimaed Error ( Esimaed Error ( Pixel Error: Gaussian noise wih = m m m Pixel Error: Gaussian noise wih = m m m Pixel Error: Gaussian noise wih = m m m Table 1. Esimaion error (mean and variance of he soluion o he P3P problem wih varying focal lengh of he camera and pixel error Fig. 3. The soluions o he vision-based algorihm. Blue markers (, *,, o indicae wall-climbing robo s real posiion as i moves in an arbirary rajecory and green markers (, *,, o indicae invalid posiion a four insances. (+ indicaes he hree blinking LEDs on he ground robo. The wall-climbing robo movemen can be seen following a sinusoidal wave in agreemen o programmed inpu.

5 IV. LASER SCAN FUSION USING ICP ALGORITHM The soluion obained from he vision-based algorihm is error-bound and herefore provides a good approximaion of he real pose as described in Secion II. The vision-based algorihm has wo disinc advanages: 1 I runs exremely fas (i ook abou sec o execue in MATLAB on PC wih Inel Core CPU and I provides an iniial rough esimae o he ICP algorihm. A. Preprocessing Laser Scan Daa We obain ( D scan poins from laser scanner on he ground robo and he wall-climbing robo. The ime complexiy of he ICP algorihm is O(n, where n is he number of scan poins. In addiion, he laser scan daa conains noisy daa and ouliers ha inhibi ICP algorihm from converging o he global minimum. Hence, we employed denoising of he laser scan daa. The noisy daa may be seen as he smalles possible disance (0.19m o maximum disance (4m among he oher laser scan poins. We assume ha he neighboring poins do no change posiion drasically. We idenify he oulier based on proximiy o is neares neighbors and remove he ouliers if hey are far away from is neighbors. Finally, we compue he ransformaion marix using ICP algorihm [1]. Noe ha he ICP algorihm eliminaes he need o perform camera/laser calibraion. V. EXPERIMENTAL RESULTS In our experimens, he wall-climbing robo operaes on he ceiling and he ground mobile robo operaes on he floor as shown in Fig. 6B. In he firs insance, he wall-climbing robo akes burs images of he ground robo and is processed o obain he locaions of he hree conrol poins. The Gruner s algorihm is used o compue muliple soluions of he pose of he overhead camera. In he second insance, he wall-climbing robo moves o a new posiion and akes anoher round of burs images of he ground robo and is processed. Gruner s algorihm compues anoher se of soluions a he second insance. The paricle filer algorihm picks he soluion ha is valid in boh insances. Finally, he ground mobile robo and wall-climbing robo acquires a 3D laser scan of he indoor environmen. We presen he experimenal resuls in he form of a laser poin cloud. The red dos indicae he ground mobile robo s laser scan and he blue dos indicae he wall-climbing robo s laser scan. Fig. 4 indicaes no ransformaion applied o he laser scans. The wall-climbing robo is invered by 180 abou x or y-axis and anywhere beween abou z-axis wih respec o he ground mobile robo. The able op remains a he op since he wo scans are displaced. Wih no P3P iniializaion, he ICP algorihm performs poorly (Fig. 5 and considers closes fi of he poins wihou considering he roaion of 180 abou he Y axis (invered beween he wo scans. I is possible o acquire an iniial pose esimae of a robo using orienaion sensors, bu a good pose esimae beween a ground robo and he wall-climbing robo is currenly only possible wih he vision localizaion presened here. Furher, he ICP algorihm refines he ransformaion marix based on he iniial esimae provided by he vision-based algorihm. Afer applying he resuling ransformaion marix, he wo laser scans are fused very well (Fig. 6A. Translaional error was negligible. Roaional error was small (1~ in scan maching in one axis as seen in Fig. 7B, which can be aribued o sensor noise. I is no possible o provide quaniaive measure by comparing closes poins since we don know he corresponding poins. However, he below able summarizes he ground ruh and he ICP oupu wih and wihou P3P iniializaion. Roaion Marix - R Angles (deg Translaion (m No Iniializaion x = y = z = P3P Iniializaion x = y = z = Ground Truh x = y = z = Table. Experimenal resuls showing relaive pose beween he ground robo and wall-climbing robo laser scans Fig.4. Unmached scans from he wall-climbing robo (blue and he ground mobile robo (red shown in he coordinae frame of he laser scanner Fig.5. shows mached scans using jus ICP algorihm wih no P3P iniializaion. The scans are sill invered by 180 abou Y axis in a righ hand coordinae sysem

6 (A (B Fig. 6A. Transformaion (R, T obained from vision-based algorihm brings he wo laser scans mached accuraely. 6B shows he experimenal seup REFERENCES (A (B Fig. 7A shows he side view of he maching scans afer applying ransformaion resul (obained from ICP algorihm iniialized by he vision-based algorihm 7B shows a op view of he same. VI. CONCLUSION This paper presens an inegraed approach and real experimenal demonsraion for consrucing a complee 3D laser map. The ransformaion marix beween he wall-climbing robo and he ground robo is obained by solving he P3P problem applied o he dual-robo sysem. We also inroduced a paricle filer algorihm ha idenifies he valid soluion o he P3P problem. Our conribuion is wo fold. Firs, we are he firs o use a ground mobile robo and he wall-climbing robo o consruc complee 3D maps in real ime, which preserve mos deails of ceiling, ground and furniure op surfaces in an indoor environmen. Second, we use a vision-based algorihm o deermine he relaive pose beween he wall-climbing robo and he ground robo, and serves as a good iniial esimae of he ransformaion marix for he ICP algorihm o compue ransformaion beween he wo laser scans. Our approach leads o improvemens in speed and accuracy in fusing 3D laser scans in muli-robo sysems. We are furher exending our work o build indoor maps using vision-based algorihm in a dynamic dual-robo sysem. [1] P. J. Besl and N. D. McKay, "A Mehod of Regisraion of 3-D shapes," IEEE Transacions on Paern Analysis and Machine Inelligence, vol. 14, 199. [] E. Takeuchi and T. Tsubouchi, "A 3-D Scan Maching using Improved 3-D Normal Disribuions Transform for Mobile Roboic Mapping," in IEEE Inernaional Conference on Inelligen Robos and Sysems, Beijing, China, 006. [3] A. Diosi and L. Kleeman, "Laser Scan Maching in Polar Coordinaes wih Applicaion o SLAM," in IEEE Inernaional Conference on Inelligen Robos and Sysems, Edmonon, Canada, 005. [4] C. A. Kapousis, C. P. Vavoulidis, and I. Pias, "Morphological Ieraive Closes Poin Algorihm," IEEE Transacions on Image Processing, vol. 8, [5] M. Ellio, W. Morris, A. Calle, and J. Xiao, "Ciy-Climbers a Work," in Inernaional Conference on Roboics and Auomaion, Ialy, 007. [6] R. M. Haralick, C.-N. Lee, K. Oenberg, and M. Nolle, "Analysis and Soluions of he Three Poin Perspecive Pose Esimaion Problem," in Proceedings of Compuer Vision and Paern Recogniion, [7] M. A. Fischler and R. C. Bolles, "Random Sample Consensus: A Paradigm for Model Fiing wih Applicaions o Image Analysis and Auomaed Carography," Graphics and Image Processing, vol. 4, pp , [8] L. Quan and Z. Lan, "Linear n-poin camera pose deerminaion," IEEE Transacions on Paern Analysis and Machine Inelligence, [9] Y. Feng, Z. Zhu, and J. Xiao, "Self-localizaion of a Heerogeneous Muli-Robo Team in consrained 3D Space," in IEEE Inernaional Conference on Inelligen Robos and Sysems, 007. [10] G. Weib, C. Wezler, and E. V. Pukamer, "Keeping Track of Posiion and Orienaion of Moving Indoor Sysems by Correlaion of Range-Finder Scans," in IEEE Inernaional Conference on Inelligen Robos and Sysems, [11] K. Srobl, W. Sepp, S. Fuchs, C. Paredes, and K. Arber, "Camera Calibraion Toolbox for Malab," 006. [1] S. Thrun, W. Burgard, and D. Fox, Probabilisic Roboics: The MIT Press, 005. [13] D. Fox, W. Burgard, F. Dellaer, and S. Thrun, "Mone Carlo Localizaion: Efficien Posiion Esimaion for Mobile Robos," in Proceedings of he Sixeenh Naional Conference on Arificial Inelligence, 1999.