clear all
close all
whos

%im = imread('jehlan01.jpg');
width = 2272; % sirka obrazku
height = 1704; % vyska obrazku
s = sqrt(width^2 + height^2);
T = [1/s 0 -height/2/s; 0 1/s -width/2/s; 0 0 1]; % pro normalizaci

pN = 17; % pocet bodu
pM = 8;  % pocet kamer
load corrx % homogenni souradnice x

% normalizace x
for i = 1:3:24
    x(i:i+2,:) = T*x(i:i+2,:);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% vypocet F
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
prvni = 1; druhy = 8;
for i = 1:pN
    A(i,:) = [x(3*prvni-2,i)*x(druhy*3-2,i)   x(3*prvni-2,i)*x(druhy*3-1,i)   x(3*prvni-2,i)*x(druhy*3,i) ...
              x(3*prvni-1,i)*x(druhy*3-2,i)   x(3*prvni-1,i)*x(druhy*3-1,i)   x(3*prvni-1,i)*x(druhy*3,i) ...
              x(3*prvni  ,i)*x(druhy*3-2,i)   x(3*prvni  ,i)*x(druhy*3-1,i)   x(3*prvni  ,i)*x(druhy*3,i)];
end

[U,S,V] = svd(A);
N = V(:,9);
F = [N(1:3)'; N(4:6)'; N(7:9)'];

[U,S,V] = svd(F);
S(3,3) = 0;
F = U*S*V';

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% vypocet P1, P2
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
P1 = eye(3); P1(:,4) = 0;
ec = null(F); ec = ec/ec(3);
ex = zeros(3); ex(1,2) = -ec(3); ex(1,3) = ec(2);
               ex(2,1) = ec(3); ex(2,3) = -ec(1);
               ex(3,1) = -ec(2); ex(3,2) = ec(1);
P2 = [ex*F ec];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% vypocet X
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for i = 1:pN
    xx1 = [       0       -x(3*prvni,i)  x(3*prvni-1,i); ...
           x(3*prvni,i)          0       -x(3*prvni-2,i); ...
          -x(3*prvni-1,i) x(3*prvni-2,i)         0];
    xx2 = [       0       -x(3*druhy,i)  x(3*druhy-1,i); ...
           x(3*druhy,i)          0       -x(3*druhy-2,i); ...
          -x(3*druhy-1,i) x(3*druhy-2,i)         0];
    B = [xx1 * P1; xx2 * P2];
    [U,S,V] = svd(B);
    X(:,i) = V(:,4);
end
X = X ./ kron(X(4,:), [1 1 1 1]');

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% vypocet P1-8
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for j = 1:pM
    clear B;
    for i = 1:pN
        B(i*3 - 2, 1:4) = x(3*j-1,i)*X(:,i)'; B(i*3 - 2, 5: 8) = -x(3*j-2,i)*X(:,i)';
        B(i*3 - 1, 1:4) = x(3*j  ,i)*X(:,i)'; B(i*3 - 1, 9:12) = -x(3*j-2,i)*X(:,i)';
        B(i*3    , 5:8) = x(3*j  ,i)*X(:,i)'; B(i*3    , 9:12) = -x(3*j-1,i)*X(:,i)';
    end

    [U,S,V] = svd(B);
    N = V(:,12);
    P{j} = [N(1:4)'; N(5:8)'; N(9:12)'];
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Prepocet P1-8 a X
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for i = 1:pM
    alfa{i} = (P{i}*X) ./ x(i*3-2:i*3,:);
end

alfy = cat(1, alfa{:});
a = alfy(3:3:end,:);
[U,S,V] = svd(kron(a, [1;1;1]) .* x);
S(5:end,5:end) = 0;
PP = U*sqrt(S); PP = PP(:,1:4);
XX = sqrt(S)*V'; XX = XX(1:4,:);
XX = XX ./ kron(XX(4,:), [1 1 1 1]');

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Hledani nevlastniho bodu
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
line_11 = cross(inv(T)*PP(3*prvni-2:3*prvni,:)*XX(:,5),inv(T)*PP(3*prvni-2:3*prvni,:)*XX(:,8));
line_12 = cross(inv(T)*PP(3*prvni-2:3*prvni,:)*XX(:,6),inv(T)*PP(3*prvni-2:3*prvni,:)*XX(:,7));
prus1 = cross(line_11, line_12);
line_21 = cross(inv(T)*PP(3*druhy-2:3*druhy,:)*XX(:,5),inv(T)*PP(3*druhy-2:3*druhy,:)*XX(:,8));
line_22 = cross(inv(T)*PP(3*druhy-2:3*druhy,:)*XX(:,6),inv(T)*PP(3*druhy-2:3*druhy,:)*XX(:,7));
prus2 = cross(line_21, line_22);

xx1 = [   0      -prus1(3)  prus1(2); ...
       prus1(3)     0       -prus1(1); ...
       -prus1(2) prus1(1)       0];
xx2 = [   0      -prus2(3)  prus2(2); ...
       prus2(3)     0      -prus2(1); ...
       -prus2(2) prus2(1)       0];

B = [xx1*PP(3*prvni-2:3*prvni,:); xx2*PP(3*druhy-2:3*druhy,:)];
[U,S,V] = svd(B);
Xinf = V(:,4); Xinf = Xinf/Xinf(4);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Vypocet H
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
XH      = XX(:,13:end);
XH(:,6) = Xinf;
XH = XH ./ kron(XH(4,:), [1 1 1 1]');

YH      = [0, 1, 0, 1]';
YH(:,2) = [1, 1, 0, 1]';
YH(:,3) = [1, 0, 0, 1]';
YH(:,4) = [0, 0, 0, 1]';
YH(:,5) = [0.5, 0.5, sqrt(0.5), 1]';
YH(:,6) = [0, 0, 1, 0]';

clear A;
for i = 1:size(XH,2)
    A(i*6-5,1:4)   = YH(2,i)*XH(:,i)';  A(i*6-5,5:8)   = -YH(1,i)*XH(:,i)';
    A(i*6-4,1:4)   = YH(3,i)*XH(:,i)';  A(i*6-4,9:12)  = -YH(1,i)*XH(:,i)';
    A(i*6-3,1:4)   = YH(4,i)*XH(:,i)';  A(i*6-3,13:16) = -YH(1,i)*XH(:,i)';
    A(i*6-2,5:8)   = YH(3,i)*XH(:,i)';  A(i*6-2,9:12)  = -YH(2,i)*XH(:,i)';
    A(i*6-1,5:8)   = YH(4,i)*XH(:,i)';  A(i*6-1,13:16) = -YH(2,i)*XH(:,i)';
    A(i*6  ,9:12)  = YH(4,i)*XH(:,i)';  A(i*6  ,13:16) = -YH(3,i)*XH(:,i)';
end
[U,S,V] = svd(A);
N = V(:,end);
H = [N(1:4)'; N(5:8)'; N(9:12)'; N(13:16)'];
H = H/H(4,4);

C = H*XH;
C = C ./ kron(C(4,:), [1 1 1 1]');
%[YH(:,1:5);C(:,1:5)]
    
figure(10);
YY = H*XX;
YY = YY ./ kron(YY(4,:), [1 1 1 1]');
plot3(YY(1,:), YY(2,:), YY(3,:), '.');
hold on; grid on; axis equal;
K = YY(:,1:4);
K(:,5) = K(:,1);
plot3(K(1,:), K(2,:), K(3,:), 'r');
hold on; grid on; axis equal;
K = YY(:,5:8);
K(:,5) = K(:,1);
plot3(K(1,:), K(2,:), K(3,:), 'g');
hold on; grid on; axis equal;
K = YY(:,9:12);
K(:,5) = K(:,1);
plot3(K(1,:), K(2,:), K(3,:), 'b');
hold on; grid on; axis equal;
K = YY(:,13:16);
K(:,5) = K(:,1);
plot3(K(1,:), K(2,:), K(3,:), 'c');
hold on; grid on; axis equal;
K = YY(:,13); K(:,2) = YY(:,17);
plot3(K(1,:), K(2,:), K(3,:), 'c');
hold on; grid on; axis equal;
K = YY(:,14); K(:,2) = YY(:,17);
plot3(K(1,:), K(2,:), K(3,:), 'c');
hold on; grid on; axis equal;
K = YY(:,15); K(:,2) = YY(:,17);
plot3(K(1,:), K(2,:), K(3,:), 'c');
hold on; grid on; axis equal;
K = YY(:,16); K(:,2) = YY(:,17);
plot3(K(1,:), K(2,:), K(3,:), 'c');
hold on; grid on; axis equal;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Kresleni
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if (0)
    for j = 1:pM
        figure(j);
        im{j} = imread(sprintf('jehlan%02d.jpg',j));
        imshow(im{j});
        hold on;
        Y{j} = inv(T)*P{j}*X;
        Y{j} = Y{j} ./ kron(Y{j}(3,:), [1 1 1]');
        plot(Y{j}(1,:),Y{j}(2,:),'rx');
        
        Z{j} = inv(T)*PP(3*j-2:3*j,:)*XX;
        Z{j} = Z{j} ./ kron(Z{j}(3,:), [1 1 1]');
        plot(Z{j}(1,:),Z{j}(2,:),'bx');
        axis auto;
    end
    
    figure(prvni);
    plot(prus1(1)/prus1(3), prus1(2)/prus1(3), 'r*');
    Yinf = inv(T)*PP(3*prvni-2:3*prvni,:)*Xinf;
%    plot(Yinf(1)/Yinf(3), Yinf(2)/Yinf(3), 'g*');
    figure(druhy);
    plot(prus2(1)/prus2(3), prus2(2)/prus2(3), 'r*');
    Zinf = inv(T)*PP(3*druhy-2:3*druhy,:)*Xinf;
%    plot(Zinf(1)/Zinf(3), Zinf(2)/Zinf(3), 'g*');
end
