clear all;

% nacteme vysledky
load kostky2.mat;

%pripravime pro extract
CORR = CORR_EXCHANGE;

%extrahujeme
extract;

% q1, q2 - ziskane body
Q1=[q1(:,1) q2(:,1)];
Q2=[q1(:,2) q2(:,2)];

% vektor vybranych bodu
indexy = [1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19];

% soustava pro nalezeni prvku fundamentalni matice
matice = zeros(15,9);
for i=1:15
   matice(i,1) = Q1(indexy(i),1)* Q2(indexy(i),1);
   matice(i,2) = Q1(indexy(i),1)* Q2(indexy(i),2);
   matice(i,3) = Q1(indexy(i),1);
   matice(i,4) = Q1(indexy(i),2)* Q2(indexy(i),1);
   matice(i,5) = Q1(indexy(i),2)* Q2(indexy(i),2);
   matice(i,6) = Q1(indexy(i),2);
   matice(i,7) = Q2(indexy(i),1);
   matice(i,8) = Q2(indexy(i),2);
   matice(i,9) = 1;   
end

[U, S, V] = svd(matice);

% vytvorime fundamentalni matici

F = reshape (V(:, 9), 3, 3)';

[U, S, V] = svd(F);

S(3,3) = 0;

F = U * S * V';


% spocteme epipoly

e1 = U(:, 3) ./ U(3, 3);
e2 = V(:, 3) ./ V(3, 3);

% zobrazime epipoly do obrazku + epipolary

obr1 = imread('k1.jpg', 'jpg');
figure(1);
imshow(obr1);
hold on;
axis([0 2800 0 800]);


plot(e1(1), e1(2), 'g*');
for i=1:15
   plot(Q1(i,1), Q1(i,2), 'r*');
   hold on;
end


for i=1:15
   % vypocet epipolary
   ep1 = F * [Q2(i, 1) Q2(i, 2) 1]';
   nula = [Q1(1, :) 1] * ep1;
   nula = 0;
   x1 = 0;
   x2 = 300;
   
   x3 = 3000;
   
   y1 = (nula-ep1(3) - x1 * ep1(1)) / ep1(2);
   y2 = (nula-ep1(3) - x2 * ep1(1)) / ep1(2);
   
   y3 = (nula-ep1(3) - x3 * ep1(1)) / ep1(2);
   
   line([x1 x3]', [y1 y3]');   
end

obr1 = imread('k2.jpg', 'jpg');
figure(3);
imshow(obr1);
hold on;
axis([0 1900 -200 800]);


plot(e2(1), e2(2), 'g*');
for i=1:15
   plot(Q2(i,1), Q2(i,2), 'r*');
   hold on;
end


for i=1:15
   ep2 = [Q1(i, 1) Q1(i, 2) 1] * F;
   nula = 0;
   x1 = 0;
   x3 = 1900;
   
   y1 = (nula-ep2(3) - x1 * ep2(1)) / ep2(2);
   y2 = (nula-ep2(3) - x2 * ep2(1)) / ep2(2);   
   y3 = (nula-ep2(3) - x3 * ep2(1)) / ep2(2);
   
   line([x1 x3]', [y1 y3]');   
end

% nacteme projekcni matice obou kamer
load pointcal_mat;

% sestaveni matice pro nalezeni souradnic bodu v prostoru
matice2 = zeros(4,4);
body = zeros(15,3);
for i=1:15
   matice2(1, :) = [Q1(i,1) * P1(3, 1) - P1(1, 1)
      				  Q1(i,1) * P1(3, 2) - P1(1, 2)
                    Q1(i,1) * P1(3, 3) - P1(1, 3)
                    Q1(i,1) * P1(3, 4) - P1(1, 4)]';
                 
   matice2(2, :) = [Q1(i,2) * P1(3, 1) - P1(2, 1)
      				  Q1(i,2) * P1(3, 2) - P1(2, 2)
                    Q1(i,2) * P1(3, 3) - P1(2, 3)
                    Q1(i,2) * P1(3, 4) - P1(2, 4)]';
                 
   matice2(3, :) = [Q2(i,1) * P1(3, 1) - P2(1, 1)
      				  Q2(i,1) * P1(3, 2) - P2(1, 2)
                    Q2(i,1) * P1(3, 3) - P2(1, 3)
                    Q2(i,1) * P1(3, 4) - P2(1, 4)]';
                 
   matice2(4, :) = [Q2(i,2) * P1(3, 1) - P2(2, 1)
      				  Q2(i,2) * P1(3, 2) - P2(2, 2)
                    Q2(i,2) * P1(3, 3) - P2(2, 3)
                    Q2(i,2) * P1(3, 4) - P2(2, 4)]';
                 
	[U, S, V] = svd(matice2);
	body(i,:) = V(1:3, 4)' ./  V(4, 4);          
          
end

figure(2);
% zobrazime body

for i=1:15
   	plot3(body(i,1), body(i,2), body(i,3), '*');
   hold on;
grid on;
axis equal;
      end

