%针对单传感器目标跟踪的霍夫变换
%输入参数:I--当前帧图像,oldX--上一帧图像
%输出:两帧之间的RHO_THETA矩阵
function RHO_THETA = myhough(I,oldX)
RHO_THETA = zeros(411,91);
[y, x] = find(I>240);
[oldy, oldx] = find(oldX>240);
for j=1:size(x,1)
for i=1:size(oldy,1)
Dy = y(j)- oldy(i);
Dx = x(j)- oldx(i);
if (Dy>0) & (Dx==0)
theta = 90;
elseif (Dy<0) & (Dx==0)
theta = -90;
else
theta = atand(Dy./Dx);
end
theta = theta+90;
r = x(j)*cosd(theta) + y(j)*sind(theta);
oldr = oldx(i)*cosd(theta) + oldy(i)*sind(theta);
rho = (r+oldr)./2;
rho_fix = fix(rho);%向零取整
if( abs(theta -86) < 1 )
disp([x(j), y(j), oldx(i), oldy(i), rho, theta]);
end
k = fix((rho + 410)./2); %rho_fix
if k<0
k_1 = k + 1;
else
k_1 = k+2;
k=k+1;
end
theta = round(theta./2)+1;
RHO_THETA(k,theta) = RHO_THETA(k,theta)+(k_1-206)*2 -rho;
RHO_THETA(k_1,theta) = RHO_THETA(k,theta)+rho-(k-206)*2;
end
end
return