CORRELATION ICP ALGORITHM FOR POSE ESTIMATION BASED ON LOCAL AND GLOBAL FEATURES

Transcription

1 CORRELATION ICP ALGORITHM FOR POSE ESTIMATION BASED ON LOCAL AND GLOBAL FEATURES Marco A. Chavarra, Gerald Sommer Cogntve Systems Group. Chrstan-Albrechts-Unversty of Kel, D-2498 Kel, Germany Keywords: Abstract: Pose estmaton, ICP algorthm, monogenc sgnal, pre-algnment, global and local features. In ths paper we present a new varant of ICP (teratve closest pont) algorthm based on local feature correlaton. Our approach combnes global and local feature nformaton to fnd better correspondence sets and to use them to compute the 3D pose of the object model even for the case of large dsplacements between model and mage data. For such cases, we propose a 2D algnment n the mage plane (rotaton plus translaton) before the feature extracton process. Ths has some advantages over the classcal methods lke better convergence and robustness. Furthermore, t avods the need of a normal pre-algnment step n 3D. Our approach was tested on synthetcal and real-world data to compare the convergence behavor and performance aganst other versons of the ICP algorthm combned wth a classcal pre-algnment approach. 1 INTRODUCTION The estmaton of the object poston s crucal for an effcent robot-object nteracton n research and ndustral applcatons. In ths context, geometrc algebra has been ntroduced n computer vson as a problem adaptve algebrac language for modelng geometrc related problems, see (Sommer, 21). Ths mathematcal framework was used by (Rosenhahn and Sommer, 25) to formulate the monocular pose estmaton problem and the model representaton. For every model-based pose estmaton or 3D regstraton algorthm, correspondences must be found between model and acqured data. Ths s one of the most challengng problems for ths knd of approaches. The most common and smple soluton s the ICP algorthm ntroduced by (Besl and McKay, 1992), where the mnmal Eucldean dstance constrant s used to fnd correspondences. A comparson of several varants of the ICP algorthms s presented by (Rusnkewcz and Levoy, 21), where the orgnal ICP s combned wth dfferent dstance metrcs and strateges to fnd correspondences and to algn artfcally generated 3D meshes. A trackng algorthm based on template and features data was presented by (Ladkos et al., 27), where the systems changes adaptvely between templates and features to deal wth complex trackng scenaros. The above cted methods assume trackng assumpton condtons. It means that the dsplacement between model and scene s small enough to avod convergence to a local mnmum. The approach proposed by (Shang et al., 27) uses known nformaton about the lmts of the object velocty and the mage frame rate to reduce the space transformaton between every frame. Then, the trackng s transformed nto a classfcaton problem. In the work of (Sharp et al., 22) the ICP algorthm s combned wth addtonal nvarant features lke curvature, moment nvarants and sphercal harmoncs for regstraton of range mages. In (Chavarra and Sommer, 27), structural nformaton from mage and model (convexty, concavty and straghtness of segments) s used as extra correspondence search constrants for monocular pose estmaton. The combnaton of ICP wth such structural features reduces the probablty of beng trapped n a local mnma. That means the algorthm s robust aganst the trackng assumpton up to certan lmts. For larger dsplacements between model and scene, a pre-algnment step s needed to get an ntal rough estmaton of the pose. Then, the ICP algo-

2 rthm can be appled to obtan the fnal pose. In the work of (Brujc and Rstc, 1996), a pre-algnment based on the prncpal component analyss (PCA) s used. Once that the prncpal components of the sets of ponts are computed, the rough pose s obtaned by fndng the 3D pose that algns these man components. Instead of extractng the man components by PCA, (Murno et al., 21) algn the extracted 3D skeletons of the model. A genetc algorthm s used n the work of (Lomonosov et al., 26) to compute the pre-algnment step. All these methods are based on the algnment of the man components or skeletons extracted from sets of 3D ponts. That means, the model ponts and the acqured data are defned n 3D. Ths s not the case for the monocular pose estmaton, where the model s defned n 3D, but the acqured data (contour data) s defned n the 2D mage plane. In order to perform a pre-algnment for the monocular pose estmaton, the components of the data extracted from the mage must be reconstructed n 3D. Another opton s to project the 3D model components onto the mage plane and apply a 2D verson of the above cted methods. In ths paper we present a new correlaton-based ICP algorthm for the monocular pose estmaton of 3D free-form surfaces. It combnes global and local orentaton features n an approach that performs better even for cases where the classcal versons of the ICP algorthm fal (e.g. when the trackng assumpton s not met). Local orentaton nformaton n the mage computed from the monogenc sgnal response (Felsberg and Sommer, 24) and from the projected models are used to descrbe the orentaton of contour segments for mage and model ponts. Instead of the mnmal dstance crtera used by the normal varants of the ICP algorthm, correlaton s used to measure the smlarty (n terms of the local orentaton) of the contour segments. Furthermore, global orentaton s used to algn the projected model data wth the detected contour features n the mage plane. That means, a smple 2D feature algnment s performed. Ths allows to fnd better condtoned correspondence sets even for larger dsplacements (rotatons and translatons) wthout the need of an extra pre-algnment step n 3D. Ths paper s organzed as follows, the mage feature extracton based on the monogenc sgnal s brefly ntroduced n secton 2. Secton 3 descrbes the global feature extracton for mage and projected model contours. The slhouette based pose estmaton and the correlaton based ICP algorthms are presented n secton 4. The 2D feature algnment procedure follows n secton 5. Fnally, the result of several experments made on artfcal and real-world data to valdate the effcency and robustness of our algorthm are presented n secton 6. 2 LOCAL IMAGE FEATURES The monogenc scale-space representaton and phase-based mage processng technques were ntroduced by (Felsberg and Sommer, 24). If p(x; s) and q(x;s) are the flter responses of an mage convolved wth the Posson and conjugate Posson kernels respectvely, local ampltude a(x; s) and local phase r(x;s) are obtaned for a scale s accordng to a(x;s) = q(x;s) 2 + ( p(x;s) ) 2 r(x;s) = q(x;s) q(x;s) arctan q(x;s) p(x;s). (1) The ampltude s related to the local energy of the sgnal (presence of structure). The orentaton and phase are combned n the phase vector. The phase gves nformaton about the local symmetry of the sgnal (type of structure) and the orentaton gves the drecton of the hghest sgnal varance. Once that the ampltude and phase are obtaned for a scale factor s, a contour search algorthm based on the local phase and orentaton s appled to extract the contour segments. By changng the scale factor, low contrast edges can also be detected. Thus, for every contour pont we get as features ts coordnates n the mage and the local orentaton F m = {x m,y m,α m }. 3 GLOBAL FEATURE EXTRACTION The Fourer transform of a closed contour delvers a set of complex coeffcents that can be used to obtan low pass approxmatons of t. If only the frst coeffcent s used, the contour s approxmated by a crcle. As the number of the coeffcents ncreases, a better approxmaton of the contour s obtaned. We use a 2D real valued varant of ths approach ntroduced by (Ln and Hwang, 1987). Then, a closed contour c(t) = f 1 (t)e 1 + f 2 (t)e 2 n the mage plane s approxmated by [ f 1 (t) f 2 (t) ] [ ] a = d + N 1 k= [ ][ ak b k sn( 2kπt d k c k N ) cos( 2kπt N ) ], (2)

3 Fgure 1: Examples of the extracted global orentaton. Upper fgures: projected surface models onto the mage plane (trangle, motor part and power socket) and example of realworld mages. Bottom fgures: extracted contours and the correspondng major and mnor axes. wth a = N 1 N 1 k= f1 (t) d = 1 N N 1 k= f2 (t) a k = N 1 N 1 k= f1 (t)cos( 2kπt N ) b k = N 1 N 1 k= f1 (t)sn( 2kπt T ) c k = N 1 N 1 k= f2 (t)cos( 2kπt T ) d k = 1 N N 1 k= f2 (t)sn( 2kπt T ). (3) The coeffcents a k,b k,c k,d k are called ellptc coeffcents. If only the fst coeffcent s used, the contour wll be approxmated by an ellpse. If the coeffcents are arranged n a matrx, the parameters of such ellpse, whch approxmates the contour, are extracted by sngle value decomposton accordng to: [ ak b svd k c k d k ] = [ ][ ] cosθk snθ k Ak R(ψ). snθ k cosθ k B k (4) The ndexes A k and B k are the lengths of the major and mnor axes of the ellpse and θ k s the angle of the major axs wth respect to the mage axs x. The phase matrx R(ψ), defnes the angular dstance from the man axs to the frst pont of the contour. Thus, for a projected model or a detected mage contour, we obtan as global features F mg = {p mg = [a,d ],θ k }, whch defnes the global poston and orentaton of the contour n the mage plane. Some examples for projected models and real mages can be seen n fgure 1. In the practce, ths method of computng the mayor and mnor axs s very smlar than performng a PCA analyss on the pont covarances. Despte of that, we apply the ellptcal descrptors because of the ablty to compute low pass approxmatons of the contours. Ths wll eventually allows to avod the local mnma problem n some specfc scenaros, see (Rosenhahn et al., 23). Fgure 2: Algorthm for the slhouette based pose estmaton. 4 CORRELATION BASED ICP ALGORITHM An algorthm for pose estmaton of 3D surfaces models was proposed by (Rosenhahn et al., 23), where the 3D slhouette of the model s extracted for every teraton of the mnmzaton process. Orgnally, the classcal ICP algorthm was appled to fnd the pose of the slhouette. The poston of the complete surface model s updated and the process s repeated for a gven number of teratons. We use a smlar dea, but n our approach the 3D slhouette s projected onto the mage plane and ts global and local features are computed n 2D. The algorthm s summarzed n fgure Correlaton as Smlarty Crtera Instead of the Eucldean metrc used by the orgnal ICP algorthm, we use the correlaton as a smlarty measure to fnd correspondences. For a contour segment or range n around a projected model pont x, {x n,,x +n }, we defne a vector contanng the local orentaton values of ths segment as o mod = {α mod n,,αmod +n }. We call t orentaton profle vector and the local orentaton s drectly computed from the projected model ponts. Smlarly, an orentaton profle vector of an mage contour segment (computed from the monogenc sgnal response) s defned as: o mg = {α mg n,,αmg +n }. The smlarty of these profle vectors can be computed by the correlaton matrx as corr(o mg,o mod j,o mod j ) ) = cov(omg, (5) Vmg V mod where cov(o mod j ) s the covarance matrx and V mod, V mg are the varances of mage and model data. The correlaton may vary n a range between -1 and 1, where -1 ndcates perfect negatve correspondence, ndcates no correspondence and 1 ndcates perfect correspondence. For an mage pont wth orentaton profle o mg, the correspondng model pont (wth orentaton profle o mod j MOD) wll be the one wth,o mg

4 Orentaton (degree) Smple orentaton profles Model orentaton Contour orentaton Orentaton (degree) Algned orentaton profles Model orentaton Contour orentaton Fgure 3: Left: 2D feature algnment based on the dfference of global orentaton angles and poston n the mage plane. Angular poston of a contour pont wth respect to the major axs (mddle). Interval wthn the par s consdered to be a good condtoned correspondence (rght). maxmal correlaton, corr(o mg,mod) = max j=1,,n {corr(omg 4.2 Outler Elmnaton,o mod j )}. (6) Nose n the mage and the presence of partal occlusons may cause not well condtoned correspondences that must be elmnated. To acheve that, the angular postons of mage contour θ mg and model ponts θ mod j wth respect to the major axs are used as an extra feature, see fgure 3. Once that a canddate correspondence par has been found, t s rejected f the followng crteron s fulflled θ mg θ mod j > t, (7) where t s a gven threshold value that defnes the nterval wthn whch the correct correspondence must be. The ntroducton of ths crteron n the algorthm reduces consderably the number of correspondence pars as t can be seen n the examples of fgure 7. 5 FEATURE ALIGNMENT IN 2D The prncple of the 2D feature algnment s llustrated n fgure 3. The dotted object represents the projected model and the sold object the detected object n the mage wth ther correspondng man axes wth global features F mod = {p mod,θ mod } and F mg = {p mg,θ mg } respectvely. Frst, the projected model ponts x are algned (rotated and translated n 2D) by the matrx T(φ,t r ). As can be seen n the fgure, the angle φ s the orentaton dfference of the major axes and t r the translaton vector between the respectve centers of mass. Once that the projected model ponts have been algned to the detected mage ponts, the local features (orentaton profles) are computed and the correlaton-based ICP algorthm s appled. The effect of algnng the features s vsualzed n fgure 4. The graphcs show the orentaton profles of two correspondng ponts obtaned wth the Ponts Ponts Fgure 4: Orentaton profles of two correspondng ponts wth smple correlaton (left) and wth addtonal feature algnment (rght). smple correlaton and wth the algned features. Wth algned features the profles are more smlar and therefore better condtoned correspondences are found. Let us notce that n contrast wth the classcal prealgnment approaches where the rough pose s computed n 3D by a mnmzaton approach, the rotaton and translaton n our approach are computed drectly from the global orentaton and poston dfferences. 6 EXPERIMENTS The robustness aganst the trackng assumpton (large rotatons and translatons) was tested and compared wth the normal varant of ICP algorthm (see (Rusnkewcz and Levoy, 21)) and wth the structural ICP algorthm (Chavarra and Sommer, 27). In a second experment, our ICP varant was compared wth an approach based on the PCA prealgnment used n (Brujc and Rstc, 1996) and (Murno et al., 21). In both cases the ntal poston of the model s known, then t s translated to a certan poston (ground truth) and projected onto the mage plane to generate an artfcal mage. On ths artfcal mage the local and global features are extracted. The pose s calculated wth the projectve pose estmaton algorthm of (Araujo et al., 1998) and compared wth the ground truth. Several object models were used for our experments (see fgure 1). Fnally, some examples for a real data scenaro are presented. 6.1 Robustness Aganst Rotatons From the ntal poston of the model, ts man orentaton axes were extracted n 3D. Each axs defnes a rotaton axs α, β and γ, as t can be seen n fgure 5. The model was rotated around these axes and n ts new poston the pose was computed and compared wth the ground truth. The left graphcs of fgure 5, show a comparson of the convergence behavor for the motor and power socket models. In ths case the model was rotated -3 degrees around the γ axs. The

5 Pose error for rotatons Normal ICP Correlaton ICP Convergence for the power socket NormalICP Correlaton Angle alfa Pose error for rotatons 6 Normal ICP 5 4 Correlaton ICP Iteraton Convergence for the motor part NormalICP Correlaton Iteraton Angle beta Pose error for rotatons Normal ICP Correlaton ICP Angle gamma Fgure 6: Pre-algnment comparson of PCA and 2D algnment. Intal poston (upper row), PCA pre-algnment result (mddle row) and computed pose after the frst teraton of the correlaton-based ICP (lower row). Fgure 5: Left: Setup for the experment for the rotaton case (upper fgure) and convergence behavor comparson for the power socket (mddle fgure) and motor part (bottom fgure). Rght: convergence ranges for rotatons around the axes α (upper fgure), β (mddle fgure) and γ (bottom fgure). normal ICP algorthm does not converge to the ground truth pose as t can be seen n the graphcs. In contrast to the structural ICP varant, the number of teratons needed to converge s sgnfcatvely reduced wth the correlaton based ICP. As the model rotates around the 3D axes, ts appearance changes wth respect of the mage plane and therefore ts local structure. Because of that, t s nterestng to analyze the rotaton ranges wthn whch the algorthm s capable to converge to the ground truth pose. The fgure 5 also shows a comparson of the convergence ranges for the power socket model. The normal varant of the ICP algorthm converges for relatvely small rotatons around all axes. Whereas the structural and correlaton based ICP varants allow larger rotatons. In the case of the rotaton axes α and γ the extracted slhouette changes drastcally wth respect of the mage plane as the angle ncreases, therefore the regon where the algorthm converges s smaller. Despte of that, the correlatonbased ICP varant shows larger convergence ranges than the structural and classcal varants. 6.2 Pre-algnment Comparson The PCA based pre-algnment algorthms ((Brujc and Rstc, 1996) and (Murno et al., 21)) algn clouds of ponts n 3D by algnng ts man axes. Ths mples that correspondences between the axes must be found. Wth these correspondences, the algnment s computed by a mnmzaton process that takes n general several teratons. In contrast to that, the correlaton algorthm combned wth the 2D feature algnment needs only one teraton to compute a rough pose. Thus, n the next experments we compare the frst teraton of the correlaton ICP algorthm aganst the pre-algnment based on the prncpal component analyss. Snce we algn model 3D data wth mage detected contours, we use a verson of ths approach whch algns the model n 3D by algnng the prncpal components n the mage plane. The model was translated and rotated around the γ axs from to 5 degrees, see upper row of fgure 6. The mddle row shows the result of the pre-algnment wth the PCA and the bottom row shows the result after the frst teraton wth the correlaton-based ICP. Let us remember that the PCA algnment verson used for the monocular pose estmaton algns the axes of the detected mage contour and the projected model slhouette. In contrast to the 3D case, only two prncpal axes can be extracted and algned n the mage plane. Ths loss of nformaton has the effect that, although the major and mnor axes are roughly algned, the error n 3D s sgnfcantly larger. On the other hand, the result of the correlaton based ICP s consderably better than that of the PCA based algorthm. The table 1 shows the absolute error n 3D of the pose calculaton.

6 Rotaton (degree) PCA CORRELATION Table 1: Error comparson for the pre-algnment step (motor part model). 6.3 Pre-algnment wth Partal Occlusons As descrbed n the last experments, an artfcal mage was generated where some partal occlusons where smulated. Ths can be seen n the fgure 7. Correspondences were found wth the correlaton ICP algorthm wth the outler elmnaton crtera of equaton (7). Addtonally, the threshold value t was vared to see how t affects n the convergence behavor of the algorthm. The results of ths experment are shown n fgure 8. The graphc of the left shows the convergence behavor for dfferent threshold values (from 1 to 5 degrees). A better convergence s acheved for threshold values between 2 and 3 degrees. The rght graphc shows the number of correspondences for every teraton of the algorthm after applyng the outler elmnaton crtera of equaton (7). From ntally 96 pars, the number of correspondences s consderably reduced durng the teratons. pose error (mm) convergence of the pose for the occluson case 6 t=5 degree 5 t=4 degree t=3 degree t=2 degree 4 t=1 degree teraton number of correspondences wth partal occlusons 4 number of correspondences 3 2 t=5 degree 1 t=4 degree t=3 degree t=2 degree t=1 degree teraton Fgure 8: Left graphc: convergence behavor of the algorthm for the occluson case. Rght graphcs: number of correspondences for each teraton. 6.4 Experments on Real World Images Fnally, we appled our algorthm to real mage data. A sngle calbrated camera system provdng gray value mages of 62 x 54 pxels s used. The algorthm was tested on a Lnux based system wth a 3 GHz Intel Pentum 4 processor. Fgure 9 shows some examples taken from dfferent test sequences and examples wth partal occlusons n the mage. The average computng tme per frame for the mage processng module (contour extracton, global and local feature extracton) was 224 mllseconds and the complete pose calculaton process (mage processng plus pose estmaton) was 4.73 seconds. Addtonally to the outler elmnaton crteron of equaton (7), the Eucldean dstance crteron used by (Masuda et al., 1996) was combned wth the outler elmnaton of equaton (7). Correspondence pars are rejected f ther pont-to-pont dstance s larger than 2.5 tmes the standard devaton of the complete correspondence set. 7 CONCLUSIONS AND FUTURE WORK Fgure 7: From left to rght: Artfcal generated mage wth smulated occlusons. Intal poston for the experment. Correspondences wth only the correlaton ICP algorthm. Correspondences after elmnatng the outlers. The presence of partal occlusons affects the local structure of the contour and therefore the orentaton profles of each pont. If only the correlaton correspondence search s appled, a bg number of ll condtoned correspondences are found (see fgure 7). Once they are elmnated, a reduced set of correspondences s obtaned. Despte of that, the remanng pars are the best condtoned correspondences and the pose can be computed wth them. An ICP algorthm based on feature correlaton for pose estmaton of 3D surfaces was presented. The expermental results show that our approach performs more effcently than the normal and structural ICP varants. It also shows better convergence behavor, whch reduces the probablty of beng trapped n a local mnmum durng the mnmzaton process. An mportant feature of our approach s that n the frst teraton of the process, the pose error s smaller than that of the PCA based pre-algnment step. The experments show the convergence lmts of the algorthm when only one camera s avalable. The ntegraton of an addtonal camera would ncrease the vew range over the object and therefore the convergence ranges.the computaton of local and global features n every teraton and the 3D slhouette extracton step

7 Fgure 9: Experments n real mages ncludng the partal occlusons case. Intal poston of the model (left column) and computed pose (rght column). ncrease the computaton tme of the algorthm. Real tme s not reached wth our approach, but the reported computaton tme s a good tradeoff consderng the robustness of the algorthm. A natural extenson for our approach s to adapt the correlaton ICP algorthm and combne t wth the structural ICP varant n a system whch deals wth more complex scenaros lke more general object occlusons, local model deformatons, llumnaton changes or smlar. REFERENCES Araujo, H., Carceron, R., and Brown, C. (1998). A fully projectve formulaton to mprove the accuracy of Lowe s pose-estmaton algorthm. Comput. Vs. Image Underst., 7(2): Besl, P. and McKay, N. (1992). A method for regstraton of 3-d shapes. IEEE Transactons on Pattern Analyss and Machne Intellgence, 14(2): Brujc, D. and Rstc, M. (1996). Analyss of free form surface regstraton. In Internatonal Conference of Image Processng. Lausanne, Swtzerland, pages Chavarra, M. and Sommer, G. (27). Structural cp algorthm for pose estmaton. In 2nd Internatonal Conference on Computer Vson Theory and Applcatons, VISAPP 27, March 8-11, Barcelona, Span, pages , Portugal. INSTICC. Felsberg, M. and Sommer, G. (24). The monogenc scalespace: A unfyng approach to phase-based mage processng n scale-space. Journal of Mathematcal Imagng and Vson, 21(1):5 26. Ladkos, A., Behmane, S., and Navab, N. (27). A realtme trackng system combng template-based and feature-based approaches. In 2nd Internatonal Conference on Computer Vson Theory and Applcatons, VISAPP 27, March 8-11, Barcelona, Span, pages , Portugal. INSTICC. Ln, C.-S. and Hwang, C.-L. (1987). New forms of shape nvarants from ellptc fourer descrptors. Pattern Recognton., 2(5): Lomonosov, E., Chetverkov, D., and Ekárt, A. (26). Pre-regstraton of arbtrarly orented 3d surfaces usng a genetc algorthm. Pattern Recognton Letters, 27(11): Masuda, T., Sakaue, K., and Yokoya, N. (1996). Regstraton and ntegraton of multple range mages for 3-d model constructon. In ICPR 96: Proceedngs of the 1996 Internatonal Conference on Pattern Recognton (ICPR 96) Volume I, page 879, Washngton, DC, USA. IEEE Computer Socety. Murno, V., Ronchett, L., Castellan, U., and Fusello, A. (21). Reconstructon of complex envronments by robust pre-algned ICP. In Proc. Thrd Int. Conf. on 3-D Dgtal Imagng and Modelng, pages Rosenhahn, B., Perwass, C., and Sommer, G. (23). Pose estmaton of free-form surface models. In Mchaels, B. and Krell, G., edtors, 25. Symposum für Mustererkennung, DAGM 23, Magdeburg, volume 2781 of LNCS, pages Sprnger-Verlag, Berln. Rosenhahn, B. and Sommer, G. (25). Pose estmaton n conformal geometrc algebra, part II: Real-tme pose estmaton usng extended feature concepts. Journal of Mathematcal Imagng and Vson, 22:49 7. Rusnkewcz, S. and Levoy, M. (21). Effcent varants of the ICP algorthm. In Proceedngs of the Thrd Intl. Conf. on 3D Dgtal Imagng and Modelng, pages , Quebec Cty, Canada. Shang, L., Jasobedzk, P., and Greenspan, M. (27). Model-based trackng by classfcaton n a tny dscrete pose space. IEEE Transactons on Pattern Analyss and Machne Intellgence, 29(6): Sharp, G., Lee, S., and Wehe, D. (22). ICP regstraton usng nvarant features. IEEE Transactons on Pattern Analyss and Machne Intellgence, 24(1):9 12. Sommer, G., edtor (21). Geometrc Computng wth Clfford Algebras. Sprnger-Verlag, Hedelberg.