IEEE TRANSACTIONS ON PATTERN ANALYSIS AND MACHINE INTELLIGENCE, VOL. PAMI-9, NO. 5, SEPTEMBER 1987 V. Cappellini and A. G. Constantinides, Eds. Amsterdam, The Netherlands: Elsevier, pp. 770-775. [3] J. O'Rourke, "Motion detection using Hough techniques," in Proc. Conf. Pattern Recognition and Image Processing, Dallas, TX, 1981, p. 737. [4] T. M. Silberberg, L. Davis, and D. Harwood, "An iterative Hough procedure for three-dimensional object recognition," Pattern Rec- ognition, vol. 17, no. 6, pp. 621-629, 1984. [5] H. Li, M. A. Lavin, and R. J. LeMaster, "Fast Hough Transform," in Proc. 3rd Workshop Computer Vision: Representation and Con- trol, Bellair, MI, 1985, pp. 75-83. [6] H. Li and M. A. Lavin, "Fast Hough Transform based on Bintree data structure," in Proc. Conf. Computer Vision and Pattern Rec- ognition, Miami Beach, FL, 1986, pp. 640-642. [7] R. Lumia, L. Shapiro, and 0. Zuniga, "A new connected compo- nents algorithm for virtual memory computers," Comput. Graphics Image Processing, vol. 22, pp. 287-300, 1983. [8] A. Bowyer and J. Woodwark, A programmers geometry. London: Butterworth, 1983. [9] M. Cohen and G. T. Toussaint, "On the detection of structures in noisy pictures," Pattern Recognition, vol. 9, pp. 95-98, 1977. [10] T. M. Van Veen and F. C. A. Groen, "Discretization errors in the Hough Transform," Pattern Recognition, vol. 14, pp. 137- 145, 198 1. [11] R. Lumia, "A new three-dimensional connected components algo- rithm," Comput. Vision, Graphics, Image Processing, vol. 23, pp. 207-217, 1983. Least-Squares Fitting of Two 3-D Point Sets K. S. ARUN, T. S. HUANG, AND S. D. BLOSTEIN Abstract-Two point sets { pi } and { p' }; i = 1, 2, 9 , N are re- lated by p' = Rpi + T + Ni, where R is a rotation matrix, T a trans- lation vector, and Ni a noise vector. Given { pi } and { p' }, we present an algorithm for finding the least-squares solution of R and T, which is based on the singular value decomposition (SVD) of a 3 x 3 matrix. This new algorithm is compared to two earlier algorithms with respect to computer time requirements. Index Terms-Computer vision, least-squares, motion estimation, quaternion, singular value decomposition. I. INTRODUCTION In many computer vision applications, notably the estimation of motion parameters of a rigid object using 3-D point correspon- dences [1] and the determination of the relative attitude of a rigid object with respect to a reference [2], we encounter the following mathematical problem. We are given two 3-D point sets { pi }; i - 1, 2, ,N (here, pi and p' are considered as 3 x 1 column matrices) p> = Rpi + T + N, (1) where R is a 3 x 3 rotation matrix, T is a translation vector (3 x 1 column matrix), and Ni a noise vector. (We assume that the ro- tation is around an axis passing through the origin). We want to find R and T to minimize N E2 = il p1i (Rpi + T) 2. (2) Manuscript received July 2, 1986; revised April 9, 1987. Recommended for acceptance by S. W. Zucker. This work was supported by the National Science Foundation under Grant IRI-8605400. The authors are with the Coordinated Science Laboratory, University of Illinois, Urbana, IL 61801. IEEE Log Number 8715809. An iterative algorithm for finding the solution was described in Huang, Blostein, and Margerum [3]; a noniterative algorithm based on quaternions in Faugeras and Hebert [4]. In this correspondence, we describe a new noniterative algorithm which involves the sin- gular value decomposition (SVD) of a 3 x 3 matrix. The computer time requirements of the three algorithms are compared. After the submission of our correspondence, it was brought to our attention that an algorithm similar to ours had been developed independently by Professor B. K. P. Horn, M.I.T., but not pub- lished. II. DECOUPLING TRANSLATION AND ROTATION It was shown in [3] that: If the least-squares solution to (1) is R and T, then { p' ) and { pi - Rp, - T } have the same centroid, i. e., (3) p I= p ,, where Al N 1= - z P; --- Z P;'-i~ N i=1 N " t E " R N i- =R l Ni= A I N P = -L1 Pi. (4) (5) (6) Let A qi =-pi - pP (7) (8) We have 2 = q - Rqi 112. = li Therefore, parts: (9) the original least-squares problems is reduced to two (i) Find R to minimize E2 in (9). (ii) Then, the translation is found by t= p' -Rp. (10) In the next section, we describe an algorithm for (i) which in- volves the SVD of a 3 x 3 matrix. III. AN SVD ALGORITHM FOR FINDING R A. Algorithm Step 1: From {pi }, { p! } calculate p, p'; and then {qi }, { q }. Step 2: Calculate the 3 x 3 matrix N H- E qiq' i=1 where the superscript t denotes matrix transposition. Step 3: Find the SVD of H, H1= UAV'. Step 4: Calculate X = vut. ( 11) (12) (13) Step 5: Calculate, det (x), the determinant of X. If det (x) = +1, thenR = X. If det (x) = -1, the algorithm fails. (This case usually does not occur. See Sections IV and V.) 0162-8828/87/0900-0698$01.00 © 1987 IEEE 698 Authorized licensed use limited to: Princeton University. Downloaded on January 30, 2010 at 19:45 from IEEE Xplore. Restrictions apply.