Covariance-Based Registration

Transcription

1 Covarance-Based Regstraton Charles V. Stewart Dept. of Computer Scence Rensselaer Poly. Inst. Troy, New York June 24, Introducton The regstraton problem n computer vson s the problem of fndng the transformaton that best algns (regsters) a model wth a data set or best regsters two or more data sets. The goal s to brng the model and the data set or the multple data sets nto the same coordnate system. Solutons to ths problem are requred n many applcaton domans. In ndustral nspecton, regstraton between model and data s necessary for comparng deal ( nomnal ) parts wth manufactured parts so that defects n the manufacturng process may be dentfed [32, 33]. In model constructon, placng sensor measurements n the same coordnate system s the necessary prerequste for buldng complete models rather than models dependent only on ndvdual vews. In medcne, regstraton facltates treatment montorng, mxng of sensed data from dfferent modaltes, and applcaton of surgcal plans developed off-lne [10, 13, 15]. Each of these applcatons requres precse and accurate estmates of the transformaton. The regstraton problem has many forms. Dfferences are due to the type of data, the type of model (f any) and the type of transformaton. Examples nclude mage to mage regstraton for mosac constructon [23, 40, 37], range data to range data regstraton for model constructon n reverse engneerng [3, 19, 29, 35, 9], and model to mage regstraton for trackng, moton estmaton, object recognton or camera calbraton [14, 22, 27, 28, 44]. The partcular nstance of the regstraton problem consdered here s regsterng a three-dmensonal model to one or more range data sets as precsely and accurately as possble. Ths problem, whch dffers from the range data to range data regstraton problem because the model s known n advance and s not varable, wll be what s meant by the regstraton problem. It has partcular relevance to ndustral nspecton applcatons [32].

2 RPI-CS-TR Precse and accurate regstraton requres use of constrants based drectly on the data ponts. Feature-based technques, such used n tradtonal object recognton and pose determnaton [8, 12], and methods based on global shape descrptons [26], such as extended gaussan mages, sphercal attrbute mages [18], or spn mages [24], are good for coarse postonng when a pror estmate of the transformaton s not known, but must be followed by pont-based regstraton for precson and accuracy [18, 24]. The man dea n most pont-based regstraton technques appears n several nearly smultaneous papers proposng teratve closest pont (ICP) algorthms [4, 7, 32, 46]. ICP algorthms terate (temporary) matchng and pose estmaton steps, specfcally (1) fndng the closest model pont to each data pont based on a current transformaton (pose) estmate and (2) revsng the pose estmate based on the collecton of matches. Each of these algorthms mnmzes a Eucldean dstance metrc n matchng. The pose estmaton constrants are ether the Eucldean dstance between each matched data and model pont [4, 32, 46] or the Eucldean dstance between a data pont and a lnearzaton of the model surface around the matched model pont [7, 39]. The latter s called the normal dstance. ICP algorthms have been extended to regsterng volumetrc mages and to regsterng combned range [13] and color mages [25]. An alternatve method to ICP algorthms s descrbed n [6, 41], where data to model dstance measures are represented and computed usng what s called an octree splne. Ths avods the need for an explct matchng step, but makes pose estmaton non-lnear. In lght of the goal of makng regstraton as precse and accurate as possble, t should be clear that what s mssng from current pont-based regstraton technques are measures of uncertanty n the data. The sgnfcance of ncorporatng data uncertanty nto estmaton problems such as regstraton may be seen by examnng two smpler problems, each provably equvalent to a specal case of regstraton (Appendx A). Multvarate locaton: The problem s to estmate a locaton µ from n pont measurements x, each wth an assocated covarance matrx S. The optmal estmate mnmzes the sum of the squared Mahalanobs dstances: (x µ) T S 1 (x µ). (1) Ths estmate s easly shown to be ( ) 1 ( ˆµ = S 1 S 1 x ) Ths reduces to the ordnary average only f the covarance matrces are equal. Lnear regresson: Restrctng attenton to ponts n two-dmensons, so that x = (x, y ) T, consder the dfference between ordnary regresson and orthogonal regresson. Mathematcally the error metrcs to be mnmzed

3 RPI-CS-TR are (y mx b) 2 and (a 0 + a 1 x + a 2 y ) 2 (2) respectvely, the latter beng subject to the constrant a a 2 2 = 1. (Of course, the two sets of lne parameters may be translated back and forth except when a 2 = 0.) When the error covarance matrces are S = dag(0, σ 2 ), mnmzaton of the ordnary regresson metrc yelds an unbased (and therefore most accurate) estmate of the lne parameters, whereas mnmzaton of the orthogonal regresson metrc yelds a based estmate. The stuaton s reversed when the error covarance matrces are S = dag(σ 2, σ 2 ). Dfferent error covarance matrces requre dfferent estmators for unbased estmates. In both examples, obtanng the most accurate estmates requres use of measurement error represented as error covarance matrces. The man questons addressed n ths paper are (1) how to ncorporate error covarance matrces nto pont-based regstraton algorthms, and (2) what are the practcal consequences of dong so. The frst queston s addressed by formulatng regstraton as a statstcal optmzaton problem usng dstance metrcs based on the Mahalanobs dstance. 1 Two new ICP algorthms arse naturally when solvng ths mnmzaton. One s a generalzaton of current ICP algorthms that use Eucldean dstance n the pose refnement step. A second s a generalzaton of ICP algorthms that use normal dstances n pose refnement. Octree splne regstraton algorthms are not easly extended to ncorporate measurement error n the data because the octree splne representaton depends on the model only. As a result such algorthms are not consdered further here. The practcal consequences of ncorporatng error covarance matrces nto pont-based regstraton algorthms are two-fold. The frst and obvously ntended result s more accurate regstraton. The amount of mprovement over standard ICP algorthms depends on a number of consderatons, ncludng the number and extent of the data ponts, the magntude of the nose n the data, and the object pose tself. The second and somewhat surprsng result s two addtonal new algorthms that are smpler and faster algorthm than current ICP algorthms n the specal case that the error covarance matrces are (nearly) undrectonal. Snce the measurement errors of most range sensors are concentrated along the optcal axs of the cameras [5, 43], these new algorthms wth ther mproved accuracy and effcency should generally be preferred over current ICP algorthms. 2 Problem Formulaton The dscusson begns by formulatng regstraton as an optmzaton problem. Ths n turn begns wth a dscusson of the model, the data, and the transform 1 See Chtra, Weng and Jan for an mportant but specal case soluton to the problem of statstcally optmal regstraton of two data sets.

4 RPI-CS-TR z [R, t] z y x y x C S sensor C M Fgure 1: The regstraton problem s fndng the rgd transformaton best algns the model wth the data. The transformaton s a mappng from the coordnate system C M n whch the model s descrbed nto the coordnate system C S n whch the data s acqured. to be computed. 2.1 The Model and the Data The model s assumed to be descrbed mathematcally n some convenent coordnate system, C M (Fg. 1). Ths descrpton may be an mplct functon, a parametrc model, splne patches, or a trangulated surface mesh. For smplcty n the dervatons, the model s assumed to be descrbed mplctly as the set of pontss p such that f(p) = 0. The resultng algorthms are easly adjusted for dfferent model representatons. In fact, as dscussed later, some offlne preprocessng of the model s generally necessary pror to regstraton. The data are a set of sensed ponts from an nstance of the object. Assume there are N data ponts n the set Q = {q }, where q = (x, y, z ) T, and each data pont has an assocated covarance matrx S. These ponts and matrces are n the sensor coordnate system, C S (Fg. 1). In the case of range data, the ponts may be convenently stored n or converted to an mage format (a range mage ), where each pxel locaton (u, v) stores a measured pont n R 3 denoted by q(u, v). A camera projecton functon, usually perspectve or weak perspectve, denoted by P determnes the mappng from range ponts to pxel locatons. The covarance matrces, S, whch depend on the sensor and the data pont locatons, q, are symmetrc and postve sem-defnte. It wll be mportant to

5 RPI-CS-TR consder the case of S beng sngular and even rank 1, whch represents the most extremely ansotropc dstrbuton. Rough approxmatons to the covarance matrces may be obtaned by analyzng the sensng technque and camera geometry. More complete characterzaton requres expermental analyss, often as part of a rgorous calbraton process [17]. Ths analyss wll result n a lookup table mappng sensed pont postons to covarance matrces. Sometmes ths mappng may even depend on surface orentaton, due to angle of ncdence and reflectvty effects. Such a dependence causes a problem for regstraton because surface orentaton s not known n advance. A straghtforward soluton to ths problem s dscussed later n the paper. The regstraton problem s to fnd the model-to-data transformaton, whch s assumed here to be a rgd transformaton descrbed by a rotaton R and translaton t. The nverse transformaton mappng data-to-model s R T and R T t. 2 Snce the goal s precse regstraton, t s assumed that ntal estmates of R and t are known or easly provded. These may be obtaned by usng applcaton constrants, or by usng feature-based or shape descrpton methods, or, for example, by algnment of 0th and 1st order moments n the model and n the data. 2.2 Regstraton as an Optmzaton Problem Gven a data pont and covarance matrx, q and S, gven a partcular model pont p, and gven a fxed R and t, the squared Mahalanobs dstance between q and the transformed model pont R p + t s (R p + t q ) T S 1 (R p + t q ). (3) When S s sngular, the error vector R p + t q must be entrely wthn the column space of S ; otherwse the Mahalanobs dstance s nfnte. Snce the correspondence between data ponts and the model s not known, t s mportant to formulate the Mahalanobs dstance n terms of the data pont and the model surface. Ths results n a constraned mnmzaton: D 2 M (q, S ; f; R, t) = mn p (R p + t q ) T S 1 (R p + t q ) subject to f(p) = 0 (4) Ths defnes the square Mahalanobs dstance between a data pont and the model surface as the mnmum Mahalanobs dstance over all model surface ponts. The closest pont, whch s not made explct n (4) but wll be requred later, s denoted by p. It depends on the data pont and covarance matrx, the model surface, and the transformaton R, t. Gven these defntons, regstraton becomes the problem of mnmzng the combned square Mahalanobs dstances of all data ponts. Q({q, S }; f; R, t) = D 2 M(q, S ; f; R, t). (5) 2 For notatonal convenence, rotatons are descrbed usng orthonormal matrces. In practce, any convenent representaton may be used [11, Ch. 5].

6 RPI-CS-TR Ths defnes the mnmzaton problem we want to solve to estmate R and t. A formalzaton and generalzaton of the problem addressed n current ICP papers, t nvolves a two-tered mnmzaton, frst n the Mahalanobs dstances of the ndvdual data ponts and second n the global transformaton parameters. These correspond to the two terated ICP steps of matchng and pose estmaton descrbed n the ntroducton. 3 Solvng D 2 M The frst step to mnmzng Q({q, S }; f; R, t) s to understand better the data pont to model surface mnmzaton, D M (q, S ; f; R, t) defned n (4). Ths mnmzaton s solved frst for lnear models and then for general models. 3.1 Lnear Models Lnear models (planes n R 3 and lnes n R 2 ) are of the form f L (p) = ˆη T (p p 0 ) = 0 (6) where ˆη T s a unt normal and p 0 s any fxed pont on the model surface. Incorporatng ths nto (4) and rewrtng DM 2 as D2 L to sgnfy the specalzaton of the model gves D 2 L(q, S ; ^η, p 0 ; R, t) = mn p (R p + t q ) T S 1 (R p + t q ) subject to ˆη T (p p 0 ) = 0 (7) Ths s solved by wrtng the mnmzaton usng Lagrange multplers: F (p, λ) = (R p + t q ) T S 1 (R p + t q ) + 2λ^η T (p p 0 ). (8) Takng dervatves wth respect to p and λ, equatng the results to 0 and 0, and wrtng n matrx form yelds ( R T S 1 ( ) ( R ˆη p R ˆη 0) T = T S 1 ) (q t) λ η T (9) p 0 Solvng for p results n p = R T (q t) + ^ηt (p 0 R T (q t)) ^η T R T S R T R ^η, (10) S R^η where scalar terms have been gathered n the fracton. Now, defne q = RT (q t) and S = R T S R. These are the data pont and ts covarance matrx transformed back nto the model coordnate system, though they do not depend on the model. Usng them, (10) smplfes to p = q + ˆηT (p 0 q ) ˆη T S ^η S ^η (11)

7 RPI-CS-TR Ths gves the pont p on the plane f L (p) = 0 mnmzng the square Mahalanobs dstance. From here on, ths pont wll be denoted by p. The Mahalanobs dstance may now be calculated from (11), thereby solvng (7). Usng q and S ths results n D 2 L(q, S ; p 0, ^η) = (p q ) T S 1 (p q ) = [ˆηT (p 0 q )]2 ˆη T S ^η, (12) Ths dstance metrc s calculated n model coordnates, whereas (4) s defned n data coordnates. Ths s not an ssue because the Mahalanobs dstance s a unt-less measure. Wth quanttes descrbed n ther orgnal coordnate systems, ths becomes DL(q 2, S ; p 0, ^η; R, t) = [^ηt (p 0 + R T t R T q )] 2 ^η T. (13) R T S R ^η In ether case, the result s qute smple. Importantly, t does not rely on the nverse of the covarance matrx, so t s approprate for sngular covarance matrces (errors along only one or two drectons) as well. 3 In fact, as the plane normal R^η approaches the null space of S, the Mahalanobs dstance approaches nfnty. 3.2 General Implct Models The soluton for lnear models wll be used as part of an teratve soluton for general models. The dervaton for the general case starts as above by combnng the dstance and the constrant n (4) nto a Lagrangan form: F (p, λ) = mn p [ (R p + t q ) T S 1 (R p + t q ) + 2λf(p) ] (14) Takng dervatves wth respect to both p and λ and settng the results equal to 0 yelds F p = RT S 1 Rp + R T S 1 (t q ) + λ f(p) = 0 F = f(p) = 0 (15) λ Ths must be solved teratvely by lnearzng f around an ntal closest pont estmate, usng the lnear model soluton to obtan an updated estmate, and repeatng untl convergence. The Mahalanobs dstance at the resultng pont s the soluton to (4). 3 The dervaton above s not a suffcent proof for the case of sngular S, but a careful proof would be too dstractng for our purposes.

8 RPI-CS-TR (1) repeat (2) k = k + 1 (3) p 0 = q + ˆηT,k(p,k q ) ˆη T,kS ^η S ^η,k,k (4) repeat (5) p,k+1 = p 0 + s(q p 0 ) for s such that f(p 0 + s(q p 0 )) = 0. (6) f ((q p,k+1 ) T S 1 (q p,k+1 ) < (q p,k ) T S 1 (q p,k )) (7) break; (8) else (9) p 0 = (p 0 + p,k )/2; (10) untl p,k p,k+1 < ɛ; (11) ˆη,k+1 = f(p,k+1 )/ f(p,k+1 ) (12) untl p,k p,k+1 < ɛ; Fgure 2: Procedure DM 2, an teratve procedure for fndng the model surface pont p mnmzng the Mahalanobs dstance to the nverse rotated and translated data pont. See the text for dscusson of the detals. The pont and normal estmates from the fnal teraton wll be denoted p and ˆη n the algorthm descrpton. Ths teratve soluton s not as straghtforward as t mght frst appear. To make the explanaton clear the followng notaton s used: ndex the teratons usng k, denote the estmated closest pont n teraton k by p,k, and denote the assocated unt surface normal by ˆη,k. Then, usng (11) the updated closest pont s p,k+1 = q + ˆηT,k(p,k q ) ˆη T,kS ^η S ^η,k (16),k Unfortunately, p,k+1 wll not satsfy f(p,k+1 ) = 0 unless p,k+1 = p,k or f s locally lnear. Addtonal steps are generally requred n each teraton to project p,k+1 onto the model surface and to ensure that the resultng pont n fact reduces the Mahalanobs dstance. These steps are bult nto Procedure DM 2 to fnd the closest model pont to nverse transformed data pont q. Ths procedure s gven n Fg. 2 and llustrated n Fg. 3. Several ponts about Procedure DM 2 requre explanaton. It assumes p,k has been properly ntalzed for k = 0. Intalzaton wll be consdered later as part of the entre optmzaton. Next, as set n step (3), p 0 mnmzes the Mahalanobs dstance on the plane determned by p,k and ˆη,k, but s not necessarly on the model surface. Step (5) projects ths pont onto the model surface. The drecton of projecton s along the lne through q and p0, whch preserves any constrants mposed by sngulartes n S, but, dependng on the model representaton, may not be the most effcent drecton of search. If the pont found, p,k+1, does n fact reduce the Mahalanobs dstance, t s taken as

9 RPI-CS-TR η,k q p 0 p,k (p 0+ p,k) / 2 p,k+1 Model surface Fgure 3: Illustraton of Procedure D 2 M. the next pont on the model surface, and the nner loop ends. (Ths wll usually be the case.) Otherwse, the dstance between p,k and p 0 s halved and the nner loop contnues. As a fnal note on Procedure DM 2, t s straghtforward to see that p 0 = (p 0 + p,k )/2 stll yelds a smaller Mahalanobs dstance than p,k. The reason s that the Mahalanobs dstance from q to the planar surface determned by p,k and ˆη,k s a quadratc functon whose unque mnmum s the ntal p 0. Movng p 0 toward p,k monotoncally ncrease ths dstance. Overall, whle Procedure DM 2 appears complcated, t s n fact no more complex n form than the matchng step of current ICP algorthms. These fnd the model pont mnmzng the Eucldean dstance between a data pont and the model surface usng a procedure that s a specal case of Procedure DM 2. It s obtaned by replacng S wth the 3 3 dentty matrx n Procedure DM 2. The addtonal cost of Procedure DM 2 as wrtten s therefore n computng ˆηT,kS ^η,k and S ^η,k n step (3) and computng the Mahalanobs dstance nstead of the Eucldean dstance n step (6). As we wll see, these costs are much reduced when S s less than full rank. 4 Two General Covarance-Based ICP Algorthms The foregong teratve procedure mnmzng DM 2 (q, S ; f; R, t) for fxed R and t corresponds to the matchng step of current ICP algorthms. It also helps to frame the problem of mnmzng the overall objectve functon defned n Equaton 5 and repeated here: Q({q, S }; f; R, t) = D 2 M(q, S ; f; R, t). (17)

10 RPI-CS-TR The natural approach would be to dfferentate DM 2 (q, S ; f; R, t) wth respect to the parameters descrbng R and t. Snce evaluatng DM 2 (q, S ; f; R, t) tself requres teratve mnmzaton, ths dfferentaton would necessarly be numercal. Whle ths would result n a gradent descent algorthm that s known to converge, t would be extremely expensve. The alternatve s to smplfy (17) usng an approxmaton to DM 2. Ths approxmaton should be based on the set of (current) nearest model ponts {p }. In an teratve framework, a new estmate of R and t should be computed usng the approxmaton, and then DM 2 should be mnmzed agan to fnd new model ponts p. Ths follows exactly the two teratve steps of current ICP algorthms. Two dfferent approxmatons are offered below. These approxmatons result n two dfferent algorthms, the frst a generalzaton of ICP algorthms that use pont-to-pont dstance n pose refnement and the second a generalzaton of ICP algorthm that use normal dstance n pose refnement. 4.1 C-ICP1 The frst approxmaton replaces DM 2 (q, S ; f; R, t) n (17) wth the Mahalanobs dstance between q and the current closest pont. The summaton n (17) becomes (R p + t q ) T S 1 (R p + t q ). (18) We refer to the teratve algorthm that uses Procedure DM 2 to fnd the model surface ponts p and uses (18) n pose refnement as C-ICP1 (Covarance-based ICP, Algorthm 1). Algorthm C-ICP1 s a generalzaton of the orgnal Besl-McKay ICP algorthm [4] to use Mahalanobs dstances n matchng and pose refnement. Lke the Besl-McKay algorthm, convergence of C-ICP1 s easly proved: (1) Mahalanobs dstances are beng reduced at each step (.e. each tme Procedure DM 2 s run and each mnmzaton of (18)), and (2) the model ponts p are always kept on the model constrant surface. There are mportant dsadvantages to C-ICP1, however. Frst, lke Besl-McKay algorthm, whch requres 50 or more teratons, t wll be slow to converge. Second and more mportant, sngulartes n the covarance matrces can not be tolerated. Procedure DM 2 adapts to sngulartes by ensurng that the error vector R p + t q s entrely n the column space of S. Unfortunately, ths does not carry over to (18) because as t s wrtten, smultaneously keepng all error vectors n the approprate column spaces as R and t change s not possble. 4.2 C-ICP2 The second approxmaton, and the one we adopt, s based on lnearzng DM 2 (q, S ; f; R, t) around the model pont p found by Procedure DM 2. The lnearzaton effectvely replaces DM 2 (q, S ; f; R, t) n the summaton of (17) wth the lnear Mahalanobs

11 RPI-CS-TR dstance, D 2 p(q, S ; p, ^η ; R, t): [ˆη T (p + R T t R T q )] 2 ˆη T. (19) R T S R ^η Ths lnearzaton of the constrant surface s only used to update estmates of R and t. These revsed estmates are then used to reestmate the closest model surface ponts p va Procedure DM 2, whch are n turn used n a new lnearzaton of the constrant surface. Before showng how to reestmate R and t from (19), several aspects of the approxmaton are mportant to consder. It s mmedate from the defnton that the approxmaton error s secondorder. Wth ths n mnd we may dentfy and consder three sources of sgnfcant error caused by the approxmaton: large data pont to model surface dstances, large changes n R and t, and hgh curvature n the model surface. The frst, caused mostly by sensor measurement outlers, s controlled through robust estmaton [16, 31, 34, 38]. The second s controlled by takng small steps n the mnmzaton of Equaton 19 before swtchng back to updatng the closest ponts p usng Procedure DM 2. The thrd, whch s only a sgnfcant concern for models wth a substantal number of regons of hgh curvature, may be controlled by some combnaton of (a) small step szes, (b) down-gradng the nfluence of hgh-curvature regons through weghtng, and (c) solvng regstraton n a coarse-to-fne manner used smoothed versons of the model at coarse levels. The numerator of each term n (19) s smply the perpendcular dstance from the nversely transformed data pont (.e. transformed back nto the model coordnate system) to the planar surface determned by p and ˆη. If the covarance matrces are sotropc, so that S = σi 2 then the denomnator of each term reduces to just σ 2. In the further smplfcaton that σ = σ for all data ponts, the algorthm smplfes to the method proposed by Chen and Medon [7]. 4 Ths provdes a new dervaton of these normal dstance ICP algorthms n terms of an underlyng objectve functon to be mnmzed. To understand the expresson n the denomnator when the nose s not sotropc, frst wrte S n terms of ts spectral decomposton: ] [Γ 1 Γ 2 Γ 3 S = dag(σ1, 2 σ2, 2 σ3) 2 where the Γ j s are the unt egenvectors (component drectons) of S and 4 The only dfference s that n the orgnal descrpton, two range data sets were beng regstered and the model surface was estmated from one of the data sets. Γ T 1 Γ T 2 Γ 3,

12 RPI-CS-TR Γ1 q η p q Γ 1 η p Fgure 4: When the angle between the model surface normal rotated nto data coordnates and the prncple error drecton s larger (left) the denomnator of (19) and (20) wll be smaller. Conversely, when the angle s smaller (rght) the denomnator term wll be larger. σ 1 σ 2 σ 3 0. Then, t s easly seen that ˆη T R T S R ^η = 3 (σ j Γ T j R ^η ) 2 In other words, the denomnator s computed by projectng the rotated normal onto each component drecton and scalng by the component varance. As σ 2 /σ 1 0, the denomnator reduces to just the projecton onto the prmary error drecton. The denomnator s therefore larger for rotated normals nearly algned wth the prmary error drecton and smaller for rotated normals nearly perpendcular to ths drecton. See Fgure 4. Sngulartes n S are handled naturally because S need not be nverted Updatng R and t from (19) Updatng R and t s most convenent n the model coordnate system. Usng the defntons q = RT (q t) and S = R T S R as n Secton 3.1, the ncremental translaton and rotaton to be estmated are t and R n j=1 [ˆη T (p + t R T q )] 2 ˆη T. (20) R T S R ^η Usng these, the new estmates of rotaton and translaton are R R and R R) t + t. The dffculty n estmatng t and R from (20) s the non-lnearty caused by the appearance of R n the denomnator. One soluton therefore s to approxmate the denomnator wth ˆη T S ^η, lnearzng the estmaton problem.

13 RPI-CS-TR The approxmaton ntroduces a small error n the rotaton of ˆη pror to projectng onto the components of S (see dscusson above and Fg. 4). Makng ths approxmaton, the denomnator effectvely becomes a weght, w = 1/ˆη T S ^η (21) and the summaton becomes w [ˆη T (p + t + Rq )] 2. (22) Intutvely, ths weght corrects the normal dstance to be closer to dstance along the prncple error drecton (Fg. 4). R and t are calculated updated from (20) usng well-known methods. Further refnement of these ncremental estmates may be obtaned by recomputng the weghts and reestmatng t and R, smlar to what s done n several moton estmaton algorthms [42, 45]. A second approach to handlng the non-lnearty n (20) s to use gradent descent or a Levenberg-Marquardt procedure. Parameterzng R usng a small angle approxmaton, the gradent of (20) s straghtforward and effcently computable, especally when evaluated at t = 0 and R = I. Once the gradent drecton s determned, (20) may be evaluated at several steps along the gradent vector to locate the mnmum. Ths suggests an overall algorthm wth two man phases. The frst phase alternates (a) Procedure DM 2 to update the closest model ponts p wth (b) the weghted lnearzaton (perhaps run for several reweghtng steps) to update estmates of R and t. After the frst phase converges, the second phase replaces the weghted lnearzaton wth the gradent based procedure. Ths s then run n conjuncton wth Procedure DM 2 untl fnal convergence. Ths fnal convergence should only requre one or two steps of alternatng Procedure DM 2 (for each data pont) wth the gradent procedure Intalzaton Intalzaton s an mportant consderaton both when Procedure DM 2 s frst nvoked at the start of C-ICP2 and n restartng Procedure DM 2 after each refnement to R, t. Gven an ntal transformaton estmate, a smple ntalzaton method s to form a lne from each nverse transformed data pont q and prncple error drecton Γ,1, and ntersect ths lne wth the model surface. Rentalzaton of Procedure DM 2 followng refnement of the transformaton estmate s also straghtforward. For non-sngular covarance matrces, the prevous matched model pont p serves as an approprate p,0. For sngular matrces, f q p s not n the column space of S (wth q and S recomputed from the new pose), then agan the lne through q n drecton Γ,1 may be ntersected wth the model surface Robust Estmaton Robustness to outlers s crucal n regstraton because measurement errors and measurements from background surfaces are unavodable. When reasonable n-

14 RPI-CS-TR tal transform estmates are avalable, the best choce of robust technque s an M-estmator (see dscusson n [38]), whch has the dual advantages of downgradng or completely elmnatng the nfluence of ponts wth large Mahalanobs dstances, whle sacrfcng lttle of the statstcal effcency of square error norms for ponts wth small resduals. The well-known susceptblty of M-estmators to leverage ponts, whch causes ther low breakdown pont, s not an ssue here because of the ntal pose estmate. The M-estmator s used n refnng the pose estmate, so that the lnearzed pose estmaton equaton becomes ρ( w [ˆη T (p + t + Rq )]/σ). (23) We have effectvely used both Cauchy [20] and Beaton-Tukey bweght [2] ρ functons. Several aspects of ths objectve functon are mportant to consder The weght term w (21) s ncluded n the argument of ρ( ) because t s part of the dstance computaton. The ntroducton of the scale term σ may appear strange at frst because Mahalanobs dstances are normalzed. Data-to-model error dstances are not only caused by measurement errors, however. They are also caused by regstraton errors and dscrepances between the model and the actual object. Intally these errors wll be large, but as the algorthm converges they wll be greatly reduced. Hence, the scale parameter σ must be (robustly [20]) reestmated one or more tmes durng C-ICP2. It must be fxed before the algorthm s allowed to converge. The soluton to (23) s based on teratvely reweghted least-squares (IRLS) [20]. The robust weght functon w ρ (u) = ρ (u)/u, where u = w [ˆη T (p + q )]/σ, s computed for each pont based on the current pose and match and s then used to scale each term n (20). These robust weghts are also used n the gradent phase of Algorthm 2. Wth the addton of robust weghtng, the descrpton of C-ICP2 s now complete. 5 Algorthms for Undrectonal Errors Two new regstraton algorthms may be derved n the specal case that the error covarance matrces are concentrated along a sngle drecton. Undrectonal errors of ths sort are typcal of trangulaton-based range sensors. Structured lght sensors [1, 21, 36] work by recordng black and whte patterns of lght projected from a lght source and off scene surfaces. By projectng a varety of patterns, a bt vector may be formed at each pxel whch encodes the poston of the scene surface along the backprojecton from the pxel. Measurement error (as opposed to system calbraton error), therefore, s predomnantly along ths

15 RPI-CS-TR backprojecton. Usng more tradtonal stereo sensors, where depth calculaton depends on pont, edge or lne locatons detected n one or more mages, the rato of the depth to the ntercamera dstance predomnates n determnng the error dstrbuton. When ths rato s large, whch s typcal of most sensors, the error dstrbuton s domnated by the depth drecton [5, 30]. Two new algorthms are descrbed here. The frst, a specalzaton of C- ICP2, s a general method for handlng undrectonal errors. Ths second, whch s substantally dfferent and much faster, depends on havng the range data represented as a range mage wth an assocated projecton functon P. These two algorthms are smpler to mplement, faster and, for predomnantly undrectonal errors, more accurate than current ICP algorthms. 5.1 C-ICP3 C-ICP3 ntroduces two smplfcatons over C-ICP2. The frst and most mportant s to replace Procedure DM 2 wth a much smpler technque. For data pont q let Γ be the error drecton, whch means that S = σγ 2 Γ T. Transformng Γ q and from data to model coordnates yelds Γ = R T Γ and q = RT q, respectvely. Then, the model pont mnmzng the Mahalanobs dstance to q must be along the lne through q n drecton Γ : p(u) = q + uγ. The mnmzaton n Procedure DM 2 therefore reduces to the problem of fndng the smallest u such that p(u) = 0. Denotng ths pont u, the matchng pont s p = p(u ). The second smplfcaton s n the denomnator of the objectve functon of the ncremental rotaton and translaton update equaton, (20). Usng S = σγ 2 Γ T yelds ˆη T R T S R ^η = (σ Γ T R ^η ) 2. Beyond ths, the calculaton of R and t s the same as n C-ICP2. The combnaton of these two smplfcatons results n what wll be referred to as algorthm C-ICP C-ICP4 When the data set s formed nto a range mage and measurement errors are concentrated along the lnes of sght of the pxel, a more dramatc smplfcaton of C-ICP2 s possble. The dea behnd ths arses from reversng the thnkng about the matchng process of Procedure DM 2. Consder a pont p on the model surface, and thnk about fndng the closest data pont to p based on the current transformaton estmate. Defne p = Rp + t. Then, because errors are along the lnes of sght, the closest pont to p (n a Mahalanobs dstance sense) s the pont q(u, v) such that (u, v) s the closest range mage pxel to P(p ), the projecton of p onto the mage plane (Fg. 5). Now, f Γ(u, v) s the lne of sght of pxel (u, v) (and the error

16 RPI-CS-TR p z [R, t] z P (p ) p y x y pxel (u,v) x C S C M Fgure 5: Matchng n Algorthm C-ICP4 maps model pont p nto data coordnate usng the current transformaton estmate and then projects ths onto the mage plane. The matchng data pont s the pont q(u, v) stored at the nearest pxel, (u, v). drecton for q(u, v)), then the lne through q(u, v) n drecton Γ(u, v) wll not necessarly pass through p. Therefore t wll not satsfy the sngularty constrants of the error dstrbuton. If the model pont s consdered as a model surface patch, however, wth transformed normal η, then ths lne wll pass through the patch and close to p. Furthermore, as shown above, f the patch s planar then p suffces as the match to q(u, v), and there s no need to fnd the actual closest model surface pont. Therefore, p can serve as the closest model pont to q(u, v). The approxmaton error s second-order. The sgnfcance of ths s that unlke any of our prevous algorthms and unlke any current ICP algorthm, there s no search nvolved n ths closest pont matchng process! 5 Ths dea of reversng the roles of model and data to dramatcally smplfy the matchng process leads to Algorthm C-ICP4, summarzed n Fg. 6. In C- ICP4, the model s dscretzed nto a set of planar patches represented by ponts and normals: {(p j, ˆη j )}. Ths computaton s done off-lne as a preprocessng step. On-lne, gven a range mage, Z(u, v), regstraton proceeds wth the usual terated steps of matchng and pose refnement. Matchng uses the non-teratve process descrbed above. Unqueness n the matchng may be enforced by only allowng the closest model patch for each data pont ths s generally not a problem. Pose refnement requres the soluton to a slghtly modfed verson of 5 Algorthm C-ICP3 has search n fndng the ntersecton of the error constrant lne wth the model surface.

17 RPI-CS-TR Off-lne complaton of model: Form a set of planar patches {(p j, ˆη j )}, from a regular samplng of the model surface. Avod samplng n regons of hgh curvature. On-lne: regstraton (1) Gven a range mage Z : (u, v) q(u, v). and an ntal transformaton estmate R 0, t 0. (2) k = 0; (3) repeat (4) for each model patch j { (5) p j = R k p j + t k ; (6) (u, v) T = P(p j ); (7) q j = Z(u, v); (8) } (9) Compute pose ncrements R k, t k from ρ( w [(ˆη ) T (p + t + Rq ]/σ) as n Algorthm C-ICP2. (10) Update the pose estmates: t k+1 R k (t k + t k ) and R k+1 = R k R k (11) k + +; (12) untl ( R k+1 I 3 3 and t k+1 0) Fgure 6: Outlne of Algorthm C-ICP4, a dramatcally smplfed regstraton algorthm applcable when the range data are stored n mage format and measurement errors are predomnantly along the lnes of sght (backprojecton lnes) of the pxels. (23). Wth trval adjustments, ths uses the lnearzaton and gradent descent update technques from Secton and the robust methods from Secton Ths completes the basc descrpton of Algorthm C-ICP4. More detals and varatons are consdered n what follows Dscretzaton of the Model Model dscretzaton requres predctng the model to data transformaton, transformng the model nto ths predcted vewpont, and then samplng the model surface unformly based on the vewpont. In effect, ths creates a smulated range mage from the model surface, but wth surface normals n addton to ponts. Together, the model ponts and normals create a patch set {(p j, ˆη j )}. If several substantally dfferent vewponts are possble, then multple vewpontdependent patch sets may be created. It s mportant to note, however, that wthn the same aspect regstraton does not depend sgnfcantly on the choce of model vewpont. The only dfferences should be the poston of the samples all other dfferences are elmnated when the patch set s transformed nto

18 RPI-CS-TR η p r r model surface δ Fgure 7: For sample spacng δ on the surface, the approxmaton error usng a lnear patch at model pont p wth radus of curvature r s at most r r2 (δ/2) 2 δ 2 /(2r) 2. the sensor coordnate system and these dfferences, lke dfferences n pxel postons wthn the sensor are rrelevant. Several further ssues n the dscretzaton of the model are sgnfcant. Model surface samplng densty should be controlled by tradeoffs between the desred level of precson n regstraton and the computaton tme. Exact formulas for the precson are dffcult to obtan, asde from the obvous and unattanable lower bound varance σ 2 s/n on the transformed poston of any model surface pont. (Here, σ 2 s s the varance n the data and n s the number of matches.) Hence, samplng densty should be determned expermentally based on the model, the sensor, and applcaton constrants. There should be no smoothng of the model surface ponts and normals snce ths smoothng can create bas n regstraton. If the possblty of local mnma s a concern, then a coarse-to-fne herarchy of patch sets may be created wth smoothng of the model at coarse levels pror to samplng. Even though lnear patches (ponts and normals) are used, the approxmaton error s extremely low. Suppose δ s the spacng between model patches on the model surface and suppose δ s the spacng between data ponts when backprojected onto the actual surface. Then, a smple analyss shows that the maxmum error n dscretzaton and n matchng s approxmately δ 2 /(2r) 2, where r s the local radus of curvature on the model surface (Fg. 7). Ths means hgher-order approxmatons are generally unnecessary, even for hgh-precson regstraton. It does suggest, however, that ponts of extremely hgh curvature on the model such as sharp edges and blendng regons, be avoded n the patch set of C-ICP4. Ths s desrable anyway, because these are often regons of large sensor error and places where model and actual objects are lkely to dsagree.

19 RPI-CS-TR Convergence A formal convergence proof s dffcult to obtan for Algorthm C-ICP4. The problems are two-fold: changes n the transformaton estmate wll produce changes n matchng of patches to data ponts, and because the patches are lnear approxmatons these changes are not n a strct sense smooth. The above arguments about the approxmaton error, however, may also be appled to show that the effects of any changes n matchng are extremely small. Furthermore, as the ncremental changes R k and t k become small nducng changes n p j sgnfcantly less than δ there are fewer and fewer changes to the matches. As a result, wth proper ntalzaton, convergence s not a problem n a practcal sense. 6 Expermental Results [Author s note: these are just plans. The experments should be done n a week or so.] State that these algorthms (C-ICP4, n partcular) are beng used n practce, but the real test must come through smulaton because ground truth s unknown. (Could put n GR&R tests for precson, though?) Compare C-ICP2 and C-ICP4 to a more tradtonal pont-based algorthm. Assgn σ 3 = σ 2 and vary the rato σ 2 /sgma 1. Consder the effects of varaton n surface orentaton, wdth of the surface, number of ponts, and magntude of σ 1. Use a nose model that vares σ 1 n depth. 7 Summary and Conclusons Ths paper has addressed the problem of regsterng a three-dmensonal model wth a range data set for applcatons where precson and accuracy are crucal. The man nnovaton s the formulaton of regstraton as a statstcal optmzaton problem usng as an objectve functon the summed, squared Mahalanobs dstances between data ponts and the transformed model surface. Ths bulds measurement uncertanty n the form of error covarance matrces nto the regstraton problem formulaton. Two algorthms, C-ICP1 and C-ICP2, whch generalze current pont-based regstraton algorthms, arose n solvng the regstraton optmzaton problem. Two further algorthms, C-ICP3 and C-ICP4, were derved for the specal case of covarance matrces domnated by a sngle error drecton. Ths s especally relevant to regstraton aganst range data obtaned usng trangulaton-based sensors. The measurement errors for data from these sensors tends to be concentrated along the backprojecton lnes for each pxel. Algorthm C-ICP4 s the most mportant n practce. It was derved for trangulaton-based range sensors where the data are (or may be) stored as a dense range mage. It reverses the role of model and data n the matchng process, and n dong so makes matchng partcularly smple, requrng no teratve

20 RPI-CS-TR closest pont search. As a result t s smpler, faster and more accurate than current regstraton algorthms. The man lmtaton of the algorthms derved here s that the covarance matrces are assumed to be known n advance. Ths s a problem when measurement error depends on surface orentaton because ths orentaton s not known unless t s estmated from the data or taken from the regstered model. One soluton s to run an algorthm such as C-ICP4 to convergence and then use surface orentatons for the regstered model to rentalze the covarance matrces. Once ths s complete the more general algorthm C-ICP2 may be used to refne the transformaton estmate. Practcally, ths seems unlkely to be a major concern, (perhaps reference back to expermental results) although ths ntuton wll need to be confrmed expermentally, Fnally, the covarance-base regstraton algorthms may be extended trvally to handle multple range data sets. If the transformaton between these data sets s known n advance, for example by rotatng the object usng a hghprecson rotary stage, the multple data sets provde smultaneous constrants on the poston of the object wthn the stage. If the transformaton between data sets s not known, regstraton of the model aganst each data set determnes the calbraton transformatons between them. Appendx A: Specal Cases of Regstraton Ths appendx shows that multvarate locatons and lnear regresson are specal cases of regstraton estmaton. Ths means that known results about bas n these estmaton problems carry over to regstraton. The dervatons buld on the problem formulaton n Equaton 5 and the closest pont and dstance equatons for lnear models developed n Secton 3.1. The multvarate locaton problem s obtaned from regstraton by choosng the model f(p) = p = 0. In dong ths, rotaton becomes rrelevant and Equatons 4 and 5 reduce to (t q ) T S 1 (t q ), whch s equvalent to (1). The lnear regresson problem s obtaned from regstraton for lnear models. For lnear models, the objectve functon (5) reduces to [ˆη T (p 0 + R T t R T q )] 2 ˆη T. (24) R T S R ^η Ths may be smplfed much further by choosng the plane z = 0, whch makes p 0 = 0 and ˆη = (0, 0, 1) T. In dong ths, restrct attenton to R 2, wrte ( ) cos θ sn θ R =, sn θ cos θ

21 RPI-CS-TR and defne γ = (cos θ, sn θ) t. Makng all these substtutons n (24) and smplfyng yelds the optmzaton equaton Wth S = dag(0, σ 2 ), ths s equvalent to (x m y + b) 2 (x sn θ y cos θ + γ) 2 [ sn θ, cos θ] S [ sn θ, cos θ] T. (25) where m = tan θ, b = γ/ cos θ, and the scale term has been dropped. Wth S = dag(σ 2, σ 2 ), ths s equvalent to (x sn θ y cos θ + γ) 2. These are the ordnary and orthogonal regresson objectve functons as dscussed n the ntroducton (2). References [1] J. Batlle, E. Mouaddb, and J. Salv. Recent progress n coded structured lght as a technque to solve the correspondence problem: A survey. Pattern Recognton, 31(7): , [2] A. E. Beaton and J. W. Tukey. The fttng of power seres, meanng polynomals, llustrated on band-spectroscopc data. Technometrcs, 16: , [3] R. Bergevn, M. Soucy, H. Gagnon, and D. Laurendeau. Towards a general multvew regstraton technque. IEEE Trans. on PAMI, 18(5): , [4] P. Besl and N. McKay. A method for regstraton of 3-d shapes. IEEE Trans. on PAMI, 14(2): , [5] S. Blosten and T. Huang. Error analyss n stereo determnaton of 3-d pont postons. IEEE Trans. on PAMI, 9(6): , [6] G. Champleboux, S. Lavallee, R. Szelsk, and L. Brune. From accurate range magng sensor calbraton to accurate model-based 3-d object localzaton. In Proc. CVPR, pages 83 89, [7] Y. Chen and G. Medon. Object modelng by regstraton of multple range mages. Image and Vson Computng, 10(3): , [8] C. Chua and R. Jarvs. 3d free-form surface regstraton and object recognton. Int. J. of Computer Vson, 17(1):77 99, [9] B. Curless and M. Levoy. A volumetrc method for buldng complex models from range mages. In SIGGRAPH 96, pages , [10] G. J. Ettnger. Herarchcal Three-Dmensonal Medcal Image Regstraton. PhD thess, MIT, [11] O. Faugeras. Three-Dmensonal Computer Vson. MIT Press, [12] O. Faugeras and M. Hebert. The representaton, recognton, and locatng of 3-d objects. Int. J. of Robotcs Research, 5(3):27 52, [13] J. Feldmar, J. Declerck, G. Malandan, and N. Ayache. Extenson of the ICP algorthm to nonrgd ntensty-based regstraton of 3d volumes. Computer Vson and Image Understandng, 66(2): , May 1997.

22 RPI-CS-TR [14] P. Fua and Y. Leclerc. Regstraton wthout correspondences. In Proc. CVPR, pages , [15] W. Grmson, T. Lozano-Perez, W. Wells, G. Ettnger, and S. Whte. An automatc regstraton method for frameless stereotaxy, mage, guded surgery and enhanced realty vsualzaton. In Proc. CVPR, pages , [16] F. R. Hampel, P. J. Rousseeuw, E. Ronchett, and W. A. Stahel. Robust Statstcs: The Approach Based on Influence Functons. John Wley & Sons, [17] P. Hébert. From Ponts to Shape Recovery: Relable Geometrc Prmtve Extracton. PhD thess, Unversté Laval, [18] K. Hguch, M. Hebert, and K. Ikeuch. Buldng 3-d models from unregstered range mages. Graphcal Models and Image Processng, 57(4): , [19] A. Hlton, A. Stoddart, J. Illngworth, and T. Wndeatt. Relable surface reconstructon from multple range mages. In Proc. 4th ECCV, pages , [20] P. W. Holland and R. E. Welsch. Robust regresson usng teratvely reweghted leastsquares. Commun. Statst.-Theor. Meth., A6: , [21] E. Horn and N. Kryat. Toward optmal structured lght patterns. Image and Vson Computng, 17(2):87 97, [22] D. P. Huttenlocher and S. Ullman. Recognzng sold objects by algnment. Int. J. of Computer Vson, 5(2): , [23] M. Iran, P. Anandan, and S. Hsu. Mosac based representatons of vdeo sequences and ther applcatons. In Proc. IEEE Int. Conf. on Computer Vson, pages , [24] A. Johnson and M. Hebert. Surface matchng for object recognton n complex 3- dmensonal scenes. Image and Vson Computng, 16(9-10): , July [25] A. Johnson and S. Kang. Regstraton and ntegraton of textured 3-d data. Image and Vson Computng, 17(2): , [26] R. Krshnapuram and D. Casasent. Determnaton of three dmensonal object locaton and orentaton from range data. IEEE Trans. on PAMI, 11(11): , [27] S. Lavallee and R. Szelsk. Recoverng the poston and orentaton of free-form objects from mage contours usng 3d dstance maps. IEEE Trans. on PAMI, 17(4): , [28] D. Lowe. Three-dmensonal object recognton from sngle two-dmensonal mages. Artfcal Intellgence, 31(3): , [29] T. Masuda and N. Yokoya. A robust method for regstraton and segmentaton of multple range mages. Computer Vson and Image Understandng, 61(3): , [30] L. Matthes and S. Shafer. Error modellng n stereo navgaton. IEEE J. of Robotcs and Automaton, 3(3): , [31] P. Meer, D. Mntz, A. Rosenfeld, and D. Y. Km. Robust regresson methods for computer vson: A revew. Int. J. of Computer Vson, 6:59 70, [32] C.-H. Menq, H.-T. Yau, and G.-Y. La. Automated precson measurement of surface profle n CAD-drected nspecton. IEEE Trans. on Robotcs and Automaton, 8(2): , [33] T. S. Newman and A. K. Jan. A survey of automated vsual nspecton. Computer Vson and Image Understandng, 61(2): , [34] P. J. Rousseeuw and A. M. Leroy. Robust Regresson and Outler Detecton. John Wley & Sons, [35] M. Rutshauser, M. Strcker, and M. Trobna. Mergng range mages of arbtrarly shaped objects. In Proc. CVPR, pages , [36] K. Sato and S. Inokuch. Range-magng system utlzng nematc lqud crystal mask. In Proc. IEEE Int. Conf. on Computer Vson, pages , 1987.

23 RPI-CS-TR [37] H. Sawhney and R. Kumar. True mult-mage algnment and ts applcaton to mosacng and lens dstorton correcton. IEEE Trans. on PAMI, 21(3): , [38] C. V. Stewart. Robust parameter estmaton n computer vson. SIAM Revews, 41(3), September [39] A. Stoddart, S. Lemke, A. Hlton, and T. Renn. Estmatng pose uncertanty for surface regstraton. Image and Vson Computng, 16(2): , [40] R. Szelsk. Vdeo mosacs for vrtual envronments. IEEE CGA, 16(2):22 30, [41] R. Szelsk and S. Lavallee. Matchng 3-d anatomcal surfaces wth non-rgd deformatons usng octree-splnes. Int. J. of Computer Vson, 18(2): , [42] P. Torr and D. Murray. The development and comparson of robust methods for estmatng the fundamental matrx. Int. J. of Computer Vson, 24(3): , [43] M. Trobna. Error model of a coded-lght range sensor. Techncal Report BIWI-TR-164, ETH, [44] P. Vola and W. Wells III. Algnment by maxmzaton of mutual nformaton. In Proc. IEEE Int. Conf. on Computer Vson, pages 16 23, [45] J. Weng, T. Huang, and N. Ahuja. Moton and structure from two perspectve vews: Algorthms, error analyss, and error estmaton. IEEE Trans. on PAMI, 11(5): , [46] Z. Zhang. Iteratve pont matchng for regstraton of free-form curves and surfaces. Int. J. of Computer Vson, 13(2): , 1994.