%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%General Hough Transform                        %
%                                               %
%Hao Jiang, Oct 2007                            %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

ims = imread('binary.bmp');
ims = rgb2gray(ims);
ims = im2double(ims);
imt = imread('binaryt2.bmp');
imt = rgb2gray(imt);
imt = im2double(imt);

w = size(imt,2);
h = size(imt,1);

% add some noise
nimage = 1-(rand(h,w) < 0.01);
nimage = im2double(nimage);
imt = imt .* nimage;

%find the center of the template
[idy, idx] = find(ims == 0);
p = [idx, idy]; % x and y coordinates
c = round(mean(p));
%compute the voting vector dv for each point 
cmat = repmat(c, size(p,1), 1);
dv = cmat - p;

[idy, idx] = find(imt == 0);
q = [idx, idy]; 

vote_space = zeros(h,w);
for n = 1 : size(q,1)
    pmat = repmat(q(n,:), size(dv,1), 1);
    %vote for the center
    vote_points = pmat + dv;
    id = find(vote_points(:,1) >= 1 & vote_points(:,1) <= w &...
              vote_points(:,2) >= 1 & vote_points(:,2) <= h);
    if ~isempty(id)      
      pid = vote_points(id,2) + (vote_points(id,1) - 1) * h;
      vote_space(pid) = vote_space(pid) + 1;
    end
end    

%smooth the votes
h = fspecial('Gaussian', 10, 5);
vote_space = imfilter(vote_space, h, 'same');

%find the largest peak
[y,x] = find(vote_space == max(vote_space(:)));
imshow(imt);
hold on;
plot(x(1), y(1), 'o');

%find all the local peaks above a threshold
map_smooth = ordfilt2(vote_space, 100, ones(10, 10));
thresh = 0.1 * size(dv,1);
cim = (vote_space==map_smooth)&(map_smooth>thresh);
[rr,cc] = find(cim);

plot(cc, rr, 'ro');
hold off;

figure;
imshow(ims);

figure;
surf(vote_space);
shading interp;