%CS223B Problem Set 2, Homework 4
% Linear Fit for Perspective Camera Model Calibration
% Code overview: Load in data, extract corners, create correspondences,then
% solve for the missing parameters via least squares

%% Load in images
img1 = imread('fig1imgAlabel.jpg');
img2 = imread('fig1imgBlabel.jpg');
figure, imagesc(img1); colormap(gray);
title('Click a point on this image'); axis image;
a = []

%% Take in corner input - make sure to click in order corresponding to A
%% below.

for i = 1:12
[x y] = ginput(1);
a = [a [x;y]]
img1(round(y),round(x)) = 255;
imagesc(img1); colormap(gray);
title('Click a point on this image'); axis image;
end

figure, imagesc(img2); colormap(gray);
title('Click a point on this image'); axis image;
for i = 1:12
[x y] = ginput(1);
a = [a [x;y]]
img1(round(y),round(x)) = 255;
imagesc(img2); colormap(gray);
title('Click a point on this image'); axis image;
end
save a
%% 3D coordinates
A  = [];
A = [ [ 0 0 0]' [80 0 0]' [160 0 0]' [240 0 0]' [320 0 0]' [400 0 0]' ...
[400 80 0]' [400 160 0]' [400 240 0]' [400 320 0]' [400 400 0]' [450 450 0]' ];
A = [ A [ 0 0 150]' [80 0 150]' [160 0 150]' [240 0 150]' [320 0 150]' [400 0 150]' ...
[400 80 150]' [400 160 150]' [400 240 150]' [400 320 150]' [400 400 150]' [450 450 150]' ];
A = [A;ones(1,size(A,2)) ]
aFix = [a; ones(1, size(a,2))]

%Solve for the matrix (least norm)
% P = aFix*A'*inv(A*A')

%Solve using least squares 
P = findHMatrixCalibration(A', a');

%RMS Calculation
aCalculated = P * A
RMS = sqrt(mean(sum((aCalculated - aFix).^2,1)))


% RMS =
% 
%     0.9706



%TO-DO: least squares solution
