% wczytanie obrazu znieksztaĹ?conego
%colorInputImage = imread('20190314_103859.jpg');
%colorInputImage = imresize(colorInputImage, 0.25);
%imwrite(colorInputImage, 'test.bmp');
colorInputImage = imread('Do.jpg');
imshow(colorInputImage);

% konwersja do obrazu monochromatycznego
grayInputImage = rgb2gray(colorInputImage);
[height, width] = size(grayInputImage);

% punkty referencyjne w obrazie znieksztaĹ?conym
Ap = [979; 854];
Bp = [2095; 935];
Cp = [2016; 3485];
Dp = [288; 3163];

% punkty referencyjne w obrazie po korekcie
margin = 50;
A = [1+margin; 1+margin];
B = [width-margin; 1+margin];
C = [width-margin; height-margin];
D = [1+margin; height-margin];

% wyznaczenie macierzy opisujÄ?cej znieksztaĹ?cenie
W=[
A(1), A(2), 1, 0, 0, 0, -A(1)*Ap(1), -A(2)*Ap(1);
0, 0, 0, A(1), A(2), 1, -A(1)*Ap(2), -A(2)*Ap(2);
B(1), B(2), 1, 0, 0, 0, -B(1)*Bp(1), -B(2)*Bp(1);
0, 0, 0, B(1), B(2), 1, -B(1)*Bp(2), -B(2)*Bp(2);
C(1), C(2), 1, 0, 0, 0, -C(1)*Cp(1), -C(2)*Cp(1);
0, 0, 0, C(1), C(2), 1, -C(1)*Cp(2), -C(2)*Cp(2);
D(1), D(2), 1, 0, 0, 0, -D(1)*Dp(1), -D(2)*Dp(1);
0, 0, 0, D(1), D(2), 1, -D(1)*Dp(2), -D(2)*Dp(2)
];

V=[
Ap(1);
Ap(2);
Bp(1);
Bp(2);
Cp(1);
Cp(2);
Dp(1);
Dp(2)
];

X = W\V;

M = [X(1), X(2), X(3); X(4), X(5), X(6); X(7), X(8), 1]

% korekta obrazu
colorOutputImage = zeros(height, width, 3);

for y=1:height
  for x=1:width
    P = [x; y; 1];
    Pp = M*P;
    xp = round(Pp(1)/Pp(3));
    yp = round(Pp(2)/Pp(3));
    if (xp >= 1) && (xp <= width) && (yp >=1) && (yp<=height)
      colorOutputImage(y, x, 1) = colorInputImage(yp, xp, 1);
      colorOutputImage(y, x, 2) = colorInputImage(yp, xp, 2);
      colorOutputImage(y, x, 3) = colorInputImage(yp, xp, 3);
    end
  end
  %printf('%d of %d\n', y, height);
  %fflush(stdout);
end

figure
colorOutputImage = colorOutputImage/256;
imshow(colorOutputImage);