clear all
close all
whos

im1 = (imread('camrot01.jpg')); % obrazek vpravo
im2 = (imread('camrot02.jpg')); % obrazek vlevo
width = size(im1,2);
height = size(im1,1);
s = sqrt(width^2 + height^2);
T = [1/s 0 -height/2/s; 0 1/s -width/2/s; 0 0 1];
Ti = inv(T);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% vypocet matice omega
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

index=1;
for j = 2:6
    load(sprintf('rccorr01-%02d.mat', j)); % souradnice "identickych" bodu
    x = T*u(4:6,:);
    y = T*u(1:3,:);

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % vypocet matice H
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    for i = 1: size(u,2)
        A(i*3 - 2, 1:3) = y(3,i)*x(:,i)'; A(i*3 - 2, 7:9) = -y(1,i)*x(:,i)';
        A(i*3 - 1, 4:6) = y(3,i)*x(:,i)'; A(i*3 - 1, 7:9) = -y(2,i)*x(:,i)';
        A(i*3    , 1:3) = y(2,i)*x(:,i)'; A(i*3    , 4:6) = -y(1,i)*x(:,i)';
    end

    [U,S,V] = svd(A);
    N = V(:,9);
    H = inv( [N(1:3)'; N(4:6)'; N(7:9)'] );
    H = Ti*H*T;

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % slucovani matice B
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    H = H / (abs(det(H))^(1/3));
    B(index  ,:) = [H(1,1)*H(1,1)+H(2,1)*H(2,1)   H(1,1)*H(3,1)+H(3,1)*H(1,1)     H(2,1)*H(3,1)+H(3,1)*H(2,1)     H(3,1)*H(3,1)];
    B(index+1,:) = [H(1,1)*H(1,3)+H(2,1)*H(2,3)   H(1,1)*H(3,3)+H(3,1)*H(1,3)     H(2,1)*H(3,3)+H(3,1)*H(2,3)     H(3,1)*H(3,3)];
    B(index+2,:) = [H(1,2)*H(1,3)+H(2,2)*H(2,3)   H(1,2)*H(3,3)+H(3,2)*H(1,3)     H(2,2)*H(3,3)+H(3,2)*H(2,3)     H(3,2)*H(3,3)];
    B(index+3,:) = [H(1,3)*H(1,3)+H(2,3)*H(2,3)   H(1,3)*H(3,3)+H(3,3)*H(1,3)     H(2,3)*H(3,3)+H(3,3)*H(2,3)     H(3,3)*H(3,3)];
    index = index + 4;
    
    B(index-4:index-1,:) = B(index-4:index-1,:) - eye(4);
    clear A U S V N i j k l x y u;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% vypocet matice B
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[U,S,V] = svd(B);
N = V(:,4);
O = [N(1) 0 N(2); 0 N(1) N(3); 0 0 N(4)];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% vypocet matice K
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
K = inv(chol(O/O(3,3)));
K = K/K(3,3);

clear s U S V N O B H index;