% 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 = ...
      ((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
    % Improve this stopping criterion...
    finished = 0;

%    title(loop)
%    drawnow
%    pause
  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);
  % 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
