function [s,R,t,rmse] = similarity_est2d(X,P,reflect)
%SIMILARITY_EST2D  Estimate similarity transformation between point sets
%   [s,R,t,rmse] = similarity_est2d(X,P,reflect) calculates the similarity
%   transformation required to align a measured set of data points P with a
%   model set X.  That is, the parameters are chosen such that for points x
%   in X and p in P, the relationship
%     x = s*R*p + t
%   is true.  The solution is optimal in a least squares sense.  The output
%   rmse calculates the RMS error between the aligned sets of points.
%
%   The basic algorithm assumes that s is positive, so only translation and
%   rotation (no reflection) is permitted.  If reflect is nonzero then
%   reflection is allowed, and the returned value of s may be negative.

disp('Reflect parameter functionality not implemented');

% Checks
N = size(X,2);
if size(X,1)~=size(P,1) | size(X,2)~=size(P,2)
  error('Inputs must be the same size');
end
if size(X,1)~=2
  error('Inputs must have 2 rows');
end

% Estimate translation from means
xm = mean(X,2);  Xz = X - xm*ones(1,N);
pm = mean(P,2);  Pz = P - pm*ones(1,N);

% Find product matrix and extract theta
S = Xz*Pz';
sv = zeros(2,1);
sv(1) = S(1,1) + S(2,2);
sv(2) = S(2,1) - S(1,2);
thest = atan2(2*sv(1)*sv(2), sv(1)^2 - sv(2)^2)/2;
R = [cos(thest) -sin(thest); sin(thest) cos(thest)];

% Calculate scale factor between points
s = [cos(-thest) -sin(-thest)]*sv/sum(sum(Pz.^2));

% Find required translation
t = xm - s*R*pm;

% Calculate RMS error if required
if nargout>3
  rmse = sqrt(mean(mean((Xz - s*R*Pz).^2)));
end
