%load kostky2.cor -mat

%a = CORR_EXCHANGE.d.corr;

%u = [a(:,1,1) a(:,1,2) ones(12,1)]; %prvni obraz x,y - j, i

%uc = [a(:,2,1) a(:,2,2) ones(12,1)]; %druhy obraz x,y - j, i
u = [213.7757 248.2053 1

388.8416 183.7096 1

389.8417 114.4060 1

469.6757 84.6479 1

613.2651 348.1799 1

527.7550 384.6424 1

524.7146 440.0959 1

261.8330 368.9630 1]

uc = [135.7548 327.4878 1

245.4035 194.7429 1

241.2867 124.8000 1

288.9964 68.2094 1

540.4365 240.0000 1

499.6123 303.9771 1

495.2898 358.2171 1

228.0285 418.5920 1]

count = 8;

%**************** zobrazeni naklikanych bodu ***********************

figure;

im1=(imread('k1.jpg'));

imshow(im1);

axis([-2000 2000 -2000 2000]);

hold;

plot(u(:,1),u(:,2),'*g');

hold off;

figure;

im2=(imread('k2.jpg'));

imshow(im2);

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

hold;

plot(uc(:,1),uc(:,2),'*g');

hold off;

count = size(u,1);
%*************** Epipolarni geometrie ********************

%vypocet matice A
A=[u(:,1).*uc(:,1), u(:,1).*uc(:,2), u(:,1), u(:,2).*uc(:,1), u(:,2).*uc(:,2), u(:,2), uc(:,1), uc(:,2), ones(size(u,1),1)];

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

A2 = U*S*V';

% vypocet fundamentalni matice Q - nulovy prostor matice A

q = null(A2);

Q = reshape(q, [3 3])';

[ux,s,v] = svd(Q);

s(3,3) = 0;

%vypocet epipolu

Qsvd = ux*s*v';

u1=[u(:,2) u(:,1) ones(size(u,1),1)];

uc1=[uc(:,2) uc(:,1) ones(size(uc,1),1)];

%vypocet epipolar z fundamentalni matice
lambda1 = []; %v prvnim obrazku

lambda2 = []; %v druhem obrazku

for i = 1:count

lambda1 = [lambda1; (Q*uc(i,:)')'];

lambda2 = [lambda2; u(i,:)*Q];

end;

%vypocet a zobrazeni epipolar
b1 = [];

b2 = [];

k = 5000;

for i = 1 : count

b1 = [b1 [k; -k]]; %x1 x2

b2 = [b2 [-(lambda1(i,3)+k*lambda1(i,1))/lambda1(i,2); -(lambda1(i,3)-k*lambda1(i,1))/lambda1(i,2)]]; %y1 y2

end

figure(1);

hold on;

axis([0 2600 0 700]);

axis on;

plot(b1, b2);

e1 = null(Qsvd');

e1 = e1/e1(3,1);

plot(e1(1,1),e1(2,1),'k*');

b1 = [];

b2 = [];

k = 5000;

for i = 1 : count

b1 = [b1 [k; -k]]; %x1 x2

b2 = [b2 [(-lambda2(i,3)-k*lambda2(i,1))/lambda2(i,2); (-lambda2(i,3)+k*lambda2(i,1))/lambda2(i,2)]]; %y1 y2

end

figure(2);

hold on;

axis([0 1600 -100 600]);

axis on;

plot(b1, b2);

e2 = null(Qsvd);

e2 = e2/e2(3,1);
%zobrazeni epipolu

plot(e2(1,1),e2(2,1),'k*');

%******************* Rekonstrukce sceny pomoci kalibrovanych kamer ************
%nacteni zadaneho souboru
load pointcal_mat;

A1 = P1(1:3, 1:3);

b1 = P1(:, 4);

A2 = P2(1:3, 1:3);

b2 = P2(:, 4);

A11 = inv(A1);

A22 = inv(A2);

u1=[u1(:,2) u1(:,1) u1(:,3)];

uc1=[uc1(:,2) uc1(:,1) uc1(:,3)];

X1 = [];

X2 = [];

for i = 1:count

Au1 = A11 * u1(i,:)';

Ab1 = A11 * b1;

Au2 = A22 * uc1(i,:)';

Ab2 = A22 * b2;

LEFT = [ Au1'*Au1, -Au2'*Au1; Au1'*Au2, -Au2'*Au2 ];

RIGHT = [ (Ab1'-Ab2')*Au1; (Ab1'-Ab2')*Au2 ];

alphas = inv(LEFT) * RIGHT;

X1 = [ X1; [alphas(1) * Au1 - Ab1; 1]' ];

X2 = [ X2; [alphas(2) * Au2 - Ab2; 1]' ];

end;

%presny bod jako prumer obou prumetu

X = (X1 + X2) / 2;

figure(3);

plot3(X(:,1), X(:,2), X(:,3), '*');

grid on;

axis equal;

%normalizace na 3.slozku == 1

newPoints1 = ( (P1*X') ./ ([1;1;1]*[P1(3,:) * X']) )';
newPoints2 = ( (P2*X') ./ ([1;1;1]*[P2(3,:) * X']) )';

%zobrazeni pretransformovanych bodu

%pause;

figure(1);

hold on;

plot(newPoints1(:,1),newPoints1(:,2),'y*'); % zobrazeni bodu

figure(2);

hold on;

plot(newPoints2(:,1),newPoints2(:,2),'y*'); % zobrazeni bodu