houghline.m

hough line extraction with original sensor data

% hough line extraction
clc
clear 

%------data initialization----------------------------------------------
data = load('laserdata_X-Y.txt');
X = data(:,1);
Y = data(:,2);
n = size(X,1);
rho_step = 10;
theta_step = 1;
rho_max = floor(sqrt((max(abs(X)))^2+(max(abs(Y)))^2));   
rho_arr = -rho_max:rho_step:rho_max;
theta_arr = 0:theta_step:180-theta_step;
Accumulator = zeros(length(theta_arr),length(rho_arr));

%-----hough transform---------------------------------------------------
%figure 
%hold on
for i = 1:n
    theta_index = 0;
    for theta = theta_arr*pi/180
        theta_index = theta_index+1;
        rho = X(i)*cos(theta)+Y(i)*sin(theta);
        rho_index = round((rho+rho_max)./rho_step)+1;
        %plot(theta_arr(theta_index),rho_arr(rho_index))
        Accumulator(theta_index,rho_index) = Accumulator(theta_index,rho_index)+1;
    end
end

%------peaks detection--------------------------------------------------
% find first peak(max), set the neighbourhood zero, then repeat 
figure
[th, r]= meshgrid(-rho_max:rho_step:rho_max, 0:theta_step:180-theta_step);
mesh(th,r,Accumulator)
hold on
grid on

threshold = 16;
numpeaks = 3;
nhood = [11 101];
done=false;
Accumulator_new=Accumulator;theta_peaks=[]; rho_peaks=[];
while ~done
    [p,q]=find(Accumulator_new==max(Accumulator_new(:)));
    p=p(1);q=q(1);
    if  Accumulator_new(p,q)>=threshold
        theta_peaks(end+1)=p;rho_peaks(end+1)=q;
       
        p1=p-(nhood(1)-1)/2;p2=p+(nhood(1)-1)/2;
        q1=q-(nhood(2)-1)/2;q2=q+(nhood(2)-1)/2;
        [pp,qq]=ndgrid(p1:p2,q1:q2);
        pp=pp(:);qq=qq(:);
       
        badrho=find((pp<1)|(pp>size(Accumulator,1)));
        pp(badrho)=[];qq(badrho)=[];
        
        theta_too_low=find(qq<1);
        qq(theta_too_low)=size(Accumulator,2)+qq(theta_too_low);
        pp(theta_too_low)=size(Accumulator,1)-pp(theta_too_low)+1;
        theta_too_high=find(qq>size(Accumulator,2));
        qq(theta_too_high)=qq(theta_too_high)-size(Accumulator,2);
        pp(theta_too_high)=size(Accumulator,1)-pp(theta_too_high)+1;
        
        Accumulator_new(sub2ind(size(Accumulator_new),pp,qq))=0;
        done=length(theta_peaks)==numpeaks;
    else 
        done=true;
    end
end
scatter3(((rho_peaks-1)*rho_step-rho_max), (theta_peaks-1), diag(Accumulator(theta_peaks,rho_peaks))','r')

%------draw lines---------------------------------------------------
figure 
hold on
axis([-2000 2000 -100 2500])
x = [-2000:10:2000];
plot(X, Y, 'b.');

Alpha = (theta_peaks-1)*theta_step*pi/180 
Rho = ((rho_peaks-1)*rho_step-rho_max)
Theta = Alpha-pi/2;
b = Rho./cos(Theta);
y1 = x*tan(Theta(1))+b(1);
y2 = x*tan(Theta(2))+b(2);
y3 = x*tan(Theta(3))+b(3);
plot(x, y1,'r',x,y2,'r',x,y3 ,'r');