% comments:
%   only part a) and b) are working and for part a) the radial distorition
%   seems to be sitting at zero (although other parameters are modified)
%   for b) noise seems to affect the rotation matrix much more than translation
%   there is code written for part c) but the results are clearly wrong
clear all;
data = load('data.dat');
X   = data(:,1);
Y   = data(:,2);
Z   = data(:,3);
xim = data(:,4);
yim = data(:,5);

% Find parameters from initial data

M = mkm(X,Y,Z,xim,yim);

tv(M,[X Y Z],[xim yim]);
hz(M,[X Y Z],[xim yim]);

% Add noise

X = X + randn(size(X)); Y = Y + randn(size(Y)); Z = Z + randn(size(Z));
xim = round(xim); yim = round(yim);

M = mkm(X,Y,Z,xim,yim);

tv(M,[X Y Z],[xim yim]);
hz(M,[X Y Z],[xim yim]);

% normalizations
UW = eye(4);
UC = eye(3);

W = [X Y Z ones(length(X),1) ];
C = [xim yim ones(length(xim),1)];

% zero mean
UW(:,4) = -mean(W)'; UW(4,4) = 1;
UC(:,3) = -mean(C)'; UC(3,3) = 1;

% unit variance
for i = 1:3
  UW(i,:) = UW(i,:) / std(W(:,i));
end

for i = 1:2
  UC(i,:) = UC(i,:) / std(C(:,i));
end

M = mknorm(W,C, UW,UC);
tv(M,[X Y Z],[xim yim]);
hz(M,[X Y Z],[xim yim]);