Point cloud to point cloud rigid transformations. Minimizing Rigid Registration Errors

Transcription

1 Pont cloud to pont cloud rgd transformatons Russell Taylor Fall Copyrght R. H. Taylor Mnmzng Rgd Regstraton Errors Typcally, gven a set of ponts {a } n one coordnate system and another set of ponts {b } n a second coordnate system Goal s to fnd [R,p] that mnmzes η = where e e e = (R a + p) b Ths s trcky, because of R Fall Copyrght R. H. Taylor 1

2 Pont cloud to pont cloud regstraton a k b k R a k + p = b k Fall Copyrght R. H. Taylor Mnmzng Rgd Regstraton Errors Step 1: Compute N N 1 1 a = a b = b N = 1 N = 1 a" = a a b" = b b Step : Fnd R that mnmzes ( R a" b" ) Step 3: Fnd p p = b R a Step 4: Desred transformaton s F = Frame( R, p) Fall Copyrght R. H. Taylor

3 Pont cloud to pont cloud regstraton a a k b b k R a k + p = b k Fall Copyrght R. H. Taylor Pont cloud to pont cloud regstraton p = b Ra Fall Copyrght R. H. Taylor 3

4 Rotaton Estmaton Fall Copyrght R. H. Taylor Rotaton Estmaton? Fall Copyrght R. H. Taylor 4

5 Rotaton Estmaton Fall Copyrght R. H. Taylor Rotaton Estmaton Fall Copyrght R. H. Taylor 5

6 Pont cloud to pont cloud regstraton Fall Copyrght R. H. Taylor Solvng for R: teraton method Gven {,( "a, b " ),}, want to fnd R = arg mn R"a b " Step 0: Make an ntal guess R 0 Step 1: Gven R k, compute b = R 1" b k Step : Compute ΔR that mnmzes (ΔR "a b ) Step 3: Set R k +1 = R k ΔR Step 4: Iterate Steps 1-3 untl resdual error s suffcently small (or other termnaton condton) Fall Copyrght R. H. Taylor 6

7 Iteratve method: Gettng Intal Guess We want to fnd an approxmate soluton R 0 to R 0 "a b " One way to do ths s as follows. Form matrces A = "a B= b " Solve least-squares problem M 3x3 A 3xN B 3xN T T Note : You may fnd t easer to solve A 3xN M 3x3 Set R 0 = orthogonalze(m 3x3 ). Verfy that R s a rotaton T B 3xN Our problem s now to solve R 0 ΔRA B. I.e., ΔRA R 0 1 B Fall Copyrght R. H. Taylor Iteratve method: Solvng for ΔR Approxmate Δ R as ( I + skew( α)). I.e., Δ R v v + α v for any vector v. Then, our least squares problem becomes ( Δ R a b mn " ) mn ( a" b + α a" ) ΔR α Ths s lnear least squares problem n α. Then com pute ΔR( α) Fall Copyrght R. H. Taylor 7

8 Drect Iteratve approach for Rgd Frame Gven {,( a ", b " ),}, want to fnd F = argmn F a " b " Step 0: Make an ntal guess F 0 Step 1: Gven F k, compute " a k = F k " a Step : Compute ΔF that mnmzes ΔF " a k " b Step 3: Set F k+1 = ΔFF k Step 4: Iterate Steps 1-3 untl resdual error s suffcently small (or other termnaton condton) Fall Copyrght R. H. Taylor Drect Iteratve approach for Rgd Frame To solve for ΔF = argmn ΔF a k b ΔF a k b α a k + ε + a k b α a k + ε b a k sk( a k ) α + ε b a k Solve the least-squares problem " " sk( a k ) I " " α ε Now set ΔF=[ΔR( α), ε] " b a k " Fall Copyrght R. H. Taylor 8

9 Drect Technques to solve for R Method due to K. Arun, et. al., IEEE PAMI, Vol 9, no 5, pp , Sept 1987 Step 1: Compute H = a b,x,x a b,y,x a b,z,x a b,x,y a b,y,y a b,z,y a b,x,z a b,y,z a b,z,z Step : Compute the SVD of H = USV t Step 3: R = VU t Step 4: Verfy Det(R) = 1. If not, then algorthm may fal. Falure s rare, and mostly fxable. The paper has detals Fall Copyrght R. H. Taylor Quarternon Technque to solve for R B.K.P. Horn, Closed form soluton of absolute orentaton usng unt quaternons, J. Opt. Soc. Amerca, A vol. 4, no. 4, pp 69-64, Apr Method descrbed as reported n Besl and McKay, A method for regstraton of 3D shapes, IEEE Trans. on Pattern Analyss and Machne Intellgence, vol. 14, no., February 199. Solves a 4x4 egenvalue problem to fnd a unt quaternon correspondng to the rotaton Ths quaternon may be converted n closed form to get a more conventonal rotaton matrx Fall Copyrght R. H. Taylor 9

10 Dgresson: quaternons Invented by Hamlton n Can be thought of as 4 elements: q = q 0,q 1,q, scalar & vector: q = s + v = s, v Complex number: q = q 0 + q 1 + q j + k Propertes: where = j = k = j k = 1 Lnearty: λq 1 + µ q = λs 1 + µs,λ v 1 + µ v Conjugate: q * = s v = s, v Transform vector: Product: q 1 " q = s 1 s v 1 v,s v 1 + s v1 + v 1 v q " p = q " 0, p " q * Norm: q = s + v v = q 0 + q 1 + q Fall Copyrght R. H. Taylor Dgresson contnued: unt quaternons We can assocate a rotaton by angle θ about an axs n wth the unt quaternon: θ θ Rot( n, θ) cos, sn n Exercse: Demonstrate ths relatonshp. I.e., show θ θ θ θ Rot(( n, θ ) p = cos, sn n 0 "[, p ]" cos, sn n Fall Copyrght R. H. Taylor 10

11 A bt more on quaternons Exercse: show by substtuton that the varous formulatons for quaternons are equvalent A few web references: realnormedalgebra/quaternons/ndex.htm Fall Copyrght R. H. Taylor Rotaton matrx from unt quaternon q = q 0,q 1,q,q 3 ; q = 1 q 0 + q 1 q (q 1 q q 0 ) (q 1 + q 0 q ) R(q) = (q 1 q + q 0 ) q 0 q 1 + q (q q 0 q 1 ) (q 1 q 0 q ) (q + q 0 q 1 ) q 0 q 1 q + q Fall Copyrght R. H. Taylor 11

12 Unt quaternon from rotaton matrx R(q) = r xx r yx r zx r xy r yy r zy r xz r yz r zz ; a 0 = 1+ r xx + r yy + r zz ; a 1 = 1+ r xx r yy r zz a = 1 r xx + r yy r zz ; a 3 = 1 r xx r yy + r zz a 0 = max{a k } a 1 = max{a k } a = max{a k } a 3 = max{a k } q 0 = a 0 q 0 = r yz r zy 4q 1 q 0 = r zx r xz 4q q 0 = r xy r yx 4 q 1 = r r xy yx q 4q 1 = a 1 0 q 1 = r xy + r yx 4q q 1 = r xz + r zx 4 q = r zx r xz 4q 0 q = r + r xy yx q 4q = a 1 q = r yz + r zy 4 = r yz r zy 4q 0 = r xz + r zx 4q 1 = r + r yz zy q 4 = a Fall Copyrght R. H. Taylor Rotaton axs and angle from rotaton matrx Many optons, ncludng drect trgonemetrc soluton. But ths works: [ n,θ] ExtractAxsAngle(R) { } [s, v] ConvertToQuaternon(R) return([ v / v,atan(s / v ) Fall Copyrght R. H. Taylor 1

13 Step 1: Compute H = Quaternon method for R Step : Compute trace(h) G = Δ a b,x,x a b,,x y a b,z,x a b,x, y a b,, y y a b,z, y a b,x,z a b,,z y a b,z,z Δ T H + H T trace(h)i where Δ T = H,3 H 3, H 3,1 H 1,3 H 1, H,1 Step 3: Compute egen value decomposton of G dag(λ)=q T GQ Step 4: The egenvector Q k = q 0,q 1,q, correspondng to the largest egenvalue λ k s a unt quaternon correspondng to the rotaton Fall Copyrght R. H. Taylor Another Quaternon Method for R Let q = s + v be the unt quaternon correspondng to R. Let a and b be vectors wth b = R a then we have the quaternon equaton (s + v)(0 + a)(s v) = 0 + b (s + v)(0 + a) = (0 + b)(s + v) snce (s v)(s + v) = 1+ 0 Expandng the scalar and vector parts gves v a = v b s a + v a = s b + b v Rearrangng... ( b a) v = 0 s( b a) + ( b + a) v = Fall Copyrght R. H. Taylor 13

14 Another Quaternon Method for R Expressng ths as a matrx equaton b a 0 ( ) T ( b a ) sk ( b + a ) s v = If we now express the quaternon q as a 4-vector q = s, v T, we can express the rotaton problem as the constraned lnear system M( a, b) q = 0 4 q = Fall Copyrght R. H. Taylor Fall Copyrght R. H. Taylor Another Quaternon Method for R In general, we have many observatons, and we want to solve the problem n a least squares sense: where mn M q subject to q = 1 M = M( a 1, b 1 ) " M( a n, b n ) and n s the number of observatons Takng the sngular value decomposton of M=UΣV T ths to the easer problem mn UΣV T q X reduces ( ) = Σ y subject to y = V T q = q = 1 = U Σ y 14

15 Ths problem s just Another Quaternon Method for R σ mn Σ y 0 σ = σ σ 4 y subject to y = 1 where σ are the sngular values. Recall that SVD routnes typcally return the σ 0 and sorted n decreasng magntude. So σ 4 s the smallest sngular value and the value of y wth y = 1 that mnmzes Σ y s y= T 0,0,0,1. The correspondng value of q s gven by q = V y = V 4. Where V 4 s the 4th column of V Fall Copyrght R. H. Taylor Non-reflectve spatal smlarty (rgd+scale) a k b k σr a k + p = b k Fall Copyrght R. H. Taylor 15

16 Non-reflectve spatal smlarty Step 1: Compute a = 1 N a N b = 1 N =1 N b =1 "a = a a " b = b b Step : Estmate scale σ = "b "a Step 3: Fnd R that mnmzes (R σ"a b " ) ( ) Step 4: Fnd p p = b R a Step 5: Desred transformaton s F = SmlartyFrame(σ,R, p) Fall Copyrght R. H. Taylor 16