%%%% ロボットビジョン(5T) sample03b.m（画像幾何変換）  2017/6/1
%%%% 射影変換（2組の4つの座標から、射影変換行列を求め、画像を変換する）

clear;
% 画像の読み込み
Iin = imread('image01.bmp'); 
I = double(Iin);

% 画像サイズの取得
m = size(I,1); %行数
n = size(I,2); %列数

%%% 変換前座標（4点の画像座標をそれぞれ，各行の要素として与える）
%%% 【参考】image01.bmpの画像サイズは256*256
a  = [1,1;
      1,256;
      256,256;
      256,1];

%%% 変換後座標
b  = [10,40;
      40,246;
      216,226;
      246,10];

%%% y = Ax（※講義ノート参照）のベクトルy，行列Aを用意
y = ones(8,1);
A = ones(8,8);
x = ones(8,1);

for i = 1 : 4
    y(2*i-1) = b(i,1);
    y(2*i) = b(i,2);
    
    A(2*i-1,1) = a(i,1);
    A(2*i-1,2) = a(i,2);
    A(2*i-1,3) = 1;
    A(2*i-1,4) = 0;
    A(2*i-1,5) = 0;
    A(2*i-1,6) = 0;
    A(2*i-1,7) = -1*a(i,1)*b(i,1);
    A(2*i-1,8) = -1*a(i,2)*b(i,1);
    
    A(2*i,1) = 0;
    A(2*i,2) = 0;
    A(2*i,3) = 0;
    A(2*i,4) = a(i,1);
    A(2*i,5) = a(i,2);
    A(2*i,6) = 1;
    A(2*i,7) = -1*a(i,1)*b(i,2);
    A(2*i,8) = -1*a(i,2)*b(i,2);
end;

%%% Aの逆行列を用いて，xを求める
x = inv(A) * y;

%%% xの要素を並べ替えて，射影変換行列Hを求める
H_ = [x(1),x(2),x(3);
    x(4),x(5),x(6);
    x(7),x(8),1];

%%% 求めた射影変換行列Hを用いて，入力画像Iを変換し，出力画像Ipを求める
H = inv(H_);

for i = 1 : m
    for j = 1 : n
        ii = (H(1,1)*i + H(1,2)*j + H(1,3)) / (H(3,1)*i + H(3,2)*j + H(3,3));
        jj = (H(2,1)*i + H(2,2)*j + H(2,3)) / (H(3,1)*i + H(3,2)*j + H(3,3));
        if ii > m || jj > n || ii < 1 || jj < 1
            Ip(i,j) = 0;
        else
            Ip(i,j) = I(round(ii), round(jj));
        end;
    end;
end;


% 画像の表示
figure(1);
subplot(1,2,1);
imagesc(I); colormap(gray);
subplot(1,2,2);
imagesc(Ip); colormap(gray);

%%%% EOF
