A 3-D Recognition and Positioning Algorithm Using Geometrical Matching Between Primitive Surfaces

A 3-9 RECO�~!TIDN A~O POSITIU~IN� ALGORITHM USING GEOMETRICAL MATCHING 9ETWEEN PRIMITIVE SU~PACES

D. D. PAUGERAS ~nd !~. HEBERT

I.N.R.I.A Domaine de Voluceau, Rocquencourt B.P. l ;l), 781)3 !...E CHES~AY CEDEX F' R A ~C E

object is a set (P(U 1 ))i=l .. I of regions

wll ic h are primitive quadrics), approximations of geometrical surfaces, (e.g. planes or ui being the parameter vector

pf the ith primitive. A matching two descriptions G and G 1 is a pairs (P(Ui),P 1 (Uj)) ·where P and :Jet ween set of P 1 are

primitives of G and G 1 respective!). The parameters Ui and Uj are not expressed in

the same coordinate system because G 1 and G are usually the descriptions of a model and of the observed scene, respectively. More precisely, the paramet e rs of G 1 are calculated in an object-centered fr ame whil e thos e of G 1r e c~ l c ul a t e ~ in a v i ewe r- ce nt e r e rJ f r -u: e . · .~ e s ~1 ·!I l :o 1) t h 11 th 'o .T-It c hing M is c on si stent i f the two paired sets of primitives describe the same object. Since we are de3ling with rigid solids, the matching is perfectly consistent if and only if there exists a r!gid transformation T which maps each primitive P 1 of G 1 onto its corresponding primitive P with respect to the matching M. This transformation T provides the orientation and location of the identified

object of the scene.

The basis of the recognition algorithm is a measure of consistency which allows us to control a tree search procedure. This measure must be compatible with the condition of perfect consistency as defined above. So, we define our

consistency measure by :

where T(Uj) is the vector of parameters of the the primitive the pI ( u I.) J

transformed by T. This measure of consistency involves the estimation of the best transformation defined by the matching in the least-squ3res sense. The algorithms which perform this estimation are presented in part III.

In this paper, we describe an efficient algorithm for 3-0 scene analysis. This algorithm uses a segmentation of the surfaces to be identified into geometrical primitives, the original dat a being obtained Dy a laser range finder. Moreover, the algorithm estimates precisely the location and or i entation of an identified object of the scene. Results are presented on real objects.

INTRDDUCTID~

In applications of Scene Analysis techniques to Robotics it is interesting not only to recognize objects in the scene but also to estimate as accurately as possible their position and orientation. In both cases symbolic descriptions of objects must be available. Such descriptions are usually obtained by segmenting the scene into regions homogeneous with respect to some criterion and represe.nting the result as a grap'l where nodes are regions labelled with features and arcs are relations between regions. In many cases the relations are topological (neighbor, inside ··.. ) and sufficient for recognition but not for positioning. In order to achieve recognition and positioning we need to introduce the constraint of rigidity which is global and therefore not very well suited to the graph representation. Our representation is purely geometrical in the sense that objects are segmented into regions approximated by simple

parameterized primitives such a s pl l!'-'· 11d qu a ~I 7cs [t-)1 w~ich implicitely embed the r1g1d1ty con~ra1nt. The geometrical matcher presented here is part of a general 3-0 Vision system including automatic data gathering, segmentation, model construction and Scene Analysis.

II GEDMETRICAu MATCHING

A geometrical description G of an In our case , we use ~ simple g~ome tr ica l desc ription in whi c h the scene is desc r ibed by a l is t of a l~ os t planar regions wi th the pa r ame t e r s of their f i tt ing pl anes . Th ese desc ription s a re obtaine d by ~ lgorithm s of se gment~t ion [5 - 7 ] w h 1 c h us e t he 3 - f) d a t a o b t a in e d b ) a l ase r range fin de r [7] Mo reo~ e t· , the ac qui si ti on s ys t em pr ovides ~ complete mode l o f eac h object whi c h is a segmen t a tion of the wh o l e sur f ace of the ob j ec t in to pl anar r egions . Pigs.2 and 3 show two examp l es of segmen t a t ion , each shade o f g r ay corresponding to a pl ana r region identi f ied by the a lgor i thm s of segmen tat ion in a view o f the object of f ig .l. Pigs . 2 :Ind 3 cor r espond to two di ff e r en t methods o f segme nt ~ ti on which 3r e ca l. l ed the "Hough tr an s f o rm" and the "reg ion growing ~e t hod" [5-7]

respec ti~ e l). The ref e r enct model of the comp l e t e object is obtained in the same wc1) 'In d is sho wn on f ig . 4.

III EST IMATI O ~ J? THE BEST TRANSPO RMATIO~ -------------------------------------

In lh is part we pr esen t an a lgorithm whi c h computes the best estimate of the tr ansformation T of r e lat io n ( 1 ) . A plane P is represented by two par ame ter s w ~nd d wh e r ~ w is the unit ve c tor normal to P and d is the sig ned distance of the plane P to the origin D. Since the s urf aces that we have segmented are boundari es of solids, we ass um e th~t w is alwa)S oriented to the out side of the object. Por computational purposes, it is convenient to associate to P 3 po in t M of P, therefore d=w.OI~ ( wh e r e "." is the inner product).

Let T=t~R be a transformation with i t s associa ted tran s lation and rotation. Th e not a t io n " t * R" in d i cates t. hat we apply the rot a tion f i rst. This decomposition is not unique in general but. we assume that the ax i s of rotation contains the origin of the reference coordin a te system and thi s condi tion implies the uniqueness of the decomposition. When we apply T to the pl an e P( w,d) we obt~in a new plane P 1 ( w 1 ,d 1 ) wher e :

matching with N pairs, we wh ic h minimize the sum (l) by seek t and R which is given

wh ere W is a weighting factor. This sum

can be decomposed in two terms, a sum over llvi-RvJ.II 2 which allows us to determine the best rotation and a s u1r. o ~ er ldi-dl-~i.t j 2

which determines the tr~n s l a tion. We pr esen t the two algorithms use d for minimizing the se two s um s .

Th e probl em of the estimation of the rotation is the most difficult because a r o t a tion cannot be l inea rly represented b) natural parame ter s suc h as the Euler's ang le s and classical lt ast - s qu ~re ~ethods c~nno t b e directlt appl ied .

Th e first natural representation of rot a tion s is the com posi t io n o f three e l ementary rot a t ions a round the thr ee axis o f coord in a te s .

Th e second repre sentation is obtained by usi ng the ax is w ~nd th e angle r of the rot~tion, the r o t a t io n is then gi ven by the Euler formul ~ :

(4) Rv=v+(l-cos(I·)) wA( w,.v ) +si n ( r ) wf\v Where A is the outer product. In both cases , we have t o ~inimize an express ion o f the form

wh ere p i s the vector of the pa rameters of Rand f is a non-linear function.

Th e resolution of th is minimisation problem uses gr~di e nt-l i k e a lgorithms who se conve rg ence is not e n s ur ed . At l as t, the rot a t ion can be represented by a ~ by 3 matrix R s ubject t o the cons tr ain t

RtR=Id, thi s leads t o an iterative minimisation of a quadr a tic c riterion s ubje c t to si x quadr a tic cons traint s .

We pr esen t now ~ r ep r esen tation wh ic h seems to be the simple s t one according to the minimization probl em and us es the notion of qu a t e rnion. A quaternion q is a pair ( v, s) wh ere vis a vector of E3 and s is a scalar (many other definitions exist, see [8] ). Th e set of quaternion s H is isomorphic to E4 and has a s tucture of non-commutative a lgebra, the produc t of two qu3ternion s bei ng defined by

( G) (~,s) In vectors ~ and the conjugate * ( ~ ' 's ' ) = (\A~ I + s ' v + s v ' 's s ' - v · v ' ) the sequel, we identify the of E3 with th~ quaternions (v,O) scalars s with (O,s) . The q of a quatern io n is: q=(-~,s)

if q=(v,s) and its norm i s given by llqljZ=q

Notice that if q is regarded as a vector of E4, lql is its euclidian norm.

The quaternions can be regarded as a generaliz~tion of the complex num~ers, the vector v in q corresponding to the complex part of a·ib. Since the complex numbers of ~odule 1 represent the rotations of E2, it is natural th~t the sa~e propertt holds in H. More precisely, there exists an isomor phism h fro~ the group of rotations of E3 to the grouo UH/Eq where UH is the group of the quaternions of unit norm and Eq is the equivalence relation :

q Eq q' iff q=-q' or q=q'. Moreover if R is a rotation and q is a :odeme:nt of h(R), then the following relation holds for every vector u in E3 : (8) Ru=q · u · ~ .

(the vector u and the quaternion (u,O) are identified in th is relation.)

This property means th~t rotations and normalized

The equivalence relation Eq reflects the fa c t that a rotation has two axis/angle representations, (w,r) and (-w,-r). The mapping his very simple. Let R be a rotation of axis w ~nd angle r, the associated quaternions ~re :

(9) q=(sin(r/2)w,cos(r/Z)) and -q. In the sequel, we only consider the determination of h(R) such that cos(r/2) is positive and for notation convenience, the vectors of E3 or E4 are identified with 1-row matrices. Notice th .at the rotation of null angle is the quaternion ( J' 1) ·

Converse!), if q=(v,s) quaternion of UH such that s is then the corre:sponding rotation is 3 positive is give:n by

( 1 0) r=2.Arcos(s) and w=v/sin(r/2) if Our minimization problem (5) is a minimization over the set UH, expression to be minimized is : riO. now the

This new form is legal because the norm of the quaternions is an extension of the euclidian norm of E3. Relation (7) can be applied by right multiplying the expression (11) by q which is of unit norm, so expression (ll) becomes

Therefore, the problem is to minimize (12) subject to the constraint q =l. This leads to a simple eigenvalue problem because the expression vi*q-q*vl is a

linear function of the 4-vector q, so exists a 4 by 4 matrix Ai such that

( q is now a l b) 4 matrix.) therefore the criterion P of expression (ll) is a quadratic criterion :

where B= :>--At.A. is a S)mmetric matrix. -..,-- l l l

So, we h3ve to minimize P subject to the constraint j~li=l. This is a c la ssica l problem, [9] 1nd "'mln is the smallest

eigenvalue of B. The solution q . of m1n ( 5 )

is the eigenvector of unit oositive fourth coordinate to the eigenvalue Pmin· norm an.j o f correspond in!]

Thus, we have reduced our initial pro~lem to the c~mputation of the eigenvalues of a symmetric m8trix of dimension four. In order to com plete the algorithm , we have to compute the ~ntrix B which is defined by relation ( 14). We first compute the matrices A. of relation 1

( l 3 ) .

Let q=(w,s) be a qu~ternion, and ~ and~· two vectors of E1, if q'=v~q-q~v' then :

(15) q'=((v'+V)/\ w·s(v-v'),w. ( v'-v) ) a matrix u" is 1ssociated to each vector u of E3, this matrix is defined by

(16) u u'=u' .u 0 for all u' of E3.

Moreover, if u=(u 1 ,u 2 ,u 3 ) then

we derive from relation (4) and definition

so, the matrix B of ( 14) is the sum of the (l9) which are easily derived from relatfon (18).

The matrix Rmin corresponding to the

quaternion qmin=(w,s) is given by

Estimation of the translation -----------------------------

We h3ve to determine the t which minimizes the sum : vector

the planes of the matching M (3). The minimintior o f 5 l eas t-squares pro:>lem and is of rel:Jtion is 'I simp le solved :>y the pseudo-inverse me t hod

Le t N :>e number of rni r s ' A the N

And the es tim a t e of the best tr ans lation acco rding to the c rit erion (2 1) is

And the error is given ~y

The estimation of the tr anslation comp l e t es the a lgorit hm o f es t ima tion of the best transformation r e l a tive to a matching M. Th e co rresponding consis tenc) measure g(M) is given b) (24) g(M)=P . ·W.S .

wh e re W is a weighting f ac tor, P . is the m1n

smalles t eigen v a lue of the d e fin ed by ( 14 ) , gnd S . is m 1n matrix defin ed B by

(23). Simp l e and powerful as it is, thi s te c hnique has se ve r a l weakne sses which 'Ire anal yz ed now. The es tim a tion procedures are not iter a tive in the following se nse if we add a new pair (p'J+l'p~+l) to a

given matching M, we can't use the tr ansforma tion T(M) t o compu te T(M, ( p'J+l'p~+l)) because although the

matrix B of ( 14 ) can be easily updated, its eigenvalues must be recomputed. This increases the time of comput;]tion in the tree-sear~h ~lgorithm. The second prob l em is th -Jt , ._,_ r--,ns f n1~ ·1: ~o n T compu t e1 b y the two previous a lgorithms is not necessarily unique becaus e the determination of the rotation (resp. translation) requires Jt least two pairs of non p3rallel planes (resp. three p3irs o f i ndepe nd ent planes). These constraints can give r ise to indeterminations in the tre e searc h algorithm. This probl em is tha t we . use " i nfinite" planes 'IS primitives.

IV THE MATCH!'JG A~�OR!THM ----------------------

We present now t~ e recognition a lgor i thm whi c h is a sim pl e tr ee - sea r ch procedure [1 0]. Por the s'Jke of c l arit y, we denote the two descriptions to be matched by two sets of l abe l s G=(l. .'II ) i!nd G'=(l.."J') where eac h l abe l corresponds to il plane as d~sc rib erl a~ove. So, , ~atching M is ·1 list of p'lirs (i ,j ) . Moreover, we ass ume th C~ t no li!bel of G or G' appears ~ore th3n once in M and we denote by (t~, (i ,j )) thr" new ma t chi n] obt"'ined by :ddin'] a ne w p:1i r (i ,j ) toM. At l ast , we ass ume that G :1nd G' are t he J esc ription s uf th r; ojse rv t d :: ce n t ·11~j of til ~: Tr> ·Je l nf ·1 n o j j e :: t , r t s pro c t i vel ) . Th e output o f the e~ I go r i t h .~ is J :1: ::It c h in g M w h i c h con t :1 in s the primiti\es of the model identified in th e scene, the corres pon1 i ng optimal tr ans form~tiun whi ch gi \ es the position of the objtct in th e scene and th e qu :J!it) measure g(M) .

Th e bAsic ~lgorithm cgn be described D) a r tcursi ve pro ced ur e MATCH(M,i) who se argumtn t s ~re the c urr en t matching ~ ~nd th !O c urr en t l :liH. l i of G. Th e procedure updates ~ ma t ching Mmin which is the best

one according to th e c riterion g (e q. 2~). Pr oced ure MATCH (M , i)

. Try to find a l abe l j whi c h ha s not been ~lready t es ted such thC~t the er r o r g(M,(i,j)) is 1ccept'!ble.

If none exists then Re turn Else

Th e standard algorithm must be modified in order to t a ke into acco unt the pr oblems inherent to the estimation of g ( M). At fir s t, the eva luation of g (M) is not very f ast beca use it require s some malrix computations such 39 eigenvalues ca lculation s , moreover if a pair (i,j) is strongly inconsistent with a matching M, it is not necessary to compute a precise er ror. So, we use a ne w parameter c(M,i,j) which is ca lled the "loc a l consistency". This parame ter is used as a rough estimate of the geometrical consistency of the new pair (i,j) with Lhe matching M and it enables us to perform a fast elimination of strongly inconsistent pairs.

In our case, the primitives are planes and the local consistency can be defined !:Jy

where

(i ,j) i s some pi!ir in M and the vectors v

a re th e norma l s to the corresponding planes. Thi s re l a tion come s fr om the f ac t tha t the re exis t s a rotation consistent with the pa iri ng ((i, j), (i 0 ,jJ )) if and

onl) i f v . \ . - \_ '· \.' '· 1"'1 -·J"'J Another pos sibility

wh ere R is the rotat io n defined by the 11atching "1. Althoug'"l the y a re 1. e ry simpl e , these two fun c tion s provide a good measure of i ncons i s t enc y and s peed up the a lg orithm drasticall).

Another probl em is the possible ind e t e rmination in the computation of the tr ans formation T de fined b) ( 1 ) . As pr e 1. io usl y mentioned, we need two pa ir s of non-parallel plan es i n order to es tim a t e the rot a tion and thr ee fur the tran s lat ion. It me ans th a t we ha ve to ex pl o r e the f irs t three level s of the tr ee before c hecking the geometric a l consistency. This is the main difference with the 2-D case in whi c h a tr ans format ion can be precisely estimated w i t h on l y u n e p .a i r o f p r i m i t i v e s [ 11] . Thu s we use only the "rot ::~ tional consistency" Pmin defined by relations (5)

and ( 14 ) for the control of the search, the com plete consistency g ( M) being used for se l ec ting the bes t so lution s among se ve ral ones a t the bottom of the tree.

Th e l as t problem is that the es tima t i on of the rot a tion is not "recursiv e " which mer~ns that we cannot use the es t ima ted rotatio n R of M in order to estim::Jte the rotation R' of (M,(i,j)). This is a lso an important difference with the 2-D problem as pre se nted in [ 11 J . Neverthe les s , we can perform so11e calculations in an iterative way. Mor e preci se ly, if B is the matrix defined by rel a tion (14) for the matching M, the new matrix B' corresponding to the matching ( M, ( i,j)) is: B'=B+C(i,j), where C(i,j) is the matrix of relation (19) for the pair ( ·i,vj). Conver se ly, if we remove a

pair (i,j) from the matching M, the new matrix is : B'=B-C(i,j), so we can easily update the least-squares matrix B at each level of the search. Moreover, we store the matrices C(i,j) that have been already computed so that each matrix C is computed only once . At last, if M=((l,j 1 ), ·· ,(b,jL)) is the current

matching at some level of the store the rotation matrices search, we ( R ) - p P-2 ·· L-1

and the errors (P ) p p:2, ·· ,L-l

corresponding to its ancestors in the search,

M 2 : c ( 1, j 1 l, ( 2, Jz l), .. ,

Tni s l as t impro~.ement allows us not t o r ecomput e the pa ramet e r s of the ances t or matchings ' when backtracking occ ur s . In s ummary, matrix comput'ltions occur wh e n we add a ne w pai r to a partial matching, in this case we have to compute the new es tim a te of the rot r~ tion and the co rresponding error , the matrix C defined by relation ( 19) being computed only if the new pair has nev e r be en encoun t e r ed before. Th ese impro·ements give r ise t o a ~. e ry f as t a l go r i thm in s pit e of the complex i ty of the numeri ca l a lg o r i thm s o f Par t. III.

Results a r e now pr ese nted on a simpl e sce ne built wi th the object of fig. 1. Pig. 5 shows the segment~tion of the scene _in planar region s , only regions of high qu a lity are considered. Pigs.6 and 7 s how th e re s ult of the identification of the first part. Th e estimated orientation of the object is shown on fig.6 wher e the bl ack r egions 3re the identified regions of the model. Pig.7 shows the superposition of the id entified regions of the scene (so lid l ines) with the corresponding region s of the model (dotted Jine s) , the superposition uses the es t ima t ion of the transformation. Pinally, the region s 12,2 and l h3ve not been re cognized but are e liminated by a simple s up e rposition test.

Th e computation time is ahout 15 seconds tha t may appear very fJst,but i t illustrates the efficiency of the geometrical compatibility tests. Pin a lly, the preci s ion of the tran s formatio n es tima tion is r a ther good, the mean angle be tween v and Rv' is about 0.04 r adians and the translation error is about 3 mm. These 1.alues are related to the accuracy of the acquisition which is about 1 mm.

As previously mentioned, the recognition method is quite general and can be extended to other classes of primitive s urf aces pro·ided that there exists an algorithm for the estimat i on of the transformation · In this Section, we briefly show how the previous algorithm could be ~xtended to the case of quadric surfaces.

The general equation surface is of a quadric

(27) XtQX+C.X+D:O

A transformation T:t*R maps a quadric

(Q,C ,D) onto a quadric (Q 1 ,c 1 ,o 1 ) such

that

So, i f t~ = ( P(Q. C· D·) p·rar C' D')). is 1' 1' 1 · '1' 1' 1 1=l, .. ,N matching with ~ pairs of quadrics, we hav e to minimize a sum simi lar to (3)

wh e re th ~ Wi' s are weig hting factors and Is a ~ntrix norm defined b)

(-so) !Ia 112=Tdota ) wh e r e Tr ( A) denotes the tr ace o f the rr.atrix A.

Th e tr ans lation can be easily estirr.ated by a line ar l e,s t-squar es method . Th e mos t difficult prohlem is the estima tion of R. Th e expression can ~e simplified by using the definition (30)

and the r e lations Q=Qt and RRt=Id. So, the new e xpression to he minimized is :

Th ~ theory of the Lagrange multipliers can be applied to this problem. Precisely , a quaternion with unit norm is a loc a l extrerr.um of P' only i f there exists a real k such that

Th e partial derivative is ~ polynomial fun c tion of degree three of the ' qi's. We assume that we know a rotation

q 0 , then, we seek qi and q~ which maximize

the criterion (13), the other variables being fixed. This maximization is an elimination between the polynomials Hi's and the constraint. Notice that we compute the absolute maximum of P'(q 1 ,q 2 ,q 3 °,q 4 °)

with respect to the variables q 1 and q 2 .

This method can be iteratively applied to the other variajles. At each step, the new solution q is better than the previous one according to the criterion (>3). So, the method converges to a local maximum of the function P'. The only assumption that we

have made is the existence of an a-priori estimate q 0 ·

This estimate could be obtained by only the corr.puting the tr ansforma tion using planar regions, the matching of qu o:drics being used to ~;J!idate or reject the matching of pl anes .

We have presented an algorithm of 3-D recognition using primitive surfaces whi c h gi~es a precise estimation of the positions of the objects. We are in the process of extendi ng the algorithm to more complex primitive sur f ac es sush as quadric surfaces. Our future work is to include this recognition module in a complete ~ision system including automatic dat a acquisi tion, segmentation, model cons truc tion and scene analysis.

1. D.L. Milgnm, C.M . Bjorklund, "Range Image Processing : PI anar Surface Extrastion", Proc. of the fifth ICPR, pp. 912-919, Miami, Dec l-4, l93Q.
2. R.O. Dud a , D. Nitzan, P. Barret, ",Use of Range and Reflectance Data t o find Pl an'll' Surf ac e Regions", IEEE Trans. On Pattern Analysis 3nd Machine Intelligence, Vol. PAMI-1, No 3, pp. 259-271, Jul) 1979.
3. M. Oshima,Y. Shirai, "A Scene Description Using Three-Dimens ionna l Information", Pattern Recognition, Vol. ll, 9-17, 1979. op.
4. (4] C. A. DanE , Model Builder",
5. I?J D.O. Pau]eras and M. Hebert, "Segmenting range jat.a into planar and quadrat ic patches", CVPR, washington, june B3.
6. "An Object-Centered Three-Dimen sionnal PI1D Dissert'ltion, University of Pennsylvani a , 1982.
7. M. Hebert, J. Ponce, "A New Method for Segmenting Scenes", Proc. Of the sixth ICPR, op.
8. 0 · D · P a u ::1 e r a s e t a l · , "Towards a Plexible Vision Sy stem " in ~£££i ___ ~lsi£~· A. Pugh Editor, Springer-Verlag, 1933.
9. 836-838, Munich, October 1982. 3-D
10. @] ~. Jacobson,
11. I?J G. W. Steward ,
12. "h~£l~!.~.Ll~--A b ~i.!:~£L_~~£!.~" ' Nostrand Ed., 1951. Van

"~1!.£ ~ u £.H£~_i£_~~l!..£_ co !!:.e~.!.~ll£~~" , Academ1c Press, 1973.

• [10]!... Shapiro, R. Haralick, "Structural Descriptions and Inexact Matching", PAMI, Vol. 3, No 3, Sept.. 81, pp. 504-519.
• [1~ N. Ayache,
• "A Model Based Vision System to Identify and Locate Partially Visible Parts", CVPR, Washington, June 83.

recognized regions of the scene.

corresponding regions of the model.

regions eliminated after the recognition.

Fi_~Z· Superposition of the scene and the recogni­ zeamodel.