%%%% ロボットビジョン(5T) sample07.m  2014/7/12
clear;
Iin = imread('image06.jpg');
I = double(Iin);

Itmplin = imread('image_template.jpg');
Itmpl = double(Itmplin);
%Itmpl = I(100:131,100:131); %入力画像の一部を切り出してテンプレートにする場合 

[n,m] = size(I);
[nt,mt] = size(Itmpl);

Isad = 100000*ones(n,m); %SAD類似度マップの初期値として適当な値（大きな値）をいれておく
Incc = zeros(n,m); %NCC類似度マップの初期値は0

% SADによる類似度計算

Tstart_sad = tic; % 処理時間計測（開始時刻を取得）
for j = round(nt/2) + 1 : n - round(nt/2) - 1
    for i = round(mt/2) + 1 : m - round(mt/2) - 1
        SAD = 0;
        for l = 1 : nt
            for k = 1 : mt
                %SAD
                SAD = SAD + abs(I(j-round(nt/2)-1+l,i-round(mt/2)-1+k) - Itmpl(l,k)); 
            end;
        end;
        Isad(j,i) = SAD;
    end;
end;
Tm_sad = toc(Tstart_sad); % 処理時間計測（テンプレートマッチングにかかった時間（秒）を取得）

% SADに基づいてマッチング領域の座標を取得
Vmin = Isad(round(n/2),round(m/2));
for j = 1 : n
    for i = 1 : m
        if Isad(j,i) < Vmin;
            Vmin = Isad(j,i); Xm = i; Ym = j;
        end;
    end;
end;

% NCCによる類似度計算
Tstart_ncc = tic; % 処理時間計測（開始時刻を取得）
for j = round(nt/2) + 1 : n - round(nt/2) - 1
    for i = round(mt/2) + 1 : m - round(mt/2) - 1
        NCC1=0; NCC2=0; NCC3=0;
        for l = 1 : nt
            for k = 1 : mt
                %NCC
                NCC1 = NCC1 + I(j-round(nt/2)-1+l,i-round(mt/2)-1+k)^2;
                NCC2 = NCC2 + Itmpl(l,k)^2;
                NCC3 = NCC3 + I(j-round(nt/2)-1+l,i-round(mt/2)-1+k) * Itmpl(l,k);
            end;
        end;
        Incc(j,i) = NCC3 / sqrt(NCC1*NCC2);
    end;
end;
Tm_ncc = toc(Tstart_ncc); % 処理時間計測（テンプレートマッチングにかかった時間（秒）を取得）


%結果の表示に関する処理
%類似度マップから，SAD,NCCを計算していない端の領域を切り取る
Isad = Isad(round(nt/2)+1:m-round(nt/2)-1, round(mt/2)+1:m-round(mt/2)-1);
Incc = Incc(round(nt/2)+1:m-round(nt/2)-1, round(mt/2)+1:m-round(mt/2)-1);
% マッチング領域を白線で囲む
Iout = I;
Iout(Ym-round(nt/2):Ym+round(nt/2),Xm-round(mt/2)) = 255;
Iout(Ym-round(nt/2):Ym+round(nt/2),Xm+round(mt/2)) = 255;
Iout(Ym-round(nt/2),Xm-round(mt/2):Xm+round(mt/2)) = 255;
Iout(Ym+round(nt/2),Xm-round(mt/2):Xm+round(mt/2)) = 255;

figure(1);
subplot(1,2,1); 
imagesc(Itmpl); colormap(gray);
title(['テンプレート画像']); 
subplot(1,2,2); 
imagesc(Iout); colormap(gray);
title(['マッチング結果']); 

figure(2);
subplot(1,2,1); 
mesh(Isad);
title(['SADによる類似度マップ．計算時間',num2str(Tm_sad),'秒．']); 
subplot(1,2,2); 
mesh(Incc);
title(['NCCによる類似度マップ．計算時間',num2str(Tm_ncc),'秒．']); 


%%%% EOF