M = load_corr('koresp.mat')

N = size(M,2);



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 Fundamentalni matice pomoci korespondenci

[U,D,V]=svd(H);

FF = V(:,end);

F = (reshape(FF,3,3))';



% Uprava matice na hodnost 2

[U1,D1,V1]=svd(F);

D1(3,3) = 0;

FF = U1*D1*V1'



% Vypocet epipolu

E1 = V1(:,end);

E1 = E1./E1(3)

E0 = U1(:,end);

E0 = E0./E0(3)



% Z bodu nactenych pomoci loadcorr udelame matice vektoru (u,v,1)'

U2 = [M(1:2,:);ones(1,N)];

U3 = [M(3:4,:);ones(1,N)];



% Vypocteme epopolary

L1 = ((U2'*FF)');

L2 = (FF*U3);



% Takto vypadaji epopolary pokud neni hodnost matice F=2

L3 = ((U2'*F)');

L4 = (F*U3);





% Zobrazeni prvniho obrazku s naklikanymi body, epopoly a epipolarami

% Modre jsou zobrazeny epopolary pro matici F s upravenou hodnosti na 2

% Zelene epipolary pro neupravenou matici F



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');



% Zobrazeni druheho obrazku s naklikanymi body, epopoly a epipolarami

% Modre jsou zobrazeny epopolary pro matici F s upravenou hodnosti na 2

% Zelene epipolary pro neupravenou matici F



figure(2);

im = imread('k02.jpg');

imshow(im);

hold on;

axis([-1500, 1000, -1500, 1000]);



for J=1:N

	line([-1500, 1000],[ (-L1(3,J)-L1(1,J)*-1500)/L1(2,J), (-L1(3,J)-L1(1,J)*1500)/L1(2,J)],'color','b' );

    line([-1500, 1000],[ (-L3(3,J)-L3(1,J)*-1500)/L3(2,J), (-L3(3,J)-L3(1,J)*1500)/L3(2,J)],'color','g');

    plot(M(3,J),M(4,J),'mX');

end



%plot(E1(1), E1(2), 'yX');







% Vypocet bodu v prostoru a matic kamer pomoci metody faktorizace



% sestaveni matice R z naklikanych korespondenci

R = [U2(1:3,:);U3(1:3,:)];



% projektivni hloubky

%%for J=1:N

   %vect = cross(E0,U2(:,J));

   %vect = vect./sqrt(sum(vect.^2))

%%   Lambda(J) = sum(cross(E0,U2(:,J)).^2,1)/(((FF*U3(:,J))')*cross(E0,U2(:,J)));

%%   R(4:6,J) = R(4:6,J).*Lambda(J);

%%end



% Metoda faktorizace

[Sr,Dr,Vr] = svd(R);



% Hodnost matice R ma byt 4 (pokud by byly body presne naklikane)

Dr(5:end,5:end) = 0; 



% Projektivni matice kamer P

Pp = (Sr*Dr);

Pp = Pp(:,1:4)



% Rekonstruovane body v prostoru

Xp = Vr';

Xp = Xp(1:4,:);

Xp = Xp./(ones(4,1)*Xp(4,:))



% Z matice P vybereme jednotlive projektivni matice kamer

P1 = Pp(1:3,:);    % projektivni matice pro kameru 1

P2 = Pp(4:6,:);    % projektivni matice pro kameru 2



% Abychom mohli spocitat chybu, promitneme rekonstruovane body zpet do 2D

% pomoci vypoctenych projektivnich matic obou kamer

Body1 = P1*Xp;

Body2 = P2*Xp;

Body1 = Body1./(ones(3,1)*Body1(3,:))

Body2 = Body2./(ones(3,1)*Body2(3,:))



figure(1);

plot(Body1(1,:), Body1(2,:),'yO');



figure(2);

plot(Body2(1,:), Body2(2,:),'yO');



% Vypocteme prumernou chybu(v pixelech) na jeden rekonstruovany bod

Diff1 = U2-Body1;

Chyba1 = sum(sqrt(sum(Diff1.^2,1)),2)/N

Diff2 = U3-Body2;

Chyba2 = sum(sqrt(sum(Diff2.^2,1)),2)/N





