Orthogonal Procrustes Analysis with anisotropic scaling

function [R,t,FRE,n] = anisotropic_point_register(X,Y,W,threshold) 
% X is the moving set, which is registered to the static set Y. Both are 3 
% by N, where N is the number of fiducials. W is a 3-by-3-by-N array, with 
% each page containing the weighting matrix for the Nth pair of points. 
% THRESHOLD is the size of the change to the moving set above which the 
% iteration continues. 
%
% Creation:
% R. Balachandran and J. M. Fitzpatrick 
% December 2008
if nargin<3,error('X and Y must be given as input');end 
if size(X,1)~=3 && size(Y,1)~=3,error('X and Y must be 3 by N.');end 
N = size(X,2);
if size(Y,2)~=N,error('X and Y must have the same number of points.');end 
% Initial estimate of the transformation assumes anisotropy: 
[R,t,FRE] = point_register(X,Y);
if nargin<3 % if W not given, then give the isotropic solution 
    n = 0; return
end
if nargin<4,threshold = 1e-6;end 
n = 0; % iteration index = 0; 
config_change = Inf; 
Xold = R*X+repmat(t,1,N); 
while (config_change>threshold) 
    n = n+1;
    C = C_maker(Xold,W); 
    e = e_maker(Xold,Y,W); 
    q = C\e;
    if n > 1,q = (q + oldq)/2; end %damps oscillations 
    oldq = q;
    delta_t = [q(4) q(5) q(6)]';
    delta_theta = [1 -q(3) q(2); q(3) 1 -q(1); -q(2) q(1) 1]; 
    [U,Lambda,V] = svd(delta_theta); 
    delta_R = U*V';
    R = delta_R*R; % update rotation 
    t = delta_R*t+delta_t; % update translation 
    Xnew = R*X+repmat(t,1,N); % update moving points 
    config_change = sqrt(sum(sum((Xnew-Xold).^2))/... 
        sum(sum((Xold-repmat(mean(Xold,2),1,N)).^2)));
    Xold = Xnew; 
end
for ii = 1:N
    D = W(:,:,ii)*(Xnew(:,ii)-Y(:,ii)); FRE(ii) = D'*D;
end
FRE = sqrt(mean(FRE)); 

function C = C_maker(X,W)
W1 = W(:,1,:); W2 = W(:,2,:); W3 = W(:,3,:); 
X1 = X(1,:); X2 = X(2,:); X3 = X(3,:);
X = reshape([repmat(X1,3,1);repmat(X2,3,1);repmat(X3,3,1)],size(W)); 
X1 = X(:,1,:); X2 = X(:,2,:); X3 = X(:,3,:); 
C = [-W2.*X3+W3.*X2, +W1.*X3-W3.*X1, -W1.*X2+W2.*X1, W1, W2, W3]; 
C = permute(C,[1,3,2]); 
C = reshape(C,[],6);

function e = e_maker(X,Y,W)
W1 = W(:,1,:); W2 = W(:,2,:); W3 = W(:,3,:); D = Y-X;
D1 = D(1,:); D2 = D(2,:); D3 = D(3,:);
D = reshape([repmat(D1,3,1);repmat(D2,3,1);repmat(D3,3,1)],size(W)); 
D1 = D(:,1,:); D2 = D(:,2,:); D3 = D(:,3,:); 
e = [W1.*D1 + W2.*D2 + W3.*D3]; e = e(:);

function [R, t, FRE] = point_register(X, Y)
% This function performs point-based rigid body registration with equal 
% weighting for all fiducials. It returns a rotation matrix, translation 
% vector and the FRE. 
if nargin < 2
    error('At least two input arguments are required.');
end
[Ncoords Npoints] = size(X);
[Ncoords_Y Npoints_Y] = size(Y); 
if Ncoords ~= 3 | Ncoords_Y ~= 3
    error('Each argument must have exactly three rows.') 
elseif (Ncoords ~= Ncoords_Y) | (Npoints ~= Npoints_Y) 
    error('X and Y must have the same number of columns.'); 
elseif Npoints < 3
    error('X and Y must each have 3 or more columns.'); 
end
Xbar = mean(X,2); % X centroid 
Ybar = mean(Y,2); % Y centroid
Xtilde = X-repmat(Xbar,1,Npoints); % X relative to centroid 
Ytilde = Y-repmat(Ybar,1,Npoints); % Y relative to centroid 
H = Xtilde*Ytilde'; % cross covariance matrix 
[U S V] = svd(H); % U*S*V' = H 
R = V*diag([1, 1, det(V*U)])*U'; 
t = Ybar - R*Xbar;
FREvect = R*X + repmat(t,1,Npoints) - Y; 
FRE = sqrt(mean(sum(FREvect.^2,1)));