zBestTransformation(X,Y) finds the least squares translation and 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 = shift + scale*X*R' R is the 3x3 rotation matrix scale is the scalar scale factor shift is the 3x1 translation between centers of mass
0001 % zBestTransformation(X,Y) finds the least squares translation and 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 = shift + scale*X*R' 0006 % R is the 3x3 rotation matrix 0007 % scale is the scalar scale factor 0008 % shift is the 3x1 translation between centers of mass 0009 0010 function [R,scale,shift,sshift] = zBestTransformation(X,Y) 0011 0012 n = length(X(:,1)); % number of data points 0013 0014 mX = mean(X); 0015 mY = mean(Y); 0016 0017 A = X-ones(n,1)*mX; % subtract the mean vector 0018 B = Y-ones(n,1)*mY; % subtract the mean vector 0019 0020 R = zBestRotation(A,B); % find optimal rotation matrix 0021 0022 scale = sqrt(sum(sum(B.*B)) / sum(sum(A.*A))); % the optimal scale 0023 0024 shift = (mY - mX*R')'; % the optimal shift with no scaling 0025 0026 sshift = (mY - scale*mX*R')'; % the optimal shift when rescaling 0027 0028 % Note: there is no need to calculate and return the optimal scale in our 0029 % applications, since we don't want to rescale. There should be two versions 0030 % of this program, one with scaling, one without.