%vypocet fundametalni matice (v tomhle souboru je pojmenovana q)
%z vybranych body
load bodiky4;
u1=[b2(:,1)' ; b1(:,1)'; ones(1,size(b1,1))];
u2=[b2(:,2)' ; b1(:,2)'; ones(1,size(b1,1))];
im1=imread('k1.jpg');
im2=imread('k2.jpg');

%vykresleni dvou snimku z vybranymi body (modre)
figure(1);
imshow(im1);
hold on;
plot(u1(2,:),u1(1,:),'*');
figure(2);
imshow(im2)
hold on;
plot(u2(2,:),u2(1,:),'*');


%normalizace souradnic podstatne vylepsuje presnost vypoctu
alfa1=1/std(u1(1,:));
alfa2=1/std(u2(1,:));
L1=[alfa1 0 -alfa1*mean(u1(1,:));0 alfa1 -alfa1*mean(u1(2,:));0 0 1];
L2=[alfa2 0 -alfa2*mean(u2(1,:));0 alfa2 -alfa2*mean(u2(2,:));0 0 1];
u1n=L1*u1;
u2n=L2*u2;
A=[];
for i=1:size(u1,2)
   t=u2n(:,i)*u1n(:,i)';
   A=[A; t(:)']; 
end
[U,S,V] = svd(A);
q=reshape(V(:,9),3,3)';
[U,S,V] = svd(q);
%zaruceni singularity matice q (chceme hod(q)=2)
S(3,3)=0;
%navrat matice q do puvodnic souradnic
q=L1'*U*S*V'*L2;

l1=q*u2;
l2=q'*u1;
%vykresleni epipolar v druhem snimku
drawLin(l2,size(im2,1),size(im2,2));

%vykresleni epipolu
[U,S,V] = svd(q);
eq2= V(:,3);
plot(eq2(2)/eq2(3),eq2(1)/eq2(3),'*');
axis([-1000 6000 -2000 1000]);
axis on;

hold off;

figure(1);
%vykresleni epipolar v prvem snimku
drawLin(l1,size(im1,1),size(im1,2));

eq1=U(:,3);
%vykresleni epipolu
plot(eq1(2)/eq1(3),eq1(1)/eq1(3),'*');
axis([-11000 1000 -3000 1000]);
axis on;

hold off;

