equarraybelow.m

车道检测中弯道的检测

function [array,n1,n2]=EquArrayBelow(parent,c,d)
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
for(i=1:s)
    n=-A(i,1);
    for(k=2:n+1)
        X(k-1)=d(A(i,k));
        Y(k-1)=c(A(i,k));
    end
    LineStruct(i).fn=polyfit(X(1:n),Y(1:n),1);
    if(LineStruct(i).fn(1)<0)
        LineStruct(i).top.x=X(n);
        LineStruct(i).low.x=X(1);
    else
        LineStruct(i).top.x=X(1);
        LineStruct(i).low.x=X(n);
    end
    LineStruct(i).top.y=LineStruct(i).fn(1)*LineStruct(i).top.x+LineStruct(i).fn(2);
    LineStruct(i).low.y=LineStruct(i).fn(1)*LineStruct(i).low.x+LineStruct(i).fn(2);
    LineStruct(i).length = (Y(n)-Y(1))*(Y(n)-Y(1))+(X(n)-X(1))*(X(n)-X(1));
    LineStruct(i).flag = 1;
    %LineStruct(i).k = LineStruct(i).fn(1);
end
%array = LineStruct;
for(i=1:s)
    if(LineStruct(i).flag==1)
        for(j=1:s)
            if(LineStruct(j).flag==1 && i~=j)
                k1=LineStruct(i).fn(1);
                k2=LineStruct(j).fn(1);
                if(k1*k2>0)
                    d=1000;
                    if(LineStruct(i).low.y<LineStruct(j).top.y)
                        a = LineStruct(i);
                        aa=a;
                        b = LineStruct(j);
                        xx1=(sqrt(a.fn(2)*a.fn(2)-4*a.fn(1)*(a.fn(3)-b.top.y))-a.fn(2))/2/a.fn(1);
                        xx1=(sqrt(a.fn(2)*a.fn(2)-4*a.fn(1)*(a.fn(3)-b.low.y))-a.fn(2))/2/a.fn(1);
                        d=abs(xx1-b.top.x)+abs(xx2-b.low.x);
                    elseif(LineStruct(i).top.y>LineStruct(j).low.y)
                        a=LineStruct(j);
                        aa=a;
                        b=LineStruct(i);
                        xx1=(sqrt(a.fn(2)*a.fn(2)-4*a.fn(1)*(a.fn(3)-b.top.y))-a.fn(2))/2/a.fn(1);
                        xx1=(sqrt(a.fn(2)*a.fn(2)-4*a.fn(1)*(a.fn(3)-b.low.y))-a.fn(2))/2/a.fn(1);
                        d=abs(xx1-b.top.x)+abs(xx2-b.low.x);
%                     else
%                         a=null;
%                         b=null;
                    end
%                     if(a ~=null && b~=null)
                        
%                     end
                    if(d<40)
                        a.fn(1) = (a.top.y-b.low.y)/(a.top.x-b.low.x);
                        a.fn(2) = a.top.y-a.fn(1)*a.top.x;
                        a.low = b.low;
                        a.length = (a.top.y-b.low.y)*(a.top.y-b.low.y)+(a.top.x-b.low.x)*(a.top.x-b.low.x);
                        if(isequal(aa,LineStruct(i))==1)
                            LineStruct(i) = a;
                            LineStruct(j).flag=0;
                        else
                            LineStruct(j) = a;
                            LineStruct(i).flag=0;
                        end
                    end
                end
            end
        end
    end
end

tempa=0;
tempb=0;
nMax=0;
nMax2=0;
for(i=1:s)
    if(LineStruct(i).length>tempa)
        tempa=LineStruct(i).length;
        nMax=i;
    end
end

for(i=1:s)
    if(LineStruct(i).length>tempb && i~=nMax)
        tempb=LineStruct(i).length;
        nMax2=i;
    end
end

array=LineStruct;
n1=nMax;
n2=nMax2;