Abstract metric to nd the optimal pose and to measure the distance between the measurements

Transcription

1 3D Dstance Metrc for Pose Estmaton and Object Recognton from 2D Projectons Yacov Hel-Or The Wezmann Insttute of Scence Dept. of Appled Mathematcs and Computer Scence Rehovot 761, ISRAEL Abstract Model based object recognton and model based pose estmaton requre a dstance metrc to nd the optmal pose and to measure the dstance between the measurements and possble models durng the recognton process. When the measurements are gven n 2D (such as n orthographc and perspectve projectons) the commonly used dstance between the 3D model features and the 2D mage features s the 2D Eucldean dstance measured n the mage plane. However, ths 2D dstance does not, usually, ncrease monotoncally wth the real 3D dstance and thus does not really represent the dstance beng measured. In ths paper we propose a new scheme n whch both the optmal postonng and the evaluaton of smlarty between the 2D mage and the model s performed relatve to the 3D dstance. Ths dstance s calculated between the model features and a 3D predcted object whch s a permssble reconstructon of the measured object and s the \closest" to the model features. 1

2 1 Introducton Model based object recognton and model based pose estmaton are two complementary problems that arse frequently n the vson lterature (for example [8, 9, 5, 4, 12, 6]). In model-based pose estmaton the poston n 3D space (translaton + orentaton) of a known object s determned from the object measurements. In model-based recognton a measured object s compared to a lbrary of prototype models n order to nd the model whch sthe \closest" to the vewed object. Commonly, ths process requres the estmaton of the best model postonng followed by a measure of smlartybetween the model and the measured data. Both the recognton process and the pose estmaton process, use some dstance metrc to nd the optmal pose and later to measure the dstance between the measurements and the hypothess model. Many applcatons use feature ponts such as maxmum curvature, segmentendponts and corners as a model descrpton. In these applcatons, when measurements are gven n 3D (such as range nder or stereo data), Eucldean dstance between the 3D model features and the correspondng 3D measured features s calculated to gude the optmal pose estmaton and the recognton process [11, 8, 5, 2]. If fx g =1:::n and fx g =1:::n are two setsof3d vectors representng the locatons of the model-ponts and the measured ponts respectvely, then the 3D Eucldean dstance between these two setss: D 3 = X kt (X ) ; X k 2 where T s the rgd transformaton representng the model pose. Ths dstance metrc s reasonable snce t descrbes the amount of3d dstorton a model has to be undergo n order to t the measurements. In model-based pose estmaton we am to mnmze ths dstance subject to the transformaton T and n recognton tasks the mnmzaton s performed subject to T and the model fx g. More frequently, recognton tasks deal wth measured features gven n 2D (projected mages). In ths case a commonly used dstance between the 3D model features and the 2D mage features s the 2D Eucldean dstance measured n the mage plane. If fm g =1:::n are 2D vectors representng the locatons of n correspondng measurements then the 2D Eucldean dstance s dened as: D 2 = X kt (X ) ; m k 2 2

3 where s a projecton operator. In ths case, the measured features n the mage are compared wth the model features projected onto the mage plane. The mnmzaton of ths 2D Eucldean dstance s used to gude the postonng and the recognton processes [12, 3, 14, 15, 7]. However, the problem wth ths 2D dstance s that n most cases, ths dstance does not ncrease monotoncally wth the real 3D dstance and thus does not really represent the 3D dstortons the model must undergo n order to t the measured object. Ths nadequacy of the 2D dstance causes mprecsons n the recognton and postonng especally when dealng wth perspectve projecton. For example, n perspectve mages, a 2D dstance between a measured pont and ts assocated projected model pont canhave varyng values n the real 3D dstance, accordng to the depth of these ponts n 3D. Therefore, the relatve contrbuton of ths par n the total 2D dstance may der from ts contrbuton n the total 3D dstance. The same knd of problem arses when dealng wth perturbatons n the measurements or n the model features. These perturbatons nuence derently the 2D dstance and the 3D dstance. In ths paper we propose a new scheme whch, gven a 2D mage and a canddate model, calculates the optmal postonng of the model and evaluates the \smlarty" between the 2D mage and the model. Both, the optmal postonng and the evaluaton of smlarty s performed relatve to the 3D dstance. Ths dstance s calculated between the model features and a 3D predcted object whch s a permssble reconstructon of the measured object. More precsely, we calculate the mnmum dstorton of the model whch produces a permssble reconstructon of the measured features, where a reconstructed object s consdered permssble f t does not contradct the measured data. The permssble reconstructed object whch s the \closest" to the canddate model (n terms of mnmum dstorton) s the predcted object. In ths paper we descrbe a method that deals wth recognton and pose-estmaton tasks based on 3D dstance metrc. In ths scheme we nclude uncertanty both, n the measurements and n the model features. We show that the superorty of our scheme s manly when the uncertanty n the model s sgncant. Ths characterstc becomes mportant when modelng non-rgd objects or when dealng wth uncertan models whch are used to mprove recognton of a partcular nstance of a class dened by a general model. The rest of ths paper s organzed as follows: Sectons 1 and 2 propose the general framework for dealng wth nose free measurements and precse model denton. Sectons 3

4 3,4, and 5 extend the proposed framework to deal wth uncertan models and nosy measurements. Sectons 6 and 7 present the process for pose estmaton and recognton. Some smulated results are gven n Secton 8. 2 Dentons A model of a 3D object s represented by asetofponts: fx g =1n where X =(x y z ) t s assocated wth the locaton of the th model pont and represented n an object-centered frame of reference (x y z ). A transformed model s the coordnates of the model ponts as gven relatve to the vewercentered frame of reference (x y z). If T denotes the rgd transformaton between the object-centered frame and the vewer-centered frame then the transformed model s descrbed by the collecton: fx g =1n where X =(x y z ) t = T (X ). A measurement of a 3D object s a set: fm g =1n where m represents a measurement ofthe th feature pont of the object. Ths paper deals wth the case where the measurements are obtaned from a projecton of the object onto a 2D mage plane. Thus, m =(v w ) t s represented n the mage frame of reference (v w). A predcted object s a set of 3D coordnates of the form: fu g =1n where U s an estmate of the th feature pont of the object and represented n the vewercentered frame of reference (see Fgure 1). The predcted object s the \closest" object to any transformed model satsfyng the mage constrants. Formally, we choose such an object fu g whch mnmzes the followng quantty: C = nx kt(x ) ; U k 2 (1) 4

5 z z x y T(x ) U lne(m ) v w m f y x Fgure 1: General conguraton of a perspectve projecton. The frames (x y z ), (x y z) and(v w) represent the object-centered, the vewercentered and the mage frame of reference, respectvely. under the set of constrants: project(u )=m for each =1n (2) where project(u ) s the projecton of U onto the mage plane and where the transformaton T s any rgd transformaton. 3 The Predcted Object and the Optmal Transformaton Mnmzaton of C n Equaton 1 requres evaluaton of both, the optmal transformaton T and the \closest" predcted object fu g =1n. However, knowng the optmal transformaton, t s straghtforward to nd the predcted object. In the followng we elaborate on ndng the optmal transformaton T wth respect to any predcted object. Denote by lne(m ) the collecton of ponts n 3D space where the feature pont U can 5

6 be located.e. lne(m )=fv j project(v) =m g : If the measurement s an orthographc projecton onto the plane z = then lne(m ) s a lne parallel to the z axs and passng through the pont (m t ) t. If the measurement s a perspectve projecton, as depcted n Fgure 1, then ths lne passes through the focal pont ( ) and the pont (m t f)where f s the focal length. If Q s a pont n3d we denote by kq ; lne(m )k 2 the squared Eucldean dstance between lne(m ) and the pont Q. Lemma: If fu g s the optmal predcted object mnmzng Equaton 1, then: kt(x ) ; U k 2 = kt(x ) ; lne(m )k 2 for each =1n : Proof: By denton kt(x ) ; lne(m )k 2 = mn kt(x ) ; Vk 2 V2lne(m ) However, due to the mage constrant U 2 lne(m ). Snce U s chosen s.t. kt(x ) ; U k 2 wll be mnmzed, we have: kt(x ) ; U k 2 = mn kt(x ) ; Vk 2 V2lne(m ) and thus kt(x ) ; U k 2 = kt(x ) ; lne(m )k 2 : 2 From the lemma t s clear that the optmal transformaton ^T can be derved as follows: ( ^T =arg mn T nx kt (X ) ; lne(m )k 2 ) : After ^T s calculated, the second stage s to nd the predcted object. each feature pont s chosen so that Equaton 1 s mnmzed, thus: The locaton of ( ) U = arg mn k ^T(X ) ; Vk 2 V2lne(m ) for each =1n : 6

7 d z c b e f g a y x Fgure 2: Perspectve projecton of nose. Smlar nose n the model ponts (d and e) are projected derently onto the mage plane (f and g), dependng on the dstance from the focal pont. Conversely: nose n the mage plane (a) can have avaryng nuence on the predcted object, accordng to the depth of the predcted pont (b and c). 4 Uncertanty n the Model and n the Measurements In the prevous secton no knowledge about the measurement uncertantes and the model uncertantes was assumed. Snce we work n a nosy envronment, t s mportant tocharac- terze these uncertantes especally when the measurements are taken followng a perspectve projecton. Wth ths knd of projecton, smlar nose n the model ponts (d and e n Fgure 2) are projected derently onto the mage plane (f and g n Fgure 2), dependng on the dstance from the focal pont. And conversely: nose n the mage plane (a n Fgure 2) can have avaryng nuence on the predcted object, accordng to the depth of the predcted pont (b and c n Fgure 2). In ths secton we consder measurements and model ponts whch are assocated wth some uncertanty. That s, each measurement snow represented by apar: measurement()=(^m ) 7

8 where ^m s the actual measurement of a real value m s.t. ^m = m + ".We assume that the nose term " s of zero mean and ts covarance matrx s known. The covarance matrx depcts the uncertanty n the actual measurement and s manly due to two factors: Uncertanty due to measurement nose (e.g. dgtzaton, blurrng and chromatc aberratons). Uncertanty dependent upon the feature detecton process. For example, a detected end-pont of a lne segment wll have low postonal uncertanty n the drecton perpendcular to the lne segment and a hgh uncertanty n ts drecton. Smlar to the measurements, we assocate for each model-pont acovarance matrx that each one of the model ponts s represented by a par: so model-pont() =(^X ) where ^X s an estmated locaton of the th model pont wth a 3 3covarance matrx. Ths covarance matrx denotes the uncertanty of the estmated locaton ^X that may arse from three sources: Uncertanty due to mprecse modelng of the 3D object. Uncertanty due to modelng a class of 3D objects, for example, modelng a general face. Uncertanty due to modelng non rgd objects such as rubber objects. In some cases there s no knowledge about the nose n the measurements or n the model. In such cases we assocate an dentcal covarance matrx wth each measurement or model-pont suchthat = I or = I where I s the dentty matrx and are some scalars. When dealng wth an uncertan model and nosy measurements t s clear that t s erroneous to nd a predcted object whch mnmzes C of Equaton 1. In ths equaton all terms added n the rght-hand sde nuence the soluton equally where n our case the 8

9 nuence should be nversely proportonal to the uncertanty ofeach term. Therefore the predcted object has to be calculated subject to a Mahalanobs dstance: C = nx [(T (X ) ; U ) t W ;1 (T (X ) ; U )] (3) where W sa33 covarance representng the uncertantyof(t (X );U ). Ths uncertanty can be deduced from and as wll be explaned later on (Secton 7.1). It should be noted that even f we don't have any aprorknowledge about the uncertanty n the model and n the measurements so that all are dentcal and all are dentcal as well, the term C n Equaton 1 s ncorrect and the transformaton T should be found subject to mnmzng Equaton 3. Ths s true snce W n Equaton 3 depends also on the depth of each pont from the mage plane as can be seen n Fgure 2. 5 The Mahalanobs Dstance to an Uncertan Lne Accordng to the lemma gven n Secton 3, fu g n Equaton 3 can be replaced by flne(m )g. However, n ths case, these lnes are uncertan snce they are deduced from uncertan measurements. Therefore, we are actually nterested n the Mahalanobs dstance (M.D.) between the transformed model to these uncertan lnes. Ths secton elaborates the representaton of an uncertan lne and the measure of the M.D. to t. Let u be a 3 dmensonal vector of random varables where Efug = ^u and varfug = u. Efug denotes the expectaton value of u and varfug = Ef(u ; ^u) t (u ; ^u)g denotes ts 3 3 covarance matrx. If p s some pont n 3D space (wth zero uncertanty) then the squared Mahalanobs dstance between p and ^u s dened as: d 2 =(^u ; p) t ;1 u (^u ; p) Assume the covarance matrx u s dagonal, thus there s no correlaton between the components of u: u = 2 x 2 y 2 z If we set 2 z to be an nnte value then the contours of constant M.D. from ^u wll be ellptc cylnders parallel to the z axs centered at ^u (See Fgure 3). The cross secton of these 1 C A 9

10 (x,y) = (u ^,u ) ^ x y u^ constant Mahalanobs dstance z y x Fgure 3: The Mahalanobs Dstance to an uncertan lne. When the uncertan lne s parallel to the z axs (.e. the dagonal covarance matrx u assocated wth the lne has an nnte value for z)thenthecontours 2 of constant Mahalanobs Dstance wll be ellptc cylnders parallel to the z axs. The cross secton of these cylnders along a plane parallel to the (x y) plane s composed of enclosed ellpses havng length and breadth proportonal to 2 x and 2 y n the prncpal drectons. cylnders along a plane parallel to the (x y) plane s composed of enclosed ellpses havng length and breadth proportonal to 2 x and 2 y n the prncpal drectons. Therefore, n ths case, the M.D. between a pont p and ^u depends on the dstance and drecton of p from a lne perpendcular to the (x y) plane and passng through the pont ^u. Explctly ths dstance s gven by: d 2 =(^u ; p) t 1 2 x 1 2 y 1 C A (^u ; p) =(^u x ; p x ) 2 + (^u y ; p y ) 2 2 x 2 y We can vew ths dstance as a M.D. from an uncertan lne wth some uncertanty nthe drectons perpendcular to the lne drecton. Ths uncertanty s gven by the covarance matrx: lne = 2 x 2 y 1! : (4)

11 Note, that the M.D. between p to any pont on ths lne s equal to the M.D. between p and ^u as can be seen from Equaton 4. Ths proposton s not lmted to lnes parallel to one of the axes and s vald for any uncertan lne. For example, consder an uncertan lne passng through pont ^v wth drecton represented by the unt vector R^z where R s some rotaton matrx and ^z sauntvector algned wth the z axs. Assumng the uncertanty of ths lne s the same as the prevous one, ths lne can be represented by an uncertan pont (^v v ) where ts covarance matrx s as follows: v = R 2 x 2 y 1 1 C A R t The M.D. between pont p to any pont on ths lne s n fact the M.D. from p to the uncertan pont ^v. In the followng sectons we show how to use ths dea to nd the optmal transformaton of a model and to nd the predcted object closest to the model n the sense of 3D Eucldean dstance. The method descrbed below fuses the nformaton from all the measured ponts and estmates the model transformaton T by ncremental renements usng Kalman-f lter [13, 16]. At each step a new uncertan lne s generated from the assocated measurement and an updated soluton s produced. 6 Convertng the Measurements to Uncertan Lnes As stated n Secton 3 the optmal transformaton s calculated subject to mnmzng the dstance between the transformed model and the lnes flne( ^m )g. These lnes are n fact uncertan lnes due to the uncertanty n the measurements. Each of these lnes can be represented by a3d pont ^M located on lne( ^m )andhavng a covarance matrx ;. The pont uncertanty n the drecton of projecton s nnte, and ts uncertanty n the perpendcular drectons s deduced from the assocated measurement uncertanty. Ths uncertan 3D pont (uncertan lne), ( ^M ; ), can be consdered as a 3D measurement of the th transformed model-pont T ( ^X ). In ths secton we explan how to convert the measurements nto uncertan lnes and n the followng secton we gve the algorthm for ndng the optmal transformaton usng ths nformaton. 11

12 Assume that the measurements are performed on the mage plane usng the coordnate system (v w) where the k th measured pont s: measurement(k)=[^m k =(^v k ^w k ) k vw] k sa22covarance matrx descrbng the uncertanty of the actual measurement(^v vw k ^w k ). For smplcty we ommt the subscrpt k at ths tme so that ths measurement s represented by the par [(^v ^w) vw ]. We separate our dscusson nto two cases: orthographc projecton and perspectve projecton. Orthographc projecton: In the case where the projecton s along the z-axs (orthographc) the assocated uncertan lne s represented as: ( ^M ;) = [(^v ^w ^z) where ^z s any estmate of the z coordnate. vw 1! ] Perspectve projecton: In the case of perspectve projecton, the modelng of the uncertanty s more complex. Assume that the orgn of the vewer-centered frame of reference (x y z) s at the focal pont as shown n Fgure 4 and the focal length s equal to one. We am to transform the measurement ^m =(^v ^w) gven n the mage-plane coordnate system (v w) nto a representaton n the Cartesan system (x y z). Consderng the sphercal coordnate system (r ) (Fgure 4). The vector (^v ^w) determnes the angular coordnates ( ) but leaves the value of r undetermned: ^ = arctan( p^v 2 +^w 2 ) ^ = ^v arccos( p^v2 +^w ) : 2 Addtonally, the uncertanty of(^v ^w) s transformed nto a covarance matrx n the ( ) system as follows: =!! ) w) ) s the Jacoban of the transform from (v w) to( ), and the w) 12

13 z u r v f φ w m^ y θ x Fgure 4: A perspectve projecton of the pont U onto the mage plane (v w). The pont can be represented n ether, a Cartesan system (x y z) or a sphercal system (r ). at pont (^v ^w). The Jacoban matrx w) = ^v 2 ^w 2 ; ^w 2 ^v 2 where 1 p^v2 +^w p^v2 +^w : 2 The transformaton nto sphercal coordnates, as an ntermedary stage, allows a smple representaton of the assocated uncertan lne: ( ^M ;) = [(^r ^ ^) r ] where r = 1 1 C A and ^, ^, are the expressons descrbed above. ^r s unknown but an estmaton of ^r wll be chosen as s explaned later n ths secton. 13

14 In practce we arenterested n representng the uncertan lne n Cartesan coordnates, thus, the representaton s transformed agan from the sphercal coordnates to Cartesan coordnates (x y z) as follows: ( ^M ;)=[(^x ^y ^z) xyz ] where ^x = ^r sn ^ cos ^ =^r^v ^y = ^r sn ^ sn ^ =^r ^w ^z = ^r cos ^ =^r and the covarance matrx s: xyz =!! y y z) r ) The Jacoban y ) = ^v ^r^v ;^r ^w ^w ^r ^w ^r^v ;^r= 1 C A where the dervatve staken at the pont (^r ^ ^). Here too, all values are known except for ^r. Snce the process that estmates the optmal transformaton ncrementally mproves the k;1 estmaton of T,.e, at each step k, there exsts an estmate ^T from the prevous step, we use ths estmate to calculate an estmate of ^r at step k as follows: ^r k k;1 = k ^T ( ^X )k where ^X s the locaton of the correspondng pont n the model. We emphaszethatthe uncertanty of ths estmate, as expressed n the covarance matrx, s nnte. 7 Estmaton of the Optmal Transformaton As stated, the rst step n ndng the predcted object s to estmate a transformaton T whch optmally translates the ponts f( ^X k k)g of the model onto the correspondng uncertan lnes f( ^M k ; k )g, generated from the measurements f( ^m k k )g. The optmalty of ths transformaton s n the sense of mnmzng Equaton 3. The transformaton T s a 14

15 vector representng a rgd 3-D transformaton (rotaton + orentaton) of the model from ts local coordnates to the vewer coordnates. T s estmated from the generated uncertan lnes usng the Kalman lter tools (K.F). The estmaton process s composed of an ncremental renement, for whch at each step k ; 1, there exsts an estmate ^T k;1 of the transformaton T and a covarance matrx k;1 whch represents the \qualty" of ths estmate: k;1 = Ef( ^T k;1 ; T)( ^T k;1 ; T) t g : Gven a new measurement (^m k k ), an assocate uncertan lne, f( ^M k ; k )g, s generated, and the current estmate s updated to be ^T k wth an assocated uncertanty k. The accuracy of the estmate ncreases, as addtonal measurements are fused,.e. k k;1 ( k;1 ; k s nonnegatve dente). The process termnates as soon as no addtonal measurements can be suppled. 7.1 The Kalman Flter for Parameter Estmaton The Kalman lter s a tool for parameter estmaton from gven measurements. In our case the parameter vector to be estmated s the transformaton vector T whch s composed of two components: The translaton component, expressed by thevector t t =(t x t y t z ) t : (5) The rotaton component, descrbed by the quaternon ~q (see Appendx A) [1]: ~q =(q q) =(q q 1 + q 2 j + q 3 k) : The rotaton quaternon should satsfy the normalty constrans: ~q~q = q 2 + kqk2 =1, where ~q s the conjugate of ~q. In practce we represent the rotaton component by the vector: s q q from whch the quaternon ~q can be reconstructed: q = 1 p 1+st s ~q =(q q s) : 15

16 estmated-parameter ( ^, Ω ) T k-1 k-1 tme delay Kalman-flter ( m ^ ( T ^ k, Γ k) k, Ω k) measurement box result h k (x, m T) k k, mathematcal relatonshp Fgure 5: The Kalman lter for statc-parameter estmaton The three nputs and the estmaton output. The vector s s a convenent representaton of the rotatonal component n addton to beng mnmal (havng 3 parameters) the rotaton equaton s lnear n s as wll be shown later. In order to avod sngulartes n the representaton when q =we always use two object-centered coordnate systems, smultaneously. Consderng these two components, the parameter vector to be estmated durng the lterng process s: T = s t! : The Kalman lter produces an estmate ^T of the transformaton vector, gven the uncertan lnes. At each step, the lter receves three nputs and supples a sngle output (see Fgure 5). The nputs are: 1. An a pror estmate of the evaluated parameter vector and the uncertanty assocated wth t. In our case, n the k th step, the a pror estmaton wll be the estmate evaluated at the prevous step ^T k;1 and ts assocated covarance k;1. The covarance 16

17 matrx n the ntal step wll be set to nnty snce no a pror knowledge about T s assumed and the choce of ^T does not aect the end result. 2. The current measurement and ts uncertanty, n our case ths measurement s the uncertan lne ( ^M k ; k ) generated from actual 2D measurement (^m k k ) where ^M k and ts assocated covarance matrx ; k secton. s calculated as elaborated n the prevous 3. A mathematcal relatonshp between the evaluated parameters and the measurements. Ths mathematcal relatonshp should be lnear n the evaluated parameters. In our case the relatonshp s: ~M k = ~q ~ X k ~q + ~t (6) where ~ M k ~ X k ~t are quaternons assocated wth the vectors M k X k t respectvely. Gven that ~q~q =1,multplyng Equaton (6) by ~q yelds: ~M k ~q = ~q ~ X k + ~t~q : Isolatng the vector component of ths quaternon equaton and dvdng by q we obtan the matrx equaton: h k (X k M k T) <M k + X k> s +(M k ; X k) ; (I 3 ; <s>)t = (7) where s q q as prevously dened, I 3 s the 3 3 dentty matrx and <> denotes the matrx form of a cross product,.e: <v>= 1 C A <v> m = ; <m> v = v m : ;v z v y v z ;v x ;v y v x The equaton h k (X k M k T) = s not lnear as requred n the K.F., therefore, weuse the extended Kalman f lter (E.K.F.) [13, 16] whch s a generalzaton of the Kalman lter to non-lnear systems where transton from step k ; 1 to step k s performed usng a lnear approxmaton of h k by takng the rst order Taylor expanson around ( ^X k ^M k ^T k;1 ). The lnearzaton of the measurement equatons for our case are gven n Appendx B. 17

18 The output of the K.F fuser s an updated estmaton of the evaluated parameters and ts assocated uncertanty n our case ^T k and k respectvely. The K.F. fuser s of the form: ^T k = f( ^T k;1 k;1 ^X k k ^M k ; k h k ) : Thus, at each stage k, there s no need of retanng any of the prevously consdered measurements. Only the current estmate ^T k;1 and ts assocated uncertanty k;1 need be retaned. The K.F. updatng equatons are gven n Appendx C. The K.F. updatng equatons yeld an unbased estmate of T whch s optmal n the lnear mnmal varance crteron [1],.e. ^T k C = kx =1 mnmzes ^h (T)W ;1 ^ht (T) (8) where ^h (T) =h ( ^X ^M T) andw s the covarance matrx of ^h (Appendx D). The value C n Equaton 8 s n fact the same value as n Equaton 3 whch we amed to mnmze. Thus, the desred soluton as formulated n Sectons 3 and 4 s the same soluton as obtaned from the K.F. updatng equatons. Snce the measurement equatons are lnear approxmatons of non-lnear equatons the ntal soluton obtaned by the K.F. wll not necessarly be the correct soluton. Ths case wll happen when the lnearzaton s around a pont that s not close enough to the correct soluton. In order to reduce the nuence of the lnearzaton local teratve K:F: [13, page 349] s appled. In the teratons the constrants are relnearzed around the updated soluton obtaned by the K.F. and another cycle of K.F. s performed usng the new verson of the lnearzed constrants. Note, that the general K.F. deals wth a parameter vector that s changng wth tme, whereas n our case the estmated transformaton, T, s statc and does not change between measurements. 8 From Predcted Objects to Recognton After the optmal transformaton ^T n s estmated from the n measurements, the second stage n the recognton task s to nd the \smlarty" between the predcted object and the 18

19 canddate model. Ths \smlarty" s calculated wth the 3D dstance, however t has to be consder wth the uncertanty of the model features and the measurements. Fortunately, there s no need to reconstruct the predcted object n order to calculate ts smlarty to the model: Gven a transformaton T, the M.D. between the predcted object and the transformed model s gven by C of Equaton 8 where the summaton s over the n measurements. Thus, the smlarty between the predcted object and a canddate model s gven by thevalue C calculated for the nal transformaton ^T n. If a reconstructon of the predcted object s requred, the calculaton of ts feature locatons s smple: We transform each model-pont ^X by the optmal transformaton ^T n and nd apont U such that the sum of the M.D. to the uncertan lne ( ^M ; ) and the M.D. to the transformed model pont T ( ^X ) s mnmal. The mathematcal formulaton of ths reconstructon s elaborated n Appendx E. Note that f the th measurement s a perfect measurement ( s zero) then U wll be on the lne lne( ^m ). 9 Results We tested our method by smulatng a model as a collecton of ponts. The ponts of the model were chosen by random samplng n the cube [::1] 3. The model ponts were transformed by a transformaton T composed of a rotaton s and a translaton t. The model ponts and the measurements were contamnated by whte Gaussan nose. All model ponts were contamnated by 3D nose havng dentcal uncertanty (and dagonal covarance matrx) and all the measurements were contamnated wth 2D nose whch was added to the mage plane and havng dentcal uncertanty as well. Graphs 6-8 show the convergence of the estmates of the rotaton ^s and the translaton ^t as a functon of the number of measurements. The vertcal ordnate represents the squared devaton of the estmate from the real value,.e: t error = k^t ; tk 2 left graphs and s error = k^s ; sk 2 rght graphs : The results shown n the graphs were averaged over 1 processes of 1 randomly generated objects. In the graphs two cases are shown: the convergence of the estmate when 2D dstance metrc s used as the mnmzaton crteron, and the convergence when the 3D dstance s used. The varance of the nose was not gven to the algorthms and t was assumed that each measured pont s contamnated by the same amount. 19

20 Graphs 6-8 show three typcal cases: Graph 6 shows a case where the nose n the modelponts s domnant (the s.t.d of the model nose was proportonal to 8% of the total sze of the body where the mage nose was proportonal to 1%), Graph 7 shows a case where the nose n the mage s domnant (the s.t.d of the mage nose was proportonal to 5% of the total sze of the body where the model nose was proportonal to 1%), and Grpah 8 shows a case where the nose n the model-ponts s domnant (the same as n Graph 6) but the dstance of the body from the mage plane s qute large (1 tmes the focal length) relatve to the case shown n graphs 6 (where the dstance s about 1 tmes the focal length). It s demonstrated that the 3D dstance metrc s advantageous over the 2D dstance metrc when the model nose s domnant and the object s located qute close to the mage plane. In the cases where the mage nose s domnant or when the object s far away from the mage plane the mprovement of the estmate usng the 3D dstance metrc s neglgble. These results are reasonable snce the algorthms assume the same amount of nose for each measurement. When the mage nose s domnant ths assumpton s correct and the 2D metrc gves the same results as the 3D metrc. If the model nose s domnant, ts projecton onto the mage plane s not dentcal for each measurement and therefore 3D metrc s essental. In the case where the object s far away from the mage plane, the projecton of the model nose onto the mage plane s almost dentcal for all the measurements and therefore the the 2D metrc s agan useful. 2

21 D Eucldean metrc 2D Eucldean metrc.4 3D Eucldean metrc 2D Eucldean metrc R.M.S.E of t 6 4 R.M.S.E of s no. of ponts no. of ponts Fgure 6: A comparson between a 2D dstance metrc based algorthm and a 3D dstance metrc based algorthm. Left graph: convergence of the devaton of the translaton estmate ^t. Rght graph: convergence of the devaton of the rotaton estmate ^s. In ths case the s.t.d of the model nose was proportonal to 8% of the total sze of the body where the mage nose was proportonal to 1% D Eucldean metrc 2D Eucldean metrc.4 3D Eucldean metrc 2D Eucldean metrc R.M.S.E of t 6 4 R.M.S.E of s no. of ponts no. of ponts Fgure 7: A comparson between a 2D dstance metrc based algorthm and a 3D dstance metrc based algorthm. Left graph: convergence of the devaton of the translaton estmate ^t. Rght graph: convergence of the devaton of the rotaton estmate ^s. In ths case the s.t.d of the model nose was proportonal to 1% of the total sze of the body where the mage nose was proportonal to 5%. 21

22 D Eucldean metrc 2D Eucldean metrc.4 3D Eucldean metrc 2D Eucldean metrc R.M.S.E of t 6 4 R.M.S.E of s no. of ponts no. of ponts Fgure 8: A comparson between a 2D dstance metrc based algorthm and a 3D dstance metrc based algorthm. Left graph: convergence of the devaton of the translaton estmate ^t. Rght graph: convergence of the devaton of the rotaton estmate ^s. In ths case the s.t.d of the model nose and the mage nose were the same as n graphs 6, but the dstance of the body from the mage plane was about 1 tmes the focal length. 22

23 1 Concluson Ths paper presents a new scheme whch denes a 3D dstance metrc between model features and 2D measurements obtaned followng a projecton. Ths 3D dstance s dened as the mnmum 3D dstorton the model must undergo n order to t some permssable reconstructon of the measurements. It was demonstrated that the proposed 3D dstance metrc s usefull when the model nose s domnant and when the object s close to the mage plane. Acknowledgements I wsh to thank Ronen Basr for a frutful dscusson whch led me to ths paper. Appendx: A Rotaton Quaternon A quaternon ~q s composed of two parts - the scalar part q and the vector part q: ~q =(q q) =(q q 1 + q 2 j + q 3 k) : If v,v are vectors n < 3 such that v = Rv, when R s a rotaton matrx, then the correspondng expresson n quaternon form s: ~v = ~q~v~q : The quaternons ~v ~v correspond to the vectors v,v respectvely as follows: ~v =( v) ~v =( v ) and ~q s the conjugate of ~q ~q =(q ;q) : ~q represents a rotaton of the vector v by angle around a unt vector ^n where: q = cos( 2 ) q =sn( 2 )^n 23

24 so that k~qk 2 = ~q~q = q 2 + kqk 2 =1 : B Lnearzaton of the Measurement Equatons for the E.K.F At the k th step there s a non-lnear measurement equaton: h k (X k M k T) = as wrtten n Equaton 7. Snce the K.F. deals only wth lnear processes we use the lnear approxmaton of h k by takng the rst order Taylor expanson around ( ^X k ^M k ^T k;1 ): h k (X k M k T) = h k ( ^X k ^M k ^T k;1 k (X X k ; ^X k (M k ; M ^M k k k k T (T ; ^T k;1 ) (9) Equaton 9 can be rewrtten as a lnear equaton: where z k = H k T + k (1) z k = <^s k;1 > ^t k;1 + ^X k ; ^M k H k = [< ^M k + ^X k ; ^t k;1 > (<^s k;1 > ;I 3 )] k = [I 3 ; <^s k;1 >](M k ; ^M k ) ; [I 3 + <^s k;1 >](X k ; ^X k) : z k represents the new \measurement", H k s the matrx denotng a lnear connecton between the \measurement" and the actual transformaton T. Both z k and H k can be derved from ^M k, ^X k, ^T k;1. The term k - depcts the nose n the \measurement" z k and satses: Ef k g = varf k g = [I 3 ; <^s k;1 >] k [I 3 ; <^s k;1 >] t +[I 3 + <^s k;1 >] k [I 3 + <^s k;1 >] t = W k : Notce that accordng to the K.F. denton t s assumed that there s no correlaton between the derent measurement nose (covf j g =8 6= j). Ths assumpton s not always vald. When there s correlaton between several measurements, we may consder these measurements as a sngle measurement by groupng the measurement values nto a sngle vector and by combnng ther correspondng equatons nto a sngle vector equaton. 24

25 C The Kalman Flter Equatons for Statc Systems Assume the measurement equatons are as wrtten n Equaton 1. The recursve K.F. updatng equatons for tme step k are: state estmate update : ^T k = ^T k;1 + K k (z k ; H k ^T k;1 ) state covarance update : k = k;1 ; K k H k k;1 Kalman gan matrx : K k = k;1 Hk(H t k k;1 H t k + W k ) ;1 : D Covarance Matrx of ^h The value of h (Equaton 7) can be lnearly approxmated, n the th step, by takng the rst order Taylor expanson around ( ^X ^M ): h = ^h (X X ; ^X (M ; M ^M ) where the dervatves are taken at ( ^X ^M ) and ^h clear that ^h E s a zero mean random process wth the covarance: W = Ef^h ^ht g =(@h ) X ) t ); X ) t : M M Reconstructon of Predcted Object = h ( ^X ^M T). From the above ts Let ^T = ^s^t! be the nal transformaton estmated after fusng all the avalable measurements. From ^s we can buld an assocated rotaton matrx ^R [17] and an expresson for the transformed model: ^X = ^R ^X + ^t = ^R ^Rt Gven the transformed model-pont (^X ) and the assocated measured pont (^M ; ) (uncertan lne) the predcted object s obtan by fusng these two ponts: U = ^X + K( ^M ; ^X ) 25

26 where K = ( +; ) ;1 Ths equaton can be obtan from the K.F. updatng equatons where ^X state vector and ^M as a measurement. s regarded as the References [1] B.D.O. Anderson and J.B. Moore. Optmal Flterng. Prentce-Hall, Inc., N.J., [2] K.S. Arun, T.S. Huang, and S.D. Blosten. Least squares ttng of two 3D pont sets. IEEE Trans. Pattern Analyss and Machne Intellgence, 9(5):698{7, Sept [3] R. Basr and D. Wenshal. Dstance metrc between 3D models and 2D mages. In The 9th Israel Symposum on Artcal Intellgence and Computer Vson, pages 337{356, Dec [4] T. Fan, G. Medon, and R. Nevata. Recognzng 3D objects usng surface descrptons. IEEE Trans. Pattern Analyss and Machne Intellgence, 11(11):114{1157, Nov [5] O.D. Faugeras and M. Hebert. The representaton, recognton, and postonng of 3D shapes from range data. In A. Rosenfeld, edtor, Technques for 3D Machne Percepton, pages 13{51. Elsever Scence, [6] M.A. Fschler and R.C. Bolles. Random sample consensus: A paradgm for model ttng wth applcatons to mage analyss and automated cartography. Communcatons of the ACM, 24(6):381{395, June [7] W.E.L. Grmson. On the vercaton of hypotheszed matches n model-based recognton. In European Conference on Computer Vson, pages 489{498, 199. [8] W.E.L. Grmson and T. Lozano-Perez. Model-based recognton and localzaton from sparse range or tactle data. Internatonal Journal of Robotcs Research, 3(3):3{35, [9] Y. Hel-Or and M. Werman. Absolute orentaton from uncertan data: A uned approach. In Conference on Computer Vson and Pattern Recognton, pages 77{82, [1] B.K.P. Horn. Robot Vson. MIT Press,

27 [11] B.K.P Horn. Closed-form soluton of absolute orentaton usng unt quaternon. J. Opt. Soc. Am., 4(4):629{642, [12] D.P. Huttenlocher and S. Ullman. Object recognton usng alngment. In Internatonal Conf. on Computer Vson, pages 12{111, [13] A.H. Jazwnsk. Stochastc Process and Flterng Theory. Academc Press, 197. [14] D.G. Lowe. Three-dmensonal object recognton from sngle two-dmensonal mages. Artcal Intellgence, 31:355{395, [15] D.G. Lowe. Fttng parametrzed 3-D models to mages. IEEE Trans. Pattern Analyss and Machne Intellgence, 13:441{45, May [16] P.S. Maybeck. Stochastc Models, Estmaton, and Control, volume 1. Academc Press, [17] B. Sabata and J.K. Aggarwal. Estmaton of moton from a par of range mages: A revew. Computer Vson, Graphcs and Image Processng, 54(3):39{324, Nov