clear


%%%%%%%%%%%%
% Program extracts small patch from sensor_all.png and 
% uses image convolution to localize this patch in the
% aerial image map.png
%
% Notice that the correct location is found here because we
% chose a nice image patch. Other, more ambiguous image patches
% are not easily localized.
%
%                               Sebastian Thrun, Jan 3, 2010


%%%%%%%%%%%%
% Read the map

aux=imread('map.png');		% read RGB image
map=double(rgb2gray(aux))/256;  % convert to grayscale, range [0,1]
figure(1)
imshow(map);                    % display the map
title('Map')

%%%%%%%%%%%%
% Read the second map, called 'sens'

aux2=imread('sensors_all.png');   % read RGB image
sens=double(rgb2gray(aux2))/256;  % convert to grayscale, range [0,1]
figure(2)
imshow(sens)                   % display patch
title('Second map - defines sensor measurement')


%%%%%%%%%%%%
% Extract a patch and draw in sens map

patchx=300;patchy=500;psize=80;   % patch coordinates
patch=sens(patchx:patchx+psize,patchy:patchy+psize);     % extract patch
figure(3)
imshow(patch)
title('Sensor measurement (patch)')

figure(2)
hold on
plot(line([patchy,patchy+psize],[patchx,patchx]));
plot(line([patchy,patchy+psize],[patchx+psize,patchx+psize]));
plot(line([patchy,patchy],[patchx,patchx+psize]));
plot(line([patchy+psize,patchy+psize],[patchx,patchx+psize]));
hold off



%%%%%%%%%%%%
% Match

[response bestx besty] = match(map, patch, psize);
figure(4)
imshow(response)
title('Response function')


%%%%%%%%%%%%
% draw bounding box into original map

figure(1)
hold on
plot(line([besty-psize/2,besty+psize/2],[bestx-psize/2,bestx-psize/2]));
plot(line([besty-psize/2,besty+psize/2],[bestx+psize/2,bestx+psize/2]));
plot(line([besty-psize/2,besty-psize/2],[bestx-psize/2,bestx+psize/2]));
plot(line([besty+psize/2,besty+psize/2],[bestx-psize/2,bestx+psize/2]));
hold off
title('Map with best match')
