Closed-Form Solution Of Absolute Orientation Using Unit Quaternions

Transcription

1 Closed-Form Solution Of Absolute Orientation Using Unit Berthold K. P. Horn Department of Computer and Information Sciences November 11, 2004

2 Outline 1 Introduction 2 3

3 The Problem Given: two sets of corresponding points in different coordinate systems Task: find the absolute orientation of the two systems (scale, rotation, translation) Previous approaches: Only iterative solutions Only close to least-squares solution Only handle three points Selectively neglect constraints This paper: Closed-form, exact least-squares solution No constraints neglected

4 Quaternion Notation q = q 0 + iq x + jq y + kq z q = (q 0, q x, q y, q z ) T : Basic Properties i 2 = 1 j 2 = 1 k 2 = 1 ij = k jk = i ki = j ji = k kj = i ik = j

5 Products of Quaternion Product as a Matrix Vector Product r 0 r x r y r z r q = r x r 0 r z r y r y r z r 0 r x q = R q q r = r z r y r x r 0 r 0 r x r y r z r x r 0 r z r y r y r z r 0 r x q = R q r z r y r x r 0 Watch Out! The quaternion product is not commutative!

6 Dot Product Dot Product of Two p q = p 0 q 0 + p x q x + p y q y + p z q z q 2 = q q Conjugate and Inverse p = q 0 iq x jq y kq z q q = q0 2 + qx 2 + qy 2 + qz 2 = q q A non-zero quaternion has an inverse q 1 = q q q

7 Useful Properties of Dot Products Representing Vectors Let r = (x, y, z) T R 3, then ( q p) ( q r) = ( q q)( p r) ( p q) r = p ( r q ) r = 0 + ix + iy + iz. Matrices associated with such purely imaginary quaternions are skew symmetric: (go back to matrices) R T = R R T = R

8 Representing s With Representation Using a Unit Quaternion q = cos φ 2 + sin φ 2 (iω x + jω y + kω z ) }{{} unit vector ω The imaginary part gives the direction of the axis of rotation. The angle is encoded into the real part and the magnitude of the imaginary part. Rotating a Vector Note that a vector is represented using an imaginary quaternion! r = q r q r = p r p = p( q r q ) p = p q r( p q)

9 Given: n points, each measured in a left and right coordinate system {r l,i } and {r r,i } Try to find a transformation of the form r r = sr(r l ) + r 0 from the left to the right coordinate system. There will be errors e i = r r,i sr(r l,i ) r 0 Minimize the sum of squares of errors e i 2

10 Introduction Working Relative to the Centroids r l = 1 r l,i n r l,i = r l,i r l r r = 1 n r r,i r r,i = r r,i r r Rewrite the error term: e i = r r,i sr(r l,i ) r 0 where r 0 = r 0 r r + sr( r l ) The sum of squares of errors becomes e i 2 = r r,i sr(r l,i ) r 0 2

11 Introduction Decompose the sum: = r r,i sr(r l,i ) r 0 2 ( ) r r,i sr(r l,i ) 2 2r 0 r r,i sr(r l,i ) +n r 0 2 }{{} independent of translation } {{ } =0 The total error is minimized with r 0 = 0, i.e. r 0 = r r sr( r l ). Remaining error term: e i = r r,i sr(r l,i). Now minimize r r,i sr(r l,i ) 2.

12 Finding the Decompose the sum: = r r,i sr(r l,i ) 2 r r,i 2 2s r r,i R(r l,i ) + s2 r l,i 2 = S r 2sD + s 2 S l Complete the square: Best scale is s = D/S l. (s S l D/ S l ) 2 + (S }{{} r S l D 2 )/S }{{} l =0 independent of scale

13 Finding the s = D/S l = r r,i R(r l,i ) r l,i 2 This scale factor is not symmetric! When going from the right to the left system, we get s 1 s Use a symmetrical expression for the error term instead: e i = 1 s r r,i sr(r l,i ) = e i s

14 Finding the Total error is then 1 r s r,i 2 2 r r,i R(r l,i )+s r l,i 2 = 1 s S r 2D ss l Complete the square (slightly different from Horn p. 632) ( s S l S r / s) 2 + 2( S }{{} l S r D) }{{} =0 independent of scale Best scale is now independent of rotation: s = S r /S l = n / r r,i 2 r l,i 2

15 Introduction Remaining error term to be minimized: (S r S l D 2 )/S l or 2( S l S r D) (asymmetric) (symmetric) I.e., maximize D = r r,i R(r l,i ) Use the quaternion representation: find the unit quaternion q that maximizes D = ( q r l,i q ) r r,i

16 Introduction Using ( p q) r = p ( r q ) from earlier, rewrite the term as ( q r l,i q ) r r,i = ( q r l,i ) ( r r,i q) = = ( R l,i q) (R r,i q) q T R T l,i R r,i q ) ( = q T R T l,i R r,i q = q T ( N i ) q = q T N q

17 Introduction Utilize the 3 3 matrix M := S xx S xy S xz r l,i r T r,i := S yx S yy S yz S zx S zy S zz for a convenient representation of ( Sxx + S yy + S zz S yz S zy S zx S xz S xy S yx ) N = S yz S zy S zx S xz S xx S yy S zz S xy + S yx S xy + S yx S xx + S yy S zz S zx + S xz S yz + S zy S xy S yx S zx + S xz S yz + S zy S xx S yy + S zz Note that N is symmetric, and trace(n) = 0. This contains all information to solve the least-squares problem for rotation!

18 Introduction The unit quaternion that maximizes q T N q is the eigenvector to the most positive eigenvalue of N To find this eigenvalue solve the quartic obtained from Use e.g. Ferrari s method. det(n λi) = 0 For the eigenvalue λ m, the eigenvector e m is found by solving (N λ m I) e m = 0 A lot easier nowadays using SVD...

19 The Algorithm 1 Find the centroids r l, r r, substract from measurements 2 For each pair ( r l = (x l, y l, z l ), r r = (x r, y r, z r) ) compute all possible products x l x r, x l y r,..., z l z r and add up to obtain S xx, S xy,..., S zz 3 Compute elements of N 4 Calculate the coefficients of the quartic and solve quartic 5 Pick the most positive root and obtain corresponding eigenvector. The quaternion representing the rotation is a unit vector in the same direction 6 Compute the scale 7 Compute the translation

20 Special Cases and Extensions If points are coplanar (e.g. only three points given), the calculation simplifies greatly Can also use weighted errors: Minimize weighted sum of errors: w i e i 2 Calculate weighted centroids: r l = n / w i r l,i w i etc. Change scale factor calculation: S r = n w i r r,i 2 etc. Change components of matrix for rotation recovery: S xx = n w ix l,i x r,i

21 MatLab Implementation

22 I presented a closed-form solution to the absolute orientation problem Given a mechanism for SVD or eigenvalue computation, the solution is straightforward Non-Gaussian noise, statistical outliers are not handled well Numerical stability? Project Combine with registration? Use for non-rigid motion? E.g., by doing absolute orientation on local point sets?

23 Appendix References References Berthold K. P. Horn. Closed-form solution of absolute orientation using unit quaternions. Journal of the Optical Society of America A, 4(4): , April Ross J. Micheals, Terrance E. Boult. On the Robustness of Absolute Orientation F. Sansò. An exact solution of the roto-translation problem. Photogrammetria 29: , 1973.