findequ.asv

车道检测中弯道的检测

function p = FindEqu(I,J)
JJ = J;
num = numel(J);
parent = zeros(1,num);
for(i=1:num)
    parent(i) = -1;
end;
for(i=1:num-1)
    flag=1;
    j=i+1;
    while(JJ(j)<JJ(i)+4 && flag==1)
%        if(J(j)>-1)
            y=abs(I(j)-I(i));
            if(y<=1)
                 root1 = FindRoot(i,parent);
                 root2 = FindRoot(j,parent);
                 parent = Union(root1,root2,parent);
                 J(i)=-1;
                 J(j)=-1;
            end
%        end
        if(j<=num-1)
            j=j+1;
        else
            flag=0;
        end
    end
end;
% p = parent
num = numel(parent);
s=0;
for(i=1:num)
    if(parent(i)<-1)
        s=s+1;
        A(s,1)=parent(i);
        A(s,2)=i;
        n(s) = 3;
    end
    if(parent(i)>0)
        nn=1;
        while(parent(i)~=A(nn,2))
            nn=nn+1;
        end       
        A(nn,n(nn))=i;
        n(nn) = n(nn)+1;
    end
end
p=A;