polyfitabove.asv

车道检测中弯道的检测

function [st,stNum]=PolyfitAbove(A,c,d)
s=size(A,1);
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
    if(n<6)
        %像素小于6，用直线拟合
        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).StyleFlag=1;
    else
        %像素大于6，用二次曲线拟合
        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(i).fn(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(i).fn(3);
        yTemp1 = LineStruct(i).fn(1)*X(1)*X(1)+LineStruct(i).fn(2)*X(1)+LineStruct(i).fn(3);
        yTemp2 = LineStruct(i).fn(1)*X(n)*X(n)+LineStruct(i).fn(2)*X(n)+LineStruct(i).fn(3);
        if(yTemp1<yTemp2)
            LineStruct(i).top.x = X(1);
            LineStruct(i).top.y = yTemp1;
            LineStruct(i).low.x = X(n);
            LineStruct(i).low.y = yTemp2;
        else
            LineStruct(i).top.x = X(n);
            LineStruct(i).top.y = yTemp2;
            LineStruct(i).low.x = X(1);
            LineStruct(i).low.y = yTemp1;
        end
        ydelta        
        LineStruct(i).length = ()
        LineStruct(i).flag = 1;
        LineStruct(i).StyleFlag=2;
    end
end
st=LineStruct;
stNum=s;