% PROCRUSTES Perform Procrustes analysis on landmark data
%
%  [procdata,theta]=procrustes(data,templ)
%  [procdata,theta,templ]=procrustes(data)
%  The second version estimates a template.
%
%  data(i).marks = 2D landmarks, 2-by-p matrices
%  procdata(i).marks = landmarks aligned to a template, 2-by-p matrices
%  theta = Global transformation parameters,
%          theta(i).transl = translation
%          theta(i).phi    = rotation
%          theta(i).scale  = scale
function [procdata,theta,templ]=procrustes(data,templ)
if (nargin<2)
    templ = [];
end
if isempty(templ)
    estimate_templ = 1;
    templ.marks = data(1).marks;
else
    estimate_templ = 0;
end

n = length(data);
p = size(data(1).marks,2);

theta = procrustes_local(data,templ);
for k=1:n
    procdata(k).marks = (rotmat(theta(k).phi)')*(data(k).marks - theta(k).transl*ones(1,p));
    %procdata(k).marks = ((1/theta(k).scale)*rotmat(theta(k).phi)')*(data(k).marks - theta(k).transl*ones(1,p));
end

if estimate_templ
    loop = 0;
    finished = 0;
    while ~finished & (loop<2)
        loop = loop+1;
        mean_phi = mean([theta(:).phi]);
        while abs(mean_phi)>1e-12
            for k=1:n
                theta(k).phi = theta(k).phi - mean_phi;
                theta(k).phi = mod(theta(k).phi+pi,2*pi)-pi;
            end
            mean_phi = mean([theta(:).phi]);
        end
        templ.marks = zeros(size(templ.marks));
        for k=1:n
            procdata(k).marks = ...
                ((1/theta(k).scale)*rotmat(theta(k).phi)')*...
                (data(k).marks - theta(k).transl*ones(1,p));
            templ.marks = templ.marks + procdata(k).marks/n;
        end
        mean_templ = mean(templ.marks,2);
        templ.marks = templ.marks - mean_templ*ones(1,p);
        templ_size = sqrt(sum(sum(templ.marks.^2))/p);
        templ.marks = templ.marks/templ_size;
        theta = procrustes_local(data,templ);
        for k=1:n
            procdata(k).marks = ((1/theta(k).scale)*rotmat(theta(k).phi)')*(data(k).marks - theta(k).transl*ones(1,p));
        end
        finished = 0;
    end
end


%%%%%%%%
function theta = procrustes_local(data,templ)

n = length(data);
p = size(templ.marks,2);
mean_templ = mean(templ.marks,2);

for k=1:n
    % Translate
    theta(k).transl = mean(data(k).marks,2);
    marks_tr = data(k).marks - theta(k).transl*ones(1,p);
    % Add this to not translate the template
    marks_tr = data(k).marks;
    % Rotate
    ab = [sum(templ.marks(2,:).*marks_tr(1,:) -templ.marks(1,:).*marks_tr(2,:)),sum(templ.marks(1,:).*marks_tr(1,:) + templ.marks(2,:).*marks_tr(2,:))];
    % ab*[cos;sin]=0 :
    theta(k).phi = atan2(ab(1),-ab(2));
    % Max or min?
    if ((-ab(1)*sin(theta(k).phi)+ab(2)*cos(theta(k).phi))<0)
        theta(k).phi = theta(k).phi + pi;
    end
    % phi \in [-pi,pi] :
    theta(k).phi = mod(theta(k).phi+pi,2*pi)-pi;
    R = rotmat(theta(k).phi);
    % Scale
    theta(k).scale = abs(sum(sum(marks_tr.^2))/sum(sum(marks_tr.*(R*templ.marks))));
    % Translate
    theta(k).transl = theta(k).transl - (theta(k).scale*R)*mean_templ;
end



%%%%%%%%
function doplot(templ,procdata)

for k=1:length(procdata)
    plotshape(procdata(k),'-b')
    hold on
end
plotshape(templ,'-r')
axis equal
hold off
