function Q5
% Syntax: Q5
%
% Description: CMSC828D Final Exam Q5
%
% Author: Haiying Liu
%   Date: Dec. 12, 2000
%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

dbstop if error

msg = nargchk(0, 0, nargin);
if (~isempty(msg))
  error(strcat('ERROR:', msg));
end

clear msg;

%======================================================================
%= Read the image pair.

image_1 = imread('yos.02.tif');
image_1 = double(image_1);

image_2 = imread('yos.16.tif');
image_2 = double(image_2);

nImgRow = size(image_1, 1);
nImgCol = size(image_1, 2);

%======================================================================
%= Compute the feature points.

[x_1, y_1, measure_1] = harris(image_1);
nPoints_1             = length(x_1);

[x_2, y_2, measure_2] = harris(image_2);
nPoints_2             = length(x_2);

%======================================================================
%= Select N best feature points that are well distributed
%= in the image.

% Set N.
N = 20;

% Sort the feature points in the first image.
[measure_1, order] = sort(measure_1);
x_1                = x_1(order);
y_1                = y_1(order);

% Select N feature points, each pair of whom are at least D pixels
% away from each other. All feature points are within margin.
D            = 41;
maxDisparity = 30;
margin       = maxDisparity;

bestPoints   = zeros(N, 2);
bestPtIndex  = zeros(N, 1);
nBestPoints  = 0;

ftPtMask     = zeros(size(image_1));

for index = 1:nPoints_1
  ftPt_x = x_1(index);
  
  if ftPt_x > margin & ftPt_x < nImgCol - margin + 1
    ftPt_y = y_1(index);
    if ftPt_y > margin & ftPt_y < nImgRow - margin + 1
      
      wndMask = getWnd(ftPtMask, [ftPt_y, ftPt_x], [D, D]);
      
      if sum(wndMask(:)) == 0;
        nBestPoints = nBestPoints + 1;
        bestPoints(nBestPoints, :) = [ftPt_x, ftPt_y];
        bestPtIndex(nBestPoints)   = order(index);
        ftPtMask(ftPt_y, ftPt_x)   = 1;
      end
      
      if nBestPoints == N
        break;
      end
      
    end
  end
  
end

% Show the image pair with the N best feature points.
figure;

subplot(2, 2, 1);
imshow(image_1, [0, 255]);
colormap(pink(256));
hold on;
plot(x_1, y_1, '+');
title(['# of all feature points: ', num2str(nPoints_1)]);

subplot(2, 2, 2);
imshow(image_2, [0, 255]);
colormap(pink(256));
hold on;
plot(x_2, y_2, '+');
title(['# of all feature points: ', num2str(nPoints_2)]);

%======================================================================
%= Do matching

matchWndSize = [11, 11];
bestMatch = zeros(nBestPoints, 2);

for index = 1:nBestPoints
  ftPt_x    = bestPoints(index, 1);
  ftPt_y    = bestPoints(index, 2);
  intWnd_1  = getWnd(image_1, [ftPt_y, ftPt_x], matchWndSize);
  
  minCost   = Inf;
  
  for scanRow = ftPt_y - maxDisparity:ftPt_y + maxDisparity
    for scanCol = ftPt_x - maxDisparity:ftPt_x + maxDisparity
      intWnd_2 = getWnd(image_2, [scanRow, scanCol], matchWndSize);
      cost = 1 - corr2(intWnd_1, intWnd_2);
      if cost < minCost
        minCost = cost;
        bestMatch(index, :) = [scanCol, scanRow];
      end
    end
  end
  
end

subplot(2, 2, 3);
imshow(image_1, [0, 255]);
colormap(pink(256));
hold on;
quiver(bestPoints(:, 1), bestPoints(:, 2), ...
  bestMatch(:, 1) - bestPoints(:, 1), ...
  bestMatch(:, 2) - bestPoints(:, 2), 0);
title(['# of best feature points: ', ...
    num2str(nBestPoints), ' from ', num2str(nPoints_1)]);

subplot(2, 2, 4);
imshow(image_2, [0, 255]);
colormap(pink(256));
hold on;
plot(bestMatch(:, 1), bestMatch(:, 2), '+');
title('Correspondent points');

print -djpeg Q5;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function wnd = getWnd(data, center, wndSize)
% Syntax: wnd = getWnd(data, center, wndSize)
%
%         data    - data
%         center  - center of the window [row, col]
%         wndSize - size of the window [nRow, nCol]
%
% Description: Get the windowed data specified center and size.
%              The size will be round to the closest odd number
%              larger than or equal to the wndSize.
%
% Author: Haiying Liu
%   Date: Dec 13, 2000
%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

dbstop if error

msg = nargchk(3, 3, nargin);
if (~isempty(msg))
  error(strcat('ERROR:', msg));
end

clear msg;

%===========================================================================

% Initialization.
nRow       = size(data, 1);
nCol       = size(data, 2);

halfRow    = floor(wndSize(1) ./ 2);
halfCol    = floor(wndSize(2) ./ 2);

dataUp     = center(1) - halfRow;
dataBottom = center(1) + halfRow;
dataLeft   = center(2) - halfCol;
dataRight  = center(2) + halfCol;

wnd        = zeros(dataBottom - dataUp + 1, ...
  dataRight - dataLeft + 1);

% Boudary checking
if dataUp <= 0
  wndUp    = -dataUp + 2;
  dataUp   = 1;
else
  wndUp    = 1;
end

if dataLeft <= 0
  wndLeft  = -dataLeft + 2;
  dataLeft = 1;
else
  wndLeft  = 1;
end

if dataBottom > nRow
  wndBottom  = size(wnd, 1) - dataBottom + nRow;
  dataBottom = nRow;
else
  wndBottom  = size(wnd, 1);
end

if dataRight > nCol
  wndRight   = size(wnd, 2) - dataRight + nCol;
  dataRight  = nCol;
else
  wndRight   = size(wnd, 2);
end

% Get a window of data
wnd(wndUp:wndBottom, wndLeft:wndRight) = ...
  data(dataUp:dataBottom, dataLeft:dataRight);
