New Extensons of the 3-Smplex for Exteror Orentaton John M. Stenbs Tyrone L. Vncent Wllam A. Hoff Colorado School of Mnes jstenbs@gmal.com tvncent@mnes.edu whoff@mnes.edu Abstract Object pose may be determned from a set of 2D mage ponts and correspondng 3D model ponts, gven the camera s ntrnsc parameters. In ths paper, two new exteror orentaton algorthms are proposed and then compared aganst the Effcent PnP Method and Orthogonal Iteraton Algorthm. As an alternatve to the homogeneous transformaton, both algorthms utlze the 3-smplex as a pose parameterzaton. One algorthm uses a semdefnte program and the other a Gauss-Newton algorthm. 1. Introducton By applyng computer vson technques, object pose may be determned from a set of 2D mage ponts and correspondng 3D model ponts, gven the camera s ntrnsc parameters. In general, the problem has been gven the name exteror orentaton, and for an arbtrary number of ponts t s called perspectve-n-pont or PnP. Dffculty arses due to mage pont nose, nonlnearty of perspectve projecton, and the constrants placed on the elements of the 6-DOF pose. There s a long hstory of study n ths area and many algorthms have been proposed. Lnear algorthms can be very fast but are typcally not that accurate due to relaxaton of pose constrants. Iteratve algorthms constran the pose drectly and are usually very accurate, but requre a reasonably good ntal guess for fast and proper convergence to the global mnma. A revew and comparson of exstng exteror orentaton algorthms can be found n the papers [9, [2, and [1. Recently, Moreno-Noguer et al, [8, proposed an effcent PnP algorthm EPnP) that reles on the 3- smplex and a lnear combnaton of multple egenvectors. Results showed that the egenvector combnaton can mprove pose accuracy. The scale factors wthn the egenvector combnaton are computed by a lnearzaton method. Addtonally, projecton error s not consdered when calculatng the scale factors whch can lead to naccuraces. In ths paper, two new exteror orentaton algorthms are proposed and then compared aganst the EPnP and classc Orthogonal Iteraton Algorthm of [7. Due to space lmtatons, some detals are omtted but a more complete exposton can be found n [10. 2. Lnear 3-Smplex The n-smplex s an n-dmensonal analogue of the trangle, and has n+1 vertces; e.g. the 3-smplex s a tetrahedron wth four vertces. Barycentrc coordnates, α, descrbe the locaton of ponts wth respect to the smplex vertces. Suppose we want to descrbe the locaton of 3D ponts on a rgd body, referred to as model ponts. These ponts are assumed to be known n the local reference frame attached to the rgd body, called the model frame. The four vertces of the smplex, C Ps j, j = 1,..., 4, are chosen. Then, the th model pont n the model frame, C Pm, may be wrtten as M Pm = 4 j=1 α j M Ps j, wth the constrant that the sum of each barycentrc coordnate set, 4 j=1 α j, equals +1. In ths context, the 3-smplex may be consdered a generalzaton of the homogeneous transformaton. The utlty of the smplex n an exteror orentaton algorthm s that the relatonshp between vertces, ponts, and barycentrc coordnates s rotaton and translaton nvarant. Thus, f C MH s the homogeneous transformaton from model to camera coordnates, [ C [ Pm M [ = C Pm 1 MH M = C Ps 1 MH α = 1 [ C Ps α 1 1) In other words, the same set of barycentrc coordnates stll descrbes the relatonshp between smplex vertces and ponts even when expressed n a dfferent reference frame. Suppose an mage of an object s avalable and we wsh to recover the pose of the object wth respect to the
camera. Gven a postulated pose, the 3D model ponts expressed n the camera frame can be projected to specfc ponts, u and v, on the mage plane, f the camera s ntrnsc parameters, f u, f v, u 0, and v 0 are known. The mage space error s the dfference between the measured mage ponts and the projected model ponts on the mage plane. The mage space error equatons can be rearranged nto a form whch s lnear n the model ponts expressed n the camera frame, [ e u e = v [ fu 0 u 0 u ) 0 f v v 0 v ) C Pm. 2) Snce the true projecton results n zero error, the null space of ths lnear relatonshp conssts of all ponts n the camera frame that could have created the mage ponts. An alternatve error metrc, object space error, proposed by [7 measures the error n 3D camera space. Smlarly, the object space error equatons can be rearranged nto a lnear form, e x e y e z f11 1) = f21 f12 f22 1) f13 f23 f31 f32 f33 1) C P m, 3) where f are elements of a projecton matrx. The expresson for model ponts n terms of the smplex vertces and barycentrc coordnates are substtuted nto the projecton error equatons above. Notce that the equatons are lnear n the smplex vertces and may therefore be wrtten n matrx form. Let C Ps equal the vector of smplex vertces n the camera frame so that T = M C Ps or [ e x e y e T z = M C Ps. [ e u e v The error equatons for the entre pont set, 1,..., n, are used to construct a large matrx referred to as the measurement matrx, M = [ T M 1... M n, whch s of sze 2 n 12 or 3 n 12 n ether mage space or object space. The null space of the measurement ma- { } trx, V = C Ps R 12 M C Ps = 0, s a vector of smplex vertces that result n zero projecton error. Regardless of whch error space s used, a bass for the measurement matrx null space may be computed by egenvalue decomposton of M T M, where v are the egenvectors and λ are the postve real egenvalues. If nose s present, there wll be no strctly zero egenvalue and the projecton error of each bass vector equals v T M T M v = Mv 2 = λ v. 4) In ths secton, we are only nterested n the egenvector, v 1, wth the smallest egenvalue, λ 1, whch s reshaped back nto 3 4 matrx form, v 1 C P s. A nonzero scalng exsts because the null space s a vector space, and vector spaces are closed under scalar multplcaton and vector addton. In ths context, the fnal scale factor, β F, must be determned n order to estmate model ponts n the camera frame that are approxmately the same sze as the ponts n the model frame. Fortunately, the scale factor may be easly computed, n a least-squares sense, from the sum of squared error about the model pont centrods. The scale factor s the square root of the dstance rato. After the scale factor s recovered and model ponts n the camera frame are estmated by C Pm C = β F P s α, the absolute orentaton concept of [4 or [5 s appled to fnd the relatve C transformaton between the pont sets, M H. Ths approach forces the fnal rotaton to be specal orthogonal SO3). By usng only one egenvector, pose may be determned non-teratvely by relaxng sze and shape constrants of the smplex wthn egenvalue decomposton. The lnear 3-smplex algorthm can be very fast and also very accurate f mage nose s low and many ponts are avalable. The scale factor appled to one egenvector only corrects sze but not shape. Correcton of shape from multple egenvectors s the focus of the next secton. 3. 3-Smplex-SDP The algorthm presented n ths secton s a technque that produces a more accurate result from the same framework, whch uses multple egenvectors and a polynomal semdefnte program SDP). However, t s teratve and nevtably slower than the lnear method. Recall from the last secton that only the egenvector of M T M wth the smallest egenvalue was of nterest, and n a nosy system there wll be no perfectly zero egenvalue. However, wth nose, all of the egenvectors provde some nformaton n decreasng order of sgnfcance as the sze of the egenvalue ncreases. If the frst four egenvectors, v a,..., v d, are used n a lnear combnaton, β a v a + β b v b + β c v c + β d v d ) C P s, 5) four scale factors, β a,..., β d, are requred. From the egenvector combnaton, model ponts n camera frame are estmated by C P m = β F C P s α, and then the absolute orentaton concept s appled to fnd the relatve transformaton. Projecton error of the egenvector combnaton s gven by E = Mβ a v a + β b v b + β c v c + β d v d ) 2 = β 2 a v a + β 2 b λ b v b + β 2 c λ c v c + β 2 dλ d v d. 6) 2
As suggested by [8, the betas are calculated from non-lnear rotaton and translaton nvarant shape constrants that could not be mposed durng egenvalue decomposton. Eucldean dstance between vertex pars s a logcal choce, and a set of four vectces has sx possble combnatons. In a noseless system, dstances n the model and camera frames should be equal, C Psβ) C Ps j β) 2 = M Ps M Ps j 2. 7) The dot product of a vertex trplet s another rotaton and translaton nvarant quadratc constrant, and a set of four vectces has twelve possble combnatons. In a noseless system, the drecton cosne that the dot product represents should be equal n the camera and model frames, C P sβ) C P k s β) ) C P j s β) C P k s β) ) = M Ps M Ps k ) M Ps j M Ps k ) 8). However, wth nose present, a technque s needed to mnmze shape error by comparng dstances and dot products n both frames. In [8 a lnearzaton method was used to calculate the βs whch leads to a suboptmal soluton. In addton, there was no consderaton for project error resultng from the vector combnaton. In what follows, we present a globally convergent algorthm that balances shape and projecton error wthout approxmaton. In partcular, we see to fnd β to mnmze the followng error functon 18 Eβ) = C d l β) M ) 2 d l l=1 [ λb + w ) β 2 b + λc ) β 2 c + λd ) βd 2, 9) where the C d l and M d l defne the polynomal shape constrants and w s a weght that helps restrct the amount of allowable projecton error that results from mprovng shape. As w ncreases, the strength of the projecton error penalty grows. If excessvely large, β a wll always be the only nonzero scale factor found by the SDP, defeatng the purpose of the mnmzaton. If excessvely small, the shape error s mnmzed wth essentally no projecton error penalty. Note that Eβ) s a quartc multvarate polynomal and addtonally a sum of squares. The work of Lasserre, [6, shows how mnmzng a polynomal that s a sum-of-squares can be transformed nto a convex lnear matrx nequalty LMI) problem and then effcently solved n a semdefnte program SDP). Hence, the name 3-Smplex-SDP that was gven to the algorthm. SDP s a broad generalzaton of lnear programmng LP), to the case of symmetrc matrces. A lnear objectve functon s optmzed over lnear equalty constrants wth a semdefnte matrx condton. The LMI mnmzaton problem s wrtten n standard form as mn X E = P T X, such that M 0, 10) where E s the objectve functon and M s the semdefnte matrx of the lnear nequalty constrant. The key feature of SDPs s ther convexty, snce the feasble set defned by the constrants s convex. In ths context, the SDP s used to solve convex LMI relaxatons of multvarate real-valued polynomals. In ths algorthm, the vector P contans the coeffcents of the polynomal Eβ), X s the optmzaton varable that encodes β, and M s the block dagonal semdefnte matrx constructed from three smaller matrces, M 0, M 1, M 2, whch are semdefnte themselves. The constrants of M 0 force the magntudes of the scale factors nto the correct order, β 2 a β 2 b β2 c β 2 d, whch s also ndrectly enforced through the projecton error penalty. M 1 and M 2 are both moment or covarance) matrces that are constructed from multplcaton of monomal vectors from the sum-of-squares decomposton. Unfortunately, the detals are too lengthy to nclude here but can be found n [10 and [6. After the SDP termnates, the βs are recovered from the optmal vector X. Because the sum-of-squares decomposton s not unque, a trval) sgn ambguty exsts that places the model ponts n front or behnd the camera. In Matlab, the SeDuM package, [11, s used to solve the sum-of-squares problem as a LMI. YALMIP, SOSTOOLS, and GloptPoly are nterfaces that can convert the problem nto a form that s compatble wth SeDuM. 4. 3-Smplex-GNA In ths secton, global convergence s compromsed for fast runtme n a new algorthm that also utlzes the 3-smplex parameterzaton and exstng framework. The algorthm requres an ntal guess that could come from numerous sources, ncludng the lnear 3-Smplex algorthm, the 3-Smplex-SDP, or even a recursve estmator n a trackng stuaton such as augmented realty. The measurement matrx can actually be decomposed nto a 12 12 upper trangular matrx. From SVD 3
we obtan M T M = V SU T )USV T ) = V S)SV ) T = ˆM T ˆM. 11) Ths s a very sgnfcant reducton f the number of ponts s large. QR decomposton s perhaps a qucker way to compute the reduced measurement matrx, ˆM. It can be shown, [3, that mnmzng a resdual wth the reduced measurement matrx s the same as mnmzng a resdual wth the full sze matrx, mn C Ps ˆM C Ps 2 = mn C Ps M C Ps 2. 12) Sub-optmalty of the lnear 3-smplex algorthm mght be attrbuted n part to the relaxaton of sze and shape constrants on the smplex vertces. Specfcally, the smplex vertces are gven 12-DOF even though they really only have sx. For example, dstances between all vertex pars should be the same n the model and camera frames. Sx constrants are placed on twelve parameters leavng only 6-DOF. Fortunately, t s easy to enforce these nonlnear constrants n an teratve algorthm by parameterzng the smplex. Instead of choosng an arbtrary smplex, t s logcal to choose a specfc shape, such as a unt length rght-handed orthogonal trad. e.g. M Ps = [ I 3 0. One method of constructng such a trad s by decomposng the model pont set wth SVD. The trad orgn s placed at the centrod, M t s = M Pm and the legs are M made to pont n the prncple drectons, ˆXs = v 1, M Ŷ s = v 2, M Ẑ s = ±v 3, such that detr) = +1. If chosen ths way, another reference frame, {S}, has been effectvely created. More analyss s needed to determne f ths s the optmal selecton of the smplex. If a unt length orthogonal trad s chosen, the fnal pose soluton may be quckly computed by C M H = C S H S M H, where M S H s equvalent to the chosen smplex and C SH s equvalent to the unknown smplex. Four ponts are requred to defne an orthogonal trad whch can move freely n space. One pont defnes the poston and the other three defne orthogonal drecton vectors along the legs. The rotaton and translaton of the homogeneous transform above can be used to construct an orthogonal trad, M Ps = [ M ˆXs + M t s M Ŷ s + M t s M Ẑ s + M t s M t s. 13) Euler angles can be used to parameterze the rotatonal components of the unknown trad, Rα, β, γ) = [ C ˆXs C Ŷ C s Ẑ s, and thus, a sx element vector wll parameterze the entre trad, Θ = [ T α β γ t x t y t z. A standard GNA may be used to mnmze projecton error of fθ) = ˆM C Ps Θ) 2 and wll have good convergence propertes f the ntal guess s n the neghborhood of the global mnma. By usng the reduced measurement matrx, the Jacoban computed each teraton wll always be of sze 12 6, no matter how many ponts are n the full set. Thus, a sgnfcant tme reducton occurs for many ponts over a few teratons. 5. Results Smulatons are used to compare exteror orentaton algorthm performance. Metrcs nclude poston and orentaton error, number of outlers, and runtme. An 800 pxel focal length s used to project ten 3D ponts onto the mage plane, 1024 768, and then Gaussan nose wth ten pxel standard devaton s added to the mage ponts. In each tral, a new pont cloud s randomly generated to le approxmately 5 n front of the camera wth 1 spread. The results of 1000 teratons are dsplayed n the form of a box plot. Instead of plottng the locaton of outlers, the total number s placed above the top whsker non-standard). In the 3-Smplex-SDP secton, only the four egenvector combnaton s dscussed, but of course, two and three vector combnatons are possble too. Fgure 1 shows the pose error versus number of egenvectors. These results show that pose error decreases wth the number of egenvectors used and, perhaps more mportantly, the four egenvector combnaton s consstently better. It s desrable to use addtonal egenvectors wthn the SDP, but there s a performance/complexty trade-off, and t s not recommended to use more than four egenvectors. Fgure 1. Pose Error: Egenvector Combnatons Fgure 2 compares the 3-Smplex-SDP four egenvector) to a modfed EPnP method. The modfcaton to the EPnP of [? s the use of a GNA to further refne the shape ft, although t stll does not consder projecton error. The fgure shows that the pose error of the 3-Smplex-SDP four egenvector) s sgnfcantly less 4
than the EPnP method. The fgure also shows, by the relatve number of outlers, that the convergence of the 3-Smplex-SDP s much better than the EPnP method. However, the performance ncrease comes at the expense of addtonal algorthm complexty when converted to a sum-of-squares, but n many stuatons accuracy s more mportant than runtme. 6. Conclusons Two new exteror orentaton algorthms were ntroduced and compared aganst exstng algorthms. The 3-Smplex-SDP can be useful n stuatons when a good ntal guess s unavalable. It s globally convergent and more accurate than the EPnP method. However, the algorthm s not partcularly fast, although a specal mplementaton of the SDP solver may help mprove runtme. The 3-Smplex-GNA s very fast, but t requres a reasonably good ntal guess and s not globally convergent. 7. Acknowledgments Fgure 2. Group A: Pose Error Fgure 3 shows that the pose error and convergence of the 3-Smplex-GNA and Orthogonal Iteraton Algorthm OIA) are equvalent. However, Fgure 4 shows that the runtme of the 3-Smplex-GNA s much better than the OIA. The data reducton to ˆM and asymptotc effcency of Gauss-Newton result n a very fast algorthm - possbly a lower bound on runtme. Fgure 3. Group B: Pose Error Fgure 4. Group B: Runtme Ths work was supported by the Natonal Scence Foundaton under Grant ECS 01-34132. References [1 A. Ansar and K. Danlds. Lnear pose estmaton from ponts or lnes. IEEE Trans. of Pattern Analyss and Machne Intellgence, 2003. [2 P. Fore. Effcent lnear soluton of exteror orentaton. IEEE Trans. of Pattern Analyss and Machne Intellgence, 2001. [3 R. Hartley. Mnmzng algebrac error n geometrc estmaton problems. IEEE Sxth Internatonal Conference on Computer Vson, 1998. [4 B. Horn. Closed-form soluton of absolute orentaton usng unt quaternons. Journal of the Optcal Socety of Amerca, 1986. [5 B. Horn, H. Hlden, and S. Negahdarour. Closed-form soluton of absolute orentaton usng orthonormal matrces. Journal of the Optcal Socety of Amerca, 1988. [6 J. Lasserre. Global optmzaton wth polynomals and the problem of moments. SIAM Journal of Optmzaton, 2001. [7 C. Lu, G. Hager, and E. Mjolsness. Fast and globally convergent pose estmaton from vdeo mages. IEEE Trans. of Pattern Analyss and Machne Intellgence, 2000. [8 F. Moreno-Noguer, P. Fua, and V. Lepett. Accurate non-teratve on) soluton to the pnp problem. IEEE 11th Internatonal Conference on Computer Vson, 2007. [9 L. Quan and Z. Lan. Lnear n-pont camera pose determnaton. IEEE Trans. of Pattern Analyss and Machne Intellgence, 1999. [10 J. Stenbs. A new vson & nertal pose estmaton system for handheld augmented realty: Desgn, mplementaton, & testng. CSM Masters Thess, 2008. [11 J. Sturm. Usng sedum 1.02, a matlab toolbox for optmzaton over symmetrc cones. Optmzaton Methods and Software, 1999. 5