%Nahrani korespondujicich bodu
%-----------------------------
M = load_corr('koskor.mat')
N = size(M,2);

%Vytvoreni matice pro vypocet F
%-------------------------------
for J=1:N
   H(J,1)=M(1,J)*M(3,J);
   H(J,2)=M(1,J)*M(4,J);
   H(J,3)=M(1,J);
   H(J,4)=M(2,J)*M(3,J);
   H(J,5)=M(2,J)*M(4,J);
   H(J,6)=M(2,J);
   H(J,7)=M(3,J);
   H(J,8)=M(4,J);
   H(J,9)=1;
end

%Vypocet F
%----------
[U,D,V]=svd(H);
FF = V(:,end);
F = (reshape(FF,3,3))';

%Uprava F na hod=2
%-----------------
[U1,D1,V1]=svd(F);
D1(3,3) = 0;
FF = U1*D1*V1'
rank(FF)

%Urceni epipolu
%--------------
E1 = V1(:,end);
E1 = E1./E1(3)
E0 = U1(:,end);
E0 = E0./E0(3)
E0'*F;

K = 1;

%Urceni epipolar
%---------------
U2 = [M(1:2,:);ones(1,N)*K];
U3 = [M(3:4,:);ones(1,N)*K];
L1 = ((U2'*FF)');
L2 = (FF*U3);
L3 = ((U2'*F)');
L4 = (F*U3);

for J=1:N
   L1(:,J) = L1(:,J)./L1(3,J);   
	L2(:,J) = L2(:,J)./L2(3,J);   
	L3(:,J) = L3(:,J)./L3(3,J);   
   L4(:,J) = L4(:,J)./L4(3,J);   
end

%Vykresleni prvniho obrazku s epipolarami
%----------------------------------------
figure(1);
im = imread('k01.jpg');
imshow(im);
hold on;
axis([-1000, 1000, -1000, 1000]);
for J=1:N
	line([-1000, 1000],[ (-L2(3,J)-L2(1,J)*-1000)/L2(2,J), (-L2(3,J)-L2(1,J)*1000)/L2(2,J)],'color','b');
	line([-1000, 1000],[ (-L4(3,J)-L4(1,J)*-1000)/L4(2,J), (-L4(3,J)-L4(1,J)*1000)/L4(2,J)],'color','g');
	plot(M(1,J),M(2,J),'mX');
end
plot(E0(1), E0(2), 'yX');

%Vykresleni druheho obrazku s epipolarami
%----------------------------------------
figure(2);
im = imread('k02.jpg');
imshow(im);
hold on;
axis([-1000, 1000, -1000, 1000]);
for J=1:N
   line([-1000, 1000],[ (-L1(3,J)-L1(1,J)*-1000)/L1(2,J), (-L1(3,J)-L1(1,J)*1000)/L1(2,J)],'color','b' );
   line([-1000, 1000],[ (-L3(3,J)-L3(1,J)*-1000)/L3(2,J), (-L3(3,J)-L3(1,J)*1000)/L3(2,J)],'color','g');
   plot(M(3,J),M(4,J),'mX');
end
plot(E1(1), E1(2), 'yX');



R = [U2(1:3,:);U3(1:3,:)];
[Sr,Dr,Vr] = svd(R);
Dr(5:end,5:end) = 0; 
Pp = (Sr*Dr);
Pp = Pp(:,1:4)
Xp = Vr';
Xp = Xp(1:4,:);
Xp = Xp./(ones(4,1)*Xp(4,:))
P1 = Pp(1:3,:);
P2 = Pp(4:6,:);
Body1 = P1*Xp
Body2 = P2*Xp
Body1 = Body1./(ones(3,1)*Body1(3,:))
figure(1);
plot(Body1(1,:), Body1(2,:),'yO');
Body2 = Body2./(ones(3,1)*Body2(3,:))
figure(2);
plot(Body2(1,:), Body2(2,:),'yO');

% Prumerna chyba na bod v pixelech
Diff1 = U2-Body1;
Chyba1 = sum(sqrt(sum(Diff1.^2,1)),2)/N
Diff2 = U3-Body2;
Chyba2 = sum(sqrt(sum(Diff2.^2,1)),2)/N
