%vypocitej fundametalni matici
qmat;
%nacteni euklidovskych projektivnich matic
load pointcal_mat;
P1=[P1(2,:);P1(1,:);P1(3,:)];
P2=[P2(2,:);P2(1,:);P2(3,:)];

%rekonstrukce
[X,X1,X2,alfa]=proj(u1,u2,P1,P2);
%vykresleni 3D sceny
figure(3)
hold off;
plot3(X(1,:),X(2,:),X(3,:),'*')
hold on;
plot3(X(1,1:4),X(2,1:4),X(3,1:4),'b');
plot3(X(1,[1,4]),X(2,[1,4]),X(3,[1,4]),'b');
plot3(X(1,5:7),X(2,5:7),X(3,5:7),'b');
plot3(X(1,[1,5]),X(2,[1,5]),X(3,[1,5]),'b');
plot3(X(1,[2,6]),X(2,[2,6]),X(3,[2,6]),'b');
plot3(X(1,[3,7]),X(2,[3,7]),X(3,[3,7]),'b');
plot3(X(1,12:15),X(2,12:15),X(3,12:15),'b');
plot3(X(1,8:9),X(2,8:9),X(3,8:9),'b');
plot3(X(1,10:11),X(2,10:11),X(3,10:11),'b');
plot3(X(1,[6,8]),X(2,[6,8]),X(3,[6,8]),'b');
plot3(X(1,[7,9]),X(2,[7,9]),X(3,[7,9]),'b');
plot3(X(1,[5,10]),X(2,[5,10]),X(3,[5,10]),'b');
plot3(X(1,[6,11]),X(2,[6,11]),X(3,[6,11]),'b');
plot3(X(1,[6,14]),X(2,[6,14]),X(3,[6,14]),'b');
plot3(X(1,[10,12]),X(2,[10,12]),X(3,[10,12]),'b');
plot3(X(1,[11,13]),X(2,[11,13]),X(3,[11,13]),'b');
plot3(X(1,[8,15]),X(2,[8,15]),X(3,[8,15]),'b');

XX1=[X1; ones(1,size(X1,2))];
XX2=[X2; ones(1,size(X2,2))];
XX=[X; ones(1,size(X,2))];

k1=[1;1;1]*alfa(1,:).*u1 - P1*XX1;
k2=[1;1;1]*alfa(2,:).*u2 - P2*XX2;
k3=[1;1;1]*alfa(1,:).*u1 - P1*XX;
k4=[1;1;1]*alfa(2,:).*u2 - P2*XX;

ue1=P1*XX;
ue2=P2*XX;
ue1=ue1./([1;1;1]*ue1(3,:));
ue2=ue2./([1;1;1]*ue2(3,:));

%projektivni rekonstrukce
[PP1 PP2]=projrek(q);
[Xp,Xp1,Xp2,alfap]=proj(u1,u2,PP1,PP2);
XXp=[Xp; ones(1,size(Xp,2))];

up1=PP1*XXp;
up2=PP2*XXp;
up1=up1./([1;1;1]*up1(3,:));
up2=up2./([1;1;1]*up2(3,:));

figure(1);
hold on;
%reprojekce do prvniho snimku, puvodni body byly modre
%zelene jsou z euklidovske rekonstrukce
plot(ue1(2,:),ue1(1,:),'g*');
%cervene jsou z projektivni rekonstrukce
plot(up1(2,:),up1(1,:),'r*');
hold off;

figure(2);
hold on;
%reprojekce do druheho snimku, puvodni body byly modre
%zelene jsou z euklidovske rekonstrukce
plot(ue2(2,:),ue2(1,:),'g*');
%cervene jsou z projektivni rekonstrukce
plot(up2(2,:),up2(1,:),'r*');
hold off;

%vypocet H transformace od skutecnych souradnic do projektivnich
Hmat=hmat(XX,XXp);
Hmati=inv(Hmat);


%vykresleni 3D sceny zapoci H matice z projektivni rekonstrukce do euklid.
XXh=Hmati*XXp;
XXh=XXh./([1;1;1;1]*XXh(4,:));
figure(3);
hold on;
plot3(XXh(1,:),XXh(2,:),XXh(3,:),'g*')

plot3(XXh(1,1:4),XXh(2,1:4),XXh(3,1:4),'g');
plot3(XXh(1,[1,4]),XXh(2,[1,4]),XXh(3,[1,4]),'g');
plot3(XXh(1,5:7),XXh(2,5:7),XXh(3,5:7),'g');
plot3(XXh(1,[1,5]),XXh(2,[1,5]),XXh(3,[1,5]),'g');
plot3(XXh(1,[2,6]),XXh(2,[2,6]),XXh(3,[2,6]),'g');
plot3(XXh(1,[3,7]),XXh(2,[3,7]),XXh(3,[3,7]),'g');
plot3(XXh(1,12:15),XXh(2,12:15),XXh(3,12:15),'g');
plot3(XXh(1,8:9),XXh(2,8:9),XXh(3,8:9),'g');
plot3(XXh(1,10:11),XXh(2,10:11),XXh(3,10:11),'g');
plot3(XXh(1,[6,8]),XXh(2,[6,8]),XXh(3,[6,8]),'g');
plot3(XXh(1,[7,9]),XXh(2,[7,9]),XXh(3,[7,9]),'g');
plot3(XXh(1,[5,10]),XXh(2,[5,10]),XXh(3,[5,10]),'g');
plot3(XXh(1,[6,11]),XXh(2,[6,11]),XXh(3,[6,11]),'g');
plot3(XXh(1,[6,14]),XXh(2,[6,14]),XXh(3,[6,14]),'g');
plot3(XXh(1,[10,12]),XXh(2,[10,12]),XXh(3,[10,12]),'g');
plot3(XXh(1,[11,13]),XXh(2,[11,13]),XXh(3,[11,13]),'g');
plot3(XXh(1,[8,15]),XXh(2,[8,15]),XXh(3,[8,15]),'g');
for i=1:size(X,2)
   plot3([X(1,i) XXh(1,i)],[X(2,i) XXh(2,i)],[X(3,i) XXh(3,i)],'r');
end


%stredy kamer
PP1h=P1*Hmati;
PP2h=P2*Hmati;

C1=null(P1);
C1=C1/C1(4);

C2=null(P2);
C2=C2/C2(4);
%pro zobrazeni kamer odkomentovat
%plot3(C1(1),C1(2),C1(3),'r*');
%plot3(C2(1),C2(2),C2(3),'y*');

hold off;
axis equal;
grid;

