joinabove.asv

车道检测中弯道的检测

function [array,n1,n2] = JoinAbove(LineStruct,s)
for(i=1:s)
    if(LineStruct(i).flag==1)
        for(j=1:s)
            if(LineStruct(j).flag==1 && i~=j)
                if(LineStruct(i).low.y<LineStruct(j).top.y)
                    a = LineStruct(i);
                    b = LineStruct(j);
                elseif(LineStruct(i).top.y>LineStruct(j).low.y)
                    a=LineStruct(j);
                    b=LineStruct(i);
                else
                    LineStructNull.StyleFlag=0;
                    a=LineStructNull;
                    b=LineStructNull;
                end%a是top,b是low
                aa=a;
                if(a.StyleFlag==1 && b.StyleFlag==1)%若是两条直线
                    k1=LineStruct(i).fn(1);
                    k2=LineStruct(j).fn(1);
                    if(k1*k2>0 && abs(k1-k2)<0.5)
                        d=1000;
                        xx=(b.top.y-a.fn(2))/a.fn(1);
                        d=abs(xx-b.top.x);
                        if(d<3)
                            a = Line2Line(a,b);
                            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    
                if((a.StyleFlag==2 && b.StyleFlag)==1 || (a.StyleFlag==2 && b.StyleFlag==2))%若曲线上，直线下或曲线上,曲线下
                     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);
                     xx1Del = abs(xx1-b.top.x);
                     xx2=(sqrt(a.fn(2)*a.fn(2)-4*a.fn(1)*(a.fn(3)-b.low.y))-a.fn(2))/2/a.fn(1);
                     xx2Del=abs(xx2-b.low.x);
                     if(xx1Del<6 && xx2Del<6)
                         if(xx1<xx2)
                             bPar=b.top;
                         else
                             bPar=b.low;
                         end
                         a = Line2Curve(a,bPar);
                        if(isequal(aa,LineStruct(i))==1)
                            LineStruct(i) = a;
                            LineStruct(j).flag=0;
                        else
                            LineStruct(j) = a;
                            LineStruct(i).flag=0;
                        end                         
                     end
                end
                if(a.StyleFlag==1 && b.StyleFlag==2)%若直线上,曲线下
                    xx1=(sqrt(b.fn(2)*b.fn(2)-4*b.fn(1)*(b.fn(3)-a.top.y))-b.fn(2))/2/b.fn(1);
                    xx1Del = abs(xx1-a.top.x);
                    xx2=(sqrt(b.fn(2)*b.fn(2)-4*b.fn(1)*(b.fn(3)-a.low.y))-b.fn(2))/2/b.fn(1);
                    xx2Del = abs(xx2-a.low.x);
                    if(xx1Del<5 && xx2Del<5)
                        if(xx1<xx2)
                            bPar = a.top;
                        else
                            bPar = a.low;
                        end
                        a = Curve2Line(b,bPar);
                        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

for(iSort=1:s)
    lengthArray(iSort) = LineStruct(iSort).length;
end;
[xs,idx] = sort(lengthArray);
xFirst = LineStruct(idx(s)).low.x;
iSecond=s-1;
if(xFirst<100)
    while(LineStruct(idx(iSecond)).low.x<100)
        iSecond=iSecond-1;
    end
else
    while(LineStruct(idx(iSecond)).low.x>100)
        iSecond=iSecond-1;
    end
end

array=LineStruct;
n1=idx(s);
n2=idx(iSecond);
% 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;