clear
hold off
more off

% generating the data
X = [
    0, 1;
    1, 1;
    2, 1;
    3, 1;
    4, 1;
    5, 1;
    6, 1;
    7, 1;
    8, 1;
    9, 1;
    9, 0;
    9, -1;
    9, -2;
    9, -3;
    9, -4;
    9, -5]';

P = X + 0.05*randn(size(X,1), size(X,2));
P(1,:) = P(1,:)+1;
P(2,:) = P(2,:)+1;

theta1 =  10/360*2*pi;
theta2 = 110/360*2*pi;
rot1 = [cos(theta1), -sin(theta1); sin(theta1), cos(theta1)];
rot2 = [cos(theta2), -sin(theta2); sin(theta2), cos(theta2)];

% two sets with known correspondences
P1 = rot1*P;
P2 = rot2*P;

% two sets with unknown correspondences
P3 = P1(:,randperm(size(P,2)));
P4 = P2(:,randperm(size(P,2)));

% choose a set for testing, CHANGE accordingly!
P = P1; 

% example visualization
plot(X(1,:), X(2,:), 'o');
axis("square")
hold on
plot(P(1,:), P(2,:), 'or');


% Now IMPLEMENT ICP
% The functions 'mean' and 'svd' might be helpful :)