6.1 2D and 3D feature-based alignment 275. similarity. Euclidean

Transcription

1 6.1 2D and 3D feature-based algnment 275 y translaton smlarty projectve Eucldean affne x Fgure 6.2 Basc set of 2D planar transformatons Once we have extracted features from mages, the next stage n many vson algorthms s to match these features across dfferent mages (Secton 4.1.3). An mportant component of ths matchng s to verfy whether the set of matchng features s geometrcally consstent, e.g., whether the feature dsplacements can be descrbed by a smple 2D or 3D geometrc transformaton. The computed motons can then be used n other applcatons such as mage sttchng (Chapter 9) or augmented realty (Secton 6.2.3). In ths chapter, we look at the topc of geometrc mage regstraton,.e., the computaton of 2D and 3D transformatons that map features n one mage to another (Secton 6.1). One specal case of ths problem s pose estmaton, whch s determnng a camera s poston relatve to a known 3D object or scene (Secton 6.2). Another case s the computaton of a camera s ntrnsc calbraton, whch conssts of the nternal parameters such as focal length and radal dstorton (Secton 6.3). In Chapter 7, we look at the related problems of how to estmate 3D pont structure from 2D matches (trangulaton) and how to smultaneously estmate 3D geometry and camera moton (structure from moton) D and 3D feature-based algnment Feature-based algnment s the problem of estmatng the moton between two or more sets of matched 2D or 3D ponts. In ths secton, we restrct ourselves to global parametrc transformatons, such as those descrbed n Secton and shown n Table 2.1 and Fgure 6.2, or hgher order transformaton for curved surfaces (Shashua and Toelg 1997; Can, Stewart, Roysam et al. 2002). Applcatons to non-rgd or elastc deformatons (Booksten 1989; Szelsk and Lavallée 1996; Torresan, Hertzmann, and Bregler 2008) are examned n Sectons 8.3 and D algnment usng least squares Gven a set of matched feature ponts {(x, x )} and a planar parametrc transformaton1 of the form x = f(x; p), (6.1) 1 For examples of non-planar parametrc models, such as quadrcs, see the work of Shashua and Toelg (1997); Shashua and Wexler (2001).

2 276 6 Feature-based algnment Transform Matrx Parameters p Jacoban J [ 1 0 tx [ 1 0 translaton 0 1 t y (t x,t y ) 0 1 [ cθ s θ t x [ 1 0 sθ x c θ y Eucldean s θ c θ t y (t x,t y,θ) 0 1 c θ x s θ y [ 1+a b tx [ 1 0 x y smlarty b 1+a t y (t x,t y,a,b) 0 1 y x [ 1+a00 a 01 t x [ 1 0 x y 0 0 affne a 10 1+a 11 t y (t x,t y,a 00,a 01,a 10,a 11 ) x y 1+h 00 h 01 h 02 h 10 1+h 11 h 12 projectve h 20 h 21 1 (h 00,h 01,...,h 21 ) (see Secton 6.1.3) Table 6.1 Jacobans of the 2D coordnate transformatons x = f(x; p) shown n Table 2.1, where we have re-parameterzed the motons so that they are dentty for p =0. how can we produce the best estmate of the moton parameters p? The usual way to do ths s to use least squares,.e., to mnmze the sum of squared resduals E LS = r 2 = f(x ; p) x 2, (6.2) where r = f(x ; p) x = ˆx x (6.3) s the resdual between the measured locaton ˆx and ts correspondng current predcted locaton x = f(x ; p). (See Appendx A.2 for more on least squares and Appendx B.2 for a statstcal justfcaton.) Many of the moton models presented n Secton and Table 2.1,.e., translaton, smlarty, and affne, have a lnear relatonshp between the amount of moton Δx = x x and the unknown parameters p, Δx = x x = J(x)p, (6.4) where J = f/ p s the Jacoban of the transformaton f wth respect to the moton parameters p (see Table 6.1). In ths case, a smple lnear regresson (lnear least squares problem) can be formulated as E LLS = = p T [ J(x )p Δx 2 (6.5) J T (x )J(x ) p 2p T [ J T (x )Δx + Δx 2 (6.6) = p T Ap 2p T b + c. (6.7)

3 6.1 2D and 3D feature-based algnment 277 The mnmum can be found by solvng the symmetrc postve defnte (SPD) system of normal equatons 2 Ap = b, (6.8) where A = J T (x )J(x ) (6.9) s called the Hessan and b = J T (x )Δx. For the case of pure translaton, the resultng equatons have a partcularly smple form,.e., the translaton s the average translaton between correspondng ponts or, equvalently, the translaton of the pont centrods. Uncertanty weghtng. The above least squares formulaton assumes that all feature ponts are matched wth the same accuracy. Ths s often not the case, snce certan ponts may fall nto more textured regons than others. If we assocate a scalar varance estmate σ 2 wth each correspondence, we can mnmze the weghted least squares problem nstead, 3 E WLS = σ 2 r 2. (6.10) As shown n Secton 8.1.3, a covarance estmate for patch-based matchng can be obtaned by multplyng the nverse of the patch Hessan A (8.55) wth the per-pxel nose covarance σn 2 (8.44). Weghtng each squared resdual by ts nverse covarance Σ 1 = σn 2 A (whch s called the nformaton matrx), we obtan E CWLS = r 2 Σ 1 = r T Σ 1 r = σ 2 n r T A r. (6.11) Applcaton: Panography One of the smplest (and most fun) applcatons of mage algnment s a specal form of mage sttchng called panography. In a panograph, mages are translated and optonally rotated and scaled before beng blended wth smple averagng (Fgure 6.3). Ths process mmcs the photographc collages created by artst Davd Hockney, although hs compostons use an opaque overlay model, beng created out of regular photographs. In most of the examples seen on the Web, the mages are algned by hand for best artstc effect. 4 However, t s also possble to use feature matchng and algnment technques to perform the regstraton automatcally (Nomura, Zhang, and Nayar 2007; Zelnk-Manor and Perona 2007). Consder a smple translatonal model. We want all the correspondng features n dfferent mages to lne up as best as possble. Let t j be the locaton of the jth mage coordnate frame n the global composte frame and x j be the locaton of the th matched feature n the jth mage. In order to algn the mages, we wsh to mnmze the least squares error E PLS = j (t j + x j ) x 2, (6.12) 2 For poorly condtoned problems, t s better to use QR decomposton on the set of lnear equatons J(x )p = Δx nstead of the normal equatons (Björck 1996; Golub and Van Loan 1996). However, such condtons rarely arse n mage regstraton. 3 Problems where each measurement can have a dfferent varance or certanty are called heteroscedastc models. 4

4 278 6 Feature-based algnment Fgure 6.3 A smple panograph consstng of three mages automatcally algned wth a translatonal model and then averaged together. where x s the consensus (average) poston of feature n the global coordnate frame. (An alternatve approach s to regster each par of overlappng mages separately and then compute a consensus locaton for each frame see Exercse 6.2.) The above least squares problem s ndetermnate (you can add a constant offset to all the frame and pont locatons t j and x ). To fx ths, ether pck one frame as beng at the orgn or add a constrant to make the average frame offsets be 0. The formulas for addng rotaton and scale transformatons are straghtforward and are left as an exercse (Exercse 6.2). See f you can create some collages that you would be happy to share wth others on the Web Iteratve algorthms Whle lnear least squares s the smplest method for estmatng parameters, most problems n computer vson do not have a smple lnear relatonshp between the measurements and the unknowns. In ths case, the resultng problem s called non-lnear least squares or non-lnear regresson. Consder, for example, the problem of estmatng a rgd Eucldean 2D transformaton (translaton plus rotaton) between two sets of ponts. If we parameterze ths transformaton by the translaton amount (t x,t y ) and the rotaton angle θ, as n Table 2.1, the Jacoban of ths transformaton, gven n Table 6.1, depends on the current value of θ. Notce how n Table 6.1, we have re-parameterzed the moton matrces so that they are always the dentty at the orgn p =0, whch makes t easer to ntalze the moton parameters. To mnmze the non-lnear least squares problem, we teratvely fnd an update Δp to the current parameter estmate p by mnmzng E NLS (Δp) = f(x ; p +Δp) x 2 (6.13) J(x ; p)δp r 2 (6.14)

5 6.1 2D and 3D feature-based algnment 279 = Δp T [ J T J Δp 2Δp T [ J T r + r 2 (6.15) = Δp T AΔp 2Δp T b + c, (6.16) where the Hessan 5 A s the same as Equaton (6.9) and the rght hand sde vector b = J T (x )r (6.17) s now a Jacoban-weghted sum of resdual vectors. Ths makes ntutve sense, as the parameters are pulled n the drecton of the predcton error wth a strength proportonal to the Jacoban. Once A and b have been computed, we solve for Δp usng (A + λdag(a))δp = b, (6.18) and update the parameter vector p p +Δp accordngly. The parameter λ s an addtonal dampng parameter used to ensure that the system takes a downhll step n energy (squared error) and s an essental component of the Levenberg Marquardt algorthm (descrbed n more detal n Appendx A.3). In many applcatons, t can be set to 0 f the system s successfully convergng. For the case of our 2D translaton+rotaton, we end up wth a 3 3 set of normal equatons n the unknowns (δt x,δt y,δθ). An ntal guess for (t x,t y,θ) can be obtaned by fttng a four-parameter smlarty transform n (t x,t y,c,s) and then settng θ = tan 1 (s/c). An alternatve approach s to estmate the translaton parameters usng the centrods of the 2D ponts and to then estmate the rotaton angle usng polar coordnates (Exercse 6.3). For the other 2D moton models, the dervatves n Table 6.1 are all farly straghtforward, except for the projectve 2D moton (homography), whch arses n mage-sttchng applcatons (Chapter 9). These equatons can be re-wrtten from (2.21) n ther new parametrc form as x = (1 + h 00)x + h 01 y + h 02 h 20 x + h 21 y +1 The Jacoban s therefore J = f p = 1 D and y = h 10x +(1+h 11 )y + h 12. (6.19) h 20 x + h 21 y +1 [ x y x x x y x y 1 y x y y, (6.20) where D = h 20 x + h 21 y +1s the denomnator n (6.19), whch depends on the current parameter settngs (as do x and y ). An ntal guess for the eght unknowns {h 00,h 01,...,h 21 } can be obtaned by multplyng both sdes of the equatons n (6.19) through by the denomnator, whch yelds the lnear set of equatons, [ ˆx x ŷ y = [ x y ˆx x ˆx y x y 1 ŷ x ŷ y h 00.. (6.21) 5 The Hessan A s not the true Hessan (second dervatve) of the non-lnear least squares problem (6.13). Instead, t s the approxmate Hessan, whch neglects second (and hgher) order dervatves of f(x ; p +Δp). h 21

6 280 6 Feature-based algnment However, ths s not optmal from a statstcal pont of vew, snce the denomnator D, whch was used to multply each equaton, can vary qute a bt from pont to pont. 6 One way to compensate for ths s to reweght each equaton by the nverse of the current estmate of the denomnator, D, [ 1 ˆx x D ŷ y = 1 D [ x y ˆx x ˆx y x y 1 ŷ x ŷ y h 00.. h 21. (6.22) Whle ths may at frst seem to be the exact same set of equatons as (6.21), because least squares s beng used to solve the over-determned set of equatons, the weghtngs do matter and produce a dfferent set of normal equatons that performs better n practce. The most prncpled way to do the estmaton, however, s to drectly mnmze the squared resdual equatons (6.13) usng the Gauss Newton approxmaton,.e., performng a frstorder Taylor seres expanson n p, as shown n (6.14), whch yelds the set of equatons [ ˆx x ŷ ỹ = 1 D [ x y x x x y x y 1 ỹ x ỹ y Δh 00. Δh 21. (6.23) Whle these look smlar to (6.22), they dffer n two mportant respects. Frst, the left hand sde conssts of unweghted predcton errors rather than pont dsplacements and the soluton vector s a perturbaton to the parameter vector p. Second, the quanttes nsde J nvolve predcted feature locatons ( x, ỹ ) nstead of sensed feature locatons (ˆx, ŷ ). Both of these dfferences are subtle and yet they lead to an algorthm that, when combned wth proper checkng for downhll steps (as n the Levenberg Marquardt algorthm), wll converge to a local mnmum. Note that teratng Equatons (6.22) s not guaranteed to converge, snce t s not mnmzng a well-defned energy functon. Equaton (6.23) s analogous to the addtve algorthm for drect ntensty-based regstraton (Secton 8.2), snce the change to the full transformaton s beng computed. If we prepend an ncremental homography to the current homography nstead,.e., we use a compostonal algorthm (descrbed n Secton 8.2), we get D =1(snce p =0) and the above formula smplfes to [ ˆx x ŷ y = [ x y x 2 xy x y 1 xy y 2 Δh 00., (6.24) Δh 21 where we have replaced ( x, ỹ ) wth (x, y) for concseness. (Notce how ths results n the same Jacoban as (8.63).) 6 Hartley and Zsserman (2004) call ths strategy of formng lnear equatons from ratonal equatons the drect lnear transform, but that term s more commonly assocated wth pose estmaton (Secton 6.2). Note also that our defnton of the h j parameters dffers from that used n ther book, snce we defne h to be the dfference from unty and we do not leave h 22 as a free parameter, whch means that we cannot handle certan extreme homographes.

7 6.1 2D and 3D feature-based algnment Robust least squares and RANSAC Whle regular least squares s the method of choce for measurements where the nose follows a normal (Gaussan) dstrbuton, more robust versons of least squares are requred when there are outlers among the correspondences (as there almost always are). In ths case, t s preferable to use an M-estmator (Huber 1981; Hampel, Ronchett, Rousseeuw et al. 1986; Black and Rangarajan 1996; Stewart 1999), whch nvolves applyng a robust penalty functon ρ(r) to the resduals E RLS (Δp) = ρ( r ) (6.25) nstead of squarng them. We can take the dervatve of ths functon wth respect to p and set t to 0, ψ( r ) r p = ψ( r ) r rt r p =0, (6.26) where ψ(r) =ρ (r) s the dervatve of ρ and s called the nfluence functon. If we ntroduce a weght functon, w(r) =Ψ(r)/r, we observe that fndng the statonary pont of (6.25) usng (6.26) s equvalent to mnmzng the teratvely reweghted least squares (IRLS) problem E IRLS = w( r ) r 2, (6.27) where the w( r ) play the same local weghtng role as σ 2 n (6.10). The IRLS algorthm alternates between computng the nfluence functons w( r ) and solvng the resultng weghted least squares problem (wth fxed w values). Other ncremental robust least squares algorthms can be found n the work of Sawhney and Ayer (1996); Black and Anandan (1996); Black and Rangarajan (1996); Baker, Gross, Ishkawa et al. (2003) and textbooks and tutorals on robust statstcs (Huber 1981; Hampel, Ronchett, Rousseeuw et al. 1986; Rousseeuw and Leroy 1987; Stewart 1999). Whle M-estmators can defntely help reduce the nfluence of outlers, n some cases, startng wth too many outlers wll prevent IRLS (or other gradent descent algorthms) from convergng to the global optmum. A better approach s often to fnd a startng set of nler correspondences,.e., ponts that are consstent wth a domnant moton estmate. 7 Two wdely used approaches to ths problem are called RANdom SAmple Consensus, or RANSAC for short (Fschler and Bolles 1981), and least medan of squares (LMS) (Rousseeuw 1984). Both technques start by selectng (at random) a subset of k correspondences, whch s then used to compute an ntal estmate for p. The resduals of the full set of correspondences are then computed as r = x (x ; p) ˆx, (6.28) where x are the estmated (mapped) locatons and ˆx are the sensed (detected) feature pont locatons. The RANSAC technque then counts the number of nlers that are wthn ɛ of ther predcted locaton,.e., whose r ɛ. (The ɛ value s applcaton dependent but s often around 1 3 pxels.) Least medan of squares fnds the medan value of the r 2 values. The 7 For pxel-based algnment methods (Secton 8.1.1), herarchcal (coarse-to-fne) technques are often used to lock onto the domnant moton n a scene.

8 282 6 Feature-based algnment k p S Table 6.2 Number of trals S to attan a 99% probablty of success (Stewart 1999). random selecton process s repeated S tmes and the sample set wth the largest number of nlers (or wth the smallest medan resdual) s kept as the fnal soluton. Ether the ntal parameter guess p or the full set of computed nlers s then passed on to the next data fttng stage. When the number of measurements s qute large, t may be preferable to only score a subset of the measurements n an ntal round that selects the most plausble hypotheses for addtonal scorng and selecton. Ths modfcaton of RANSAC, whch can sgnfcantly speed up ts performance, s called Preemptve RANSAC (Nstér 2003). In another varant on RANSAC called PROSAC (PROgressve SAmple Consensus), random samples are ntally added from the most confdent matches, thereby speedng up the process of fndng a (statstcally) lkely good set of nlers (Chum and Matas 2005). To ensure that the random samplng has a good chance of fndng a true set of nlers, a suffcent number of trals S must be tred. Let p be the probablty that any gven correspondence s vald and P be the total probablty of success after S trals. The lkelhood n one tral that all k random samples are nlers s p k. Therefore, the lkelhood that S such trals wll all fal s 1 P =(1 p k ) S (6.29) and the requred mnmum number of trals s S = log(1 P ) log(1 p k ). (6.30) Stewart (1999) gves examples of the requred number of trals S to attan a 99% probablty of success. As you can see from Table 6.2, the number of trals grows quckly wth the number of sample ponts used. Ths provdes a strong ncentve to use the mnmum number of sample ponts k possble for any gven tral, whch s how RANSAC s normally used n practce. Uncertanty modelng In addton to robustly computng a good algnment, some applcatons requre the computaton of uncertanty (see Appendx B.6). For lnear problems, ths estmate can be obtaned by nvertng the Hessan matrx (6.9) and multplyng t by the feature poston nose (f these have not already been used to weght the ndvdual measurements, as n Equatons (6.10) and 6.11)). In statstcs, the Hessan, whch s the nverse covarance, s sometmes called the (Fsher) nformaton matrx (Appendx B.1.1). When the problem nvolves non-lnear least squares, the nverse of the Hessan matrx provdes the Cramer Rao lower bound on the covarance matrx,.e., t provdes the mnmum

9 6.1 2D and 3D feature-based algnment 283 amount of covarance n a gven soluton, whch can actually have a wder spread ( longer tals ) f the energy flattens out away from the local mnmum where the optmal soluton s found D algnment Instead of algnng 2D sets of mage features, many computer vson applcatons requre the algnment of 3D ponts. In the case where the 3D transformatons are lnear n the moton parameters, e.g., for translaton, smlarty, and affne, regular least squares (6.5) can be used. The case of rgd (Eucldean) moton, E R3D = x Rx t 2, (6.31) whch arses more frequently and s often called the absolute orentaton problem (Horn 1987), requres slghtly dfferent technques. If only scalar weghtngs are beng used (as opposed to full 3D per-pont ansotropc covarance estmates), the weghted centrods of the two pont clouds c and c can be used to estmate the translaton t = c Rc. 8 We are then left wth the problem of estmatng the rotaton between two sets of ponts {ˆx = x c} and {ˆx = x c } that are both centered at the orgn. One commonly used technque s called the orthogonal Procrustes algorthm (Golub and Van Loan 1996, p. 601) and nvolves computng the sngular value decomposton (SVD) of the 3 3 correlaton matrx C = ˆx ˆx T = UΣV T. (6.32) The rotaton matrx s then obtaned as R = UV T. (Verfy ths for yourself when ˆx = Rˆx.) Another technque s the absolute orentaton algorthm (Horn 1987) for estmatng the unt quaternon correspondng to the rotaton matrx R, whch nvolves formng a 4 4 matrx from the entres n C and then fndng the egenvector assocated wth ts largest postve egenvalue. Lorusso, Eggert, and Fsher (1995) expermentally compare these two technques to two addtonal technques proposed n the lterature, but fnd that the dfference n accuracy s neglgble (well below the effects of measurement nose). In stuatons where these closed-form algorthms are not applcable, e.g., when full 3D covarances are beng used or when the 3D algnment s part of some larger optmzaton, the ncremental rotaton update ntroduced n Secton ( ), whch s parameterzed by an nstantaneous rotaton vector ω, can be used (See Secton for an applcaton to mage sttchng.) In some stuatons, e.g., when mergng range data maps, the correspondence between data ponts s not known a pror. In ths case, teratve algorthms that start by matchng nearby ponts and then update the most lkely correspondence can be used (Besl and McKay 1992; Zhang 1994; Szelsk and Lavallée 1996; Gold, Rangarajan, Lu et al. 1998; Davd, DeMenthon, Duraswam et al. 2004; L and Hartley 2007; Enqvst, Josephson, and Kahl 2009). These technques are dscussed n more detal n Secton When full covarances are used, they are transformed by the rotaton and so a closed-form soluton for translaton s not possble.

10 284 6 Feature-based algnment 6.2 Pose estmaton A partcular nstance of feature-based algnment, whch occurs very often, s estmatng an object s 3D pose from a set of 2D pont projectons. Ths pose estmaton problem s also known as extrnsc calbraton, as opposed to the ntrnsc calbraton of nternal camera parameters such as focal length, whch we dscuss n Secton 6.3. The problem of recoverng pose from three correspondences, whch s the mnmal amount of nformaton necessary, s known as the perspectve-3-pont-problem (P3P), wth extensons to larger numbers of ponts collectvely known as PnP (Haralck, Lee, Ottenberg et al. 1994; Quan and Lan 1999; Moreno-Noguer, Lepett, and Fua 2007). In ths secton, we look at some of the technques that have been developed to solve such problems, startng wth the drect lnear transform (DLT), whch recovers a 3 4 camera matrx, followed by other lnear algorthms, and then lookng at statstcally optmal teratve algorthms Lnear algorthms The smplest way to recover the pose of the camera s to form a set of lnear equatons analogous to those used for 2D moton estmaton (6.19) from the camera matrx form of perspectve projecton ( ), x = p 00X + p 01 Y + p 02 Z + p 03 p 20 X + p 21 Y + p 22 Z + p 23 (6.33) y = p 10X + p 11 Y + p 12 Z + p 13 p 20 X + p 21 Y + p 22 Z + p 23, (6.34) where (x,y ) are the measured 2D feature locatons and (X,Y,Z ) are the known 3D feature locatons (Fgure 6.4). As wth (6.21), ths system of equatons can be solved n a lnear fashon for the unknowns n the camera matrx P by multplyng the denomnator on both sdes of the equaton. 9 The resultng algorthm s called the drect lnear transform (DLT) and s commonly attrbuted to Sutherland (1974). (For a more n-depth dscusson, refer to the work of Hartley and Zsserman (2004).) In order to compute the 12 (or 11) unknowns n P, at least sx correspondences between 3D and 2D locatons must be known. As wth the case of estmatng homographes ( ), more accurate results for the entres n P can be obtaned by drectly mnmzng the set of Equatons ( ) usng non-lnear least squares wth a small number of teratons. Once the entres n P have been recovered, t s possble to recover both the ntrnsc calbraton matrx K and the rgd transformaton (R, t) by observng from Equaton (2.56) that P = K[R t. (6.35) Snce K s by conventon upper-trangular (see the dscusson n Secton 2.1.5), both K and R can be obtaned from the front 3 3 sub-matrx of P usng RQ factorzaton (Golub and Van Loan 1996) Because P s unknown up to a scale, we can ether fx one of the entres, e.g., p 23 =1, or fnd the smallest sngular vector of the set of lnear equatons. 10 Note the unfortunate clash of termnologes: In matrx algebra textbooks, R represents an upper-trangular matrx; n computer vson, R s an orthogonal rotaton.

11 6.2 Pose estmaton 285 p = (X,Y,Z,W ) d d j c j x x j d j p j Fgure 6.4 Pose estmaton by the drect lnear transform and by measurng vsual angles and dstances between pars of ponts. In most applcatons, however, we have some pror knowledge about the ntrnsc calbraton matrx K, e.g., that the pxels are square, the skew s very small, and the optcal center s near the center of the mage ( ). Such constrants can be ncorporated nto a non-lnear mnmzaton of the parameters n K and (R, t), as descrbed n Secton In the case where the camera s already calbrated,.e., the matrx K s known (Secton 6.3), we can perform pose estmaton usng as few as three ponts (Fschler and Bolles 1981; Haralck, Lee, Ottenberg et al. 1994; Quan and Lan 1999). The basc observaton that these lnear PnP (perspectve n-pont) algorthms employ s that the vsual angle between any par of 2D ponts ˆx and ˆx j must be the same as the angle between ther correspondng 3D ponts p and p j (Fgure 6.4). Gven a set of correspondng 2D and 3D ponts {(ˆx, p )}, where the ˆx are unt drectons obtaned by transformng 2D pxel measurements x to unt norm 3D drectons ˆx through the nverse calbraton matrx K, ˆx = N (K 1 x )=K 1 x / K 1 x, (6.36) the unknowns are the dstances d from the camera orgn c to the 3D ponts p, where (Fgure 6.4). The cosne law for trangle Δ(c, p, p j ) gves us where and p = d ˆx + c (6.37) f j (d,d j )=d 2 + d 2 j 2d d j c j d 2 j =0, (6.38) c j = cos θ j = ˆx ˆx j (6.39) d 2 j = p p j 2. (6.40) We can take any trplet of constrants (f j,f k,f jk ) and elmnate the d j and d k usng Sylvester resultants (Cox, Lttle, and O Shea 2007) to obtan a quartc equaton n d 2, g jk (d 2 )=a 4 d 8 + a 3 d 6 + a 2 d 4 + a 1 d 2 + a 0 =0. (6.41) Gven fve or more correspondences, we can generate (n 1)(n 2) 2 trplets to obtan a lnear estmate (usng SVD) for the values of (d 8,d6,d4,d2 ) (Quan and Lan 1999). Estmates for

12 286 6 Feature-based algnment d 2 can computed as ratos of successve d2n+2 estmates and these can be averaged to obtan a fnal estmate of d 2 (and hence d ). Once the ndvdual estmates of the d dstances have been computed, we can generate a 3D structure consstng of the scaled pont drectons d ˆx, whch can then be algned wth the 3D pont cloud {p } usng absolute orentaton (Secton 6.1.5) to obtaned the desred pose estmate. Quan and Lan (1999) gve accuracy results for ths and other technques, whch use fewer ponts but requre more complcated algebrac manpulatons. The paper by Moreno-Noguer, Lepett, and Fua (2007) revews more recent alternatves and also gves a lower complexty algorthm that typcally produces more accurate results. Unfortunately, because mnmal PnP solutons can be qute nose senstve and also suffer from bas-relef ambgutes (e.g., depth reversals) (Secton 7.4.3), t s often preferable to use the lnear sx-pont algorthm to guess an ntal pose and then optmze ths estmate usng the teratve technque descrbed n Secton An alternatve pose estmaton algorthm nvolves startng wth a scaled orthographc projecton model and then teratvely refnng ths ntal estmate usng a more accurate perspectve projecton model (DeMenthon and Davs 1995). The attracton of ths model, as stated n the paper s ttle, s that t can be mplemented n 25 lnes of [Mathematca code. /d 2n Iteratve algorthms The most accurate (and flexble) way to estmate pose s to drectly mnmze the squared (or robust) reprojecton error for the 2D ponts as a functon of the unknown pose parameters n (R, t) and optonally K usng non-lnear least squares (Tsa 1987; Bogart 1991; Glecher and Wtkn 1992). We can wrte the projecton equatons as x = f(p ; R, t, K) (6.42) and teratvely mnmze the robustfed lnearzed reprojecton errors E NLP = ( ) f f f ρ ΔR + Δt + R t K ΔK r, (6.43) where r = x ˆx s the current resdual vector (2D error n predcted poston) and the partal dervatves are wth respect to the unknown pose parameters (rotaton, translaton, and optonally calbraton). Note that f full 2D covarance estmates are avalable for the 2D feature locatons, the above squared norm can be weghted by the nverse pont covarance matrx, as n Equaton (6.11). An easer to understand (and mplement) verson of the above non-lnear regresson problem can be constructed by re-wrtng the projecton equatons as a concatenaton of smpler steps, each of whch transforms a 4D homogeneous coordnate p by a smple transformaton such as translaton, rotaton, or perspectve dvson (Fgure 6.5). The resultng projecton equatons can be wrtten as y (1) = f T (p ; c j )=p c j, (6.44) y (2) = f R (y (1) ; q j )=R(q j ) y (1), (6.45) y (3) = f P (y (2) )= y(2), z (2) (6.46) x = f C (y (3) ; k) =K(k) y (3). (6.47)