zBestRotation(X,Y) finds the least squares rotation of points X onto points Y X and Y are n by 3 matrices containing the locations of corresponding points What is returned is the best fit to Y = X*R' R is the 3x3 rotation matrix
0001 % zBestRotation(X,Y) finds the least squares rotation 0002 % of points X onto points Y 0003 % 0004 % X and Y are n by 3 matrices containing the locations of corresponding points 0005 % What is returned is the best fit to Y = X*R' 0006 % R is the 3x3 rotation matrix 0007 0008 % It is assumed that the means have been subtracted from X and Y 0009 0010 function [R] = zBestRotation(A,B) 0011 0012 n = length(A(:,1)); % number of data points 0013 0014 M = zeros(3,3); % initialize the matrix M 0015 0016 for i=1:n, % go through all data points 0017 M = M + A(i,:)' * B(i,:); % "outer product" of two vectors -> matrix 0018 end 0019 0020 [v,d] = eig(M'*M); % eigenvector matrix, eigenvalue matrix 0021 [e,i] = sort(-diag(d)); % sort the eigenvalues in decreasing order 0022 0023 a = v(:,i(1)); % eigenvector with largest eigenvalue 0024 b = v(:,i(2)); 0025 c = v(:,i(3)); 0026 0027 Ma = M*a/sqrt(-e(1)); 0028 Mb = M*b/sqrt(-e(2)); 0029 0030 if det([a b c]) > 0, 0031 g = cross(Ma,Mb); 0032 else 0033 g = -cross(Ma,Mb); 0034 end 0035 0036 R = [a b c]*[Ma Mb g]'; 0037