clear all
close all
whos

im1 = double(imread('KMsm03.jpg')); % obrazek vpravo
im2 = double(imread('KMsm04.jpg')); % obrazek vlevo

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

coords = load('inliers.txt'); % souradnice "identickych" bodu
x = coords(:,3:4)'; x(3,:) = 1;
y = coords(:,1:2)'; y(3,:) = 1;

for i = 1: (size(coords,1) - 1)
    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)';

    l  = cross(x(:,i), x(:,i+1));
    lc = cross(y(:,i), y(:,i+1));
    B(i*3 - 2, 1:3) = -l(2)*lc'; B(i*3 - 2, 4:6) = l(1)*lc';
    B(i*3 - 1, 1:3) = -l(3)*lc'; B(i*3 - 1, 7:9) = l(1)*lc';
    B(i*3,     4:6) = -l(3)*lc'; B(i*3,     7:9) = l(2)*lc';
end

B = B(:,[1 4 7 2 5 8 3 6 9]);
[U,S,V] = svd([A;B]);
N = V(:,9);
H = [N(1:3)'; N(4:6)'; N(7:9)'];

clear coords A B l lc N S U V x y;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% slouceni obrazku
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

width = size(im1,2);
height = size(im1,1);

pom1 = H*[1 1 1]';          pom1 = pom1./pom1(3);
pom2 = H*[1 height 1]';      pom2 = pom2./pom2(3);
minx = ceil(max([pom1(1) pom2(1)]));

im = uint8(zeros(height, -minx + width, 3));
Hi = inv(H);

for i = minx:0
    for j = 1:height
        X = Hi*[i; j; 1];
        X = (X./X(3));
% rychlejsi, hranatejsi        
%        im(j, i - minx + 1, :) = im2(round(X(2)), round(X(1)), :);
% moje interpolace
%        m(j, i - minx + 1, :) = im2(floor(X(2)), floor(X(1)), :) + (X(2)-floor(X(2)))*(im2(ceil(X(2)), floor(X(1)), :) - im2(floor(X(2)), floor(X(1)), :)) + (X(1)-floor(X(1)))*(im2(floor(X(2)), ceil(X(1)), :) - im2(floor(X(2)), floor(X(1)), :));
% rozmazana interpolace
        a = (ceil(X(2)) - X(2)) * (ceil(X(1)) - X(1));
        b = (ceil(X(2)) - X(2)) * (X(1) - floor(X(1)));
        c = (X(2) - floor(X(2))) * (ceil(X(1)) - X(1));
        d = (X(2) - floor(X(2))) * (X(1) - floor(X(1)));
        im(j, i - minx + 1, :) = a*im2(floor(X(2)), floor(X(1)), :) + b*im2(floor(X(2)), ceil(X(1)), :) + c*im2(ceil(X(2)), floor(X(1)), :) + d*im2(ceil(X(2)), ceil(X(1)), :);
    end
end    
im(:,(-minx + 1):end,:) = im1;

clear pom1 pom2 i j width height X a b c d;

figure(1);
imshow(im);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% neco jako jasova korekce
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

koef = .96; % experimentalne zjisteno pro tyto dve fotky
koef1 = koef * mean(double(im(:, -minx, 1)) ./ double(im(:, -minx + 1, 1)));
koef2 = koef * mean(double(im(:, -minx, 2)) ./ double(im(:, -minx + 1, 2)));
koef3 = koef * mean(double(im(:, -minx, 3)) ./ double(im(:, -minx + 1, 3)));

im(:,1:-minx,1) = double(im(:,1:-minx,1)) / koef1;
im(:,1:-minx,2) = double(im(:,1:-minx,2)) / koef2;
im(:,1:-minx,3) = double(im(:,1:-minx,3)) / koef3;

clear minx koef koef1 koef2 koef3;

figure(2);
imshow(im);