equarrayabove.m

车道检测中弯道的检测

function p = EquArrayAbove(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),2);
    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).top.x+LineStruct(i).fn(2)*LineStruct(i).top.x+LineStruct(3);
    LineStruct(i).low.y=LineStruct(i).fn(1)*LineStruct(i).low.x*LineStruct(i).low.x+LineStruct(i).fn(2)*LineStruct(i).low.x+LineStruct(3);
    LineStruct(i).length = (Y(n)-Y(1))*(Y(n)-Y(1))+(X(n)-X(1))*(X(n)-X(1));
    LineStruct(i).flag = 1;
end