智能车图像处理16-三叉路口的判断

前言

这篇文章主要讲述图像处理中如何判断三叉路口并进行补线操作。

一、函数主体

    void threeroad()
    {
    
    
        int hang;

        int i;

        int end = 0;

        for (hang = 1; hang < 69; hang++)  //去掉杂点
        {
    
    

            if (LCenter[hang] == 0 && LCenter[hang + 1] == 0)
            {
    
    

                end = hang - 1;
                break;
            }
            if (My_Abs(LCenter[hang], LCenter[hang + 1]) > 20 && hang > 5)
            {
    
    

                end = hang - 1;
                break;
            }
        }
        countleft10 = 0;
        countright10 = 0;
        threekuanduEQ = 0;
        secondleft = 0;
        firstleft = 0;
        //三叉先行找一下拐点




        int start=end;
        for (i = end; i < 60; i++)
        {
    
    
            if (Pixels[i][ 93] == 0)
            {
    
    
                start = i;
                break;
            }
        }




        int allwhite = 0;
        int startallright = 0;
        for (i = 45; i > 0; i--)
        {
    
    
            if (Pixels[i + 3][2] == 0 && Pixels[i + 2][ 2] == 0 && Pixels[i + 1][2] == 1 && Pixels[i][ 2] == 1)
            {
    
    
                startallright = i+1;
                break;
            }
        }

        for (i = startallright; i > 0; i--)
        {
    
    
            if (Pixels[i][ 0] == 0)
            {
    
    
                allwhite = 1;
                break;
            }
        }


        int allwhiteright = 0;
        int startallleft = 0;
        for (i = 30; i > 0; i--)
        {
    
    
            if ( Pixels[i + 2][ 185] == 0 && Pixels[i + 1][ 185] == 1 )
            {
    
    
                startallleft = i + 1;
                break;
            }
        }

        for (i = startallleft; i > 0; i--)
        {
    
    
            if (Pixels[i][ 185] == 0)
            {
    
    
                allwhiteright = 1;
                break;
            }
        }

/*
敲黑板!下面是三叉路口的思路，有助于理解代码

首先:明确十字与三叉的区别
大家可以想一下十字和三叉十字路口。十字路口的图像是一条白色直道直通到视角尽头，而三叉的白色直道道整个图像的中央就分叉开来了。
其次:三叉路口辅助判断条件含义:
(1)入口的两个下拐点:三叉和十字的下拐点判断条件是有细微差别的，大家可以具体对比一下。
(2)endmiddle:就是白色直道的最终行数:我有四十行图像，十字的endmiddle会有37、38，而三叉的endmiddle只有不超过34这样出现分叉。
(3)endmiddle改进建议:
这里的取法是找到左右下拐点取中值向上延伸，如果想要改进这个条件的准确度。建议是取左右拐点较小的行数，从0到该行用最小二乘法拟合往下延伸寻找endmiddle。
(辅助条件判断方法在另一篇文章中)
最后:
判断思路:
(1)非环岛状态
(2)最有边线的第0行必须在视野范围内(1-185)。因为在十字转弯的时候，很多情况下左下或者右下是全白的，这一个条件可以有效的排除部分十字。但也意味着想要判断三叉，必须要车身是正的。
(因为斜入十字和三叉特别像，大家估计也发现了)
(3)找到左右下拐点，且左右下拐点行数差必须要在一定范围之内。(车正 基本上左右下拐点差个5行最多了)
(4)endmiddle小于37行(我一共42行，大家自己调整这个值)
(5)
calkuan  宽度数组  在左右下拐点行数较高的那行向上1 2 3 行宽度要大于100.
(因为三叉后面呈现分叉状态,宽度会变大不少，宽度太小就不认为是三叉。)  

注:状态1-6 为一个完整的入三叉到出三叉过程，因为我的车要跑两圈 有状态7-12

*/

        //147 10  25811

        if (bigup != 0&& haveleftku == 0&&threemode ==0&&threerightdownflag == 1 && threeleftdownflag == 1 && calkuan[bigup + 1] > 110 && calkuan[bigup + 2] > 110
            && calkuan[bigup + 3] > 110 && calkuan[bigup] > 100
            && (endmidddle<=37&& endmidddle !=0)&& huandao_memory==0&&huandao_memoryforleft==0
             && turepodaoflag == 0 && (countpodao == 0 || countpodao > 1) && My_Abs(threerightdown[0], threeleftdown[0]) <= 15&&secondmid==0&&firstmid==0&&se_thrmid==0
            )//

            threemode = 1;//进入状态1  找点拉线

        if ((haveleftku == 0 && threemode == 1  && R_black[0]==0&&
             L_black[0]==186)
             && huandao_memory == 0 && huandao_memoryforleft == 0
              && turepodaoflag == 0 && (countpodao == 0 || countpodao > 1)//&&(threerightdownflag==0|| threeleftdownflag==0)
             ) threemode = 2;//进入状态2   右下角拉线

        if (haveleftku == 0 && threemode == 2 &&(R_black[0]!= 0|| L_black[0] != 186) && (threerightdownflag == 0 || threeleftdownflag == 0)
            && huandao_memory == 0 && huandao_memoryforleft == 0
             && turepodaoflag == 0 && (countpodao == 0 || countpodao > 1)
            ) threemode = 3;  //进入状态3 正常跑

        if (bigup != 0 && haveleftku == 0 && threemode == 3 && threerightdownflag == 1 && threeleftdownflag == 1 && calkuan[bigup + 1] > 100 && calkuan[bigup + 2] > 100
            && calkuan[bigup + 3] > 100 && calkuan[bigup] > 100
             && (endmidddle <= 37 && endmidddle != 0)
            && huandao_memory == 0 && huandao_memoryforleft == 0
             && turepodaoflag == 0 && (countpodao == 0 || countpodao > 1) && My_Abs(threerightdown[0], threeleftdown[0]) <= 17 && secondmid == 0 && firstmid == 0 && se_thrmid == 0
            ) threemode = 4;  //进入状态四 同1 找点拉线

        if (haveleftku == 0 && threemode == 4  && R_black[0] == 0 && L_black[0] == 186
            && huandao_memory == 0 && huandao_memoryforleft == 0
             && turepodaoflag == 0 && (countpodao == 0 || countpodao > 1)
             )//&& (threerightdownflag == 0 || threeleftdownflag == 0)
        {
    
    
                    threemode = 5; //进入状态5 同2  右下角拉线
        }
        if (haveleftku == 0 && threemode == 5 && (R_black[0] != 0 || L_black[0] != 186) && (threerightdownflag == 0 || threeleftdownflag == 0)
            && huandao_memory == 0 && huandao_memoryforleft == 0
             && turepodaoflag == 0 && (countpodao == 0 || countpodao > 1)
            )
        {
    
    
            threemode = 6; //进入6  正常跑状态
        }
        if (bigup != 0 && haveleftku == 0 && threemode == 6 && threerightdownflag == 1 && threeleftdownflag == 1 && calkuan[bigup + 1] > 110 && calkuan[bigup + 2] > 110
            && calkuan[bigup + 3] > 110 && calkuan[bigup] > 100
             && (endmidddle <= 37 && endmidddle != 0)
            && huandao_memory == 0 && huandao_memoryforleft == 0
             && turepodaoflag == 0 && (countpodao == 0 || countpodao > 1) && My_Abs(threerightdown[0], threeleftdown[0]) <= 15 && secondmid == 0 && firstmid == 0 && se_thrmid == 0
            ) threemode = 7;//进入状态7  找左下拐点拉线

        if (haveleftku == 0 && threemode == 7&&R_black[0] == 0 &&
             L_black[0] == 186
             && huandao_memory == 0 && huandao_memoryforleft == 0
              && turepodaoflag == 0 && (countpodao == 0 || countpodao > 1)
             )// &&  (threerightdownflag == 0 || threeleftdownflag == 0)
            threemode =8;//进入状态8  直接拉线

        if (haveleftku == 0 && threemode == 8 &&( L_black[0]!=186||R_black[0]!=0) && (threerightdownflag == 0 || threeleftdownflag == 0)
            && huandao_memory == 0 && huandao_memoryforleft == 0
             && turepodaoflag == 0 && (countpodao == 0 || countpodao > 1)
            ) threemode = 9;  //进入状态9  正常跑
      //  threemode = 9;
        if (bigup != 0 && haveleftku == 0 && threemode == 9 && threerightdownflag == 1 && threeleftdownflag == 1 && calkuan[bigup + 1] > 100 && calkuan[bigup + 2] > 100
            && calkuan[bigup + 3] > 100 && calkuan[bigup] > 100
           && (endmidddle <= 37 && endmidddle != 0)
            && huandao_memory == 0 && huandao_memoryforleft == 0
             && turepodaoflag == 0 && (countpodao == 0 || countpodao > 1) && My_Abs(threerightdown[0], threeleftdown[0]) <= 17 && secondmid == 0 && firstmid == 0 && se_thrmid == 0
            )   threemode = 10;//进入状态10  同6 找左下挂点拉线

        if (haveleftku == 0 && threemode == 10  && R_black[0] == 0 &&
             L_black[0] == 186
             && huandao_memory == 0 && huandao_memoryforleft == 0
              && turepodaoflag == 0 && (countpodao == 0 || countpodao > 1)
             )//&& (threerightdownflag == 0 || threeleftdownflag == 0)
        {
    
    
            threemode = 11;//进入状态11  同7 左下角拉线
        }
        if (haveleftku == 0 && threemode == 11 && (L_black[0] != 186 || R_black[0] != 0) && (threerightdownflag == 0 || threeleftdownflag == 0)
            && huandao_memory == 0 && huandao_memoryforleft == 0
             && turepodaoflag == 0 && (countpodao == 0 || countpodao > 1)
            )  //进入状态12  继续跑
        {
    
    
            threemode = 12; //无状态什么都不做继续跑
        }





        threemodeop(threemode);
      //  threemodeop(threemode);
         if (givetime != 1)
         {
    
    
             threeroadextrale = threeroadextra;
             threeroadextrari = threeroadextra;
             givetime = 1;
         }
         if (threeroadextrale != 0 && threemode == 6)
         {
    
    
             threemode = 0;
             threeroadextrale = threeroadextrale - 1;

         }
         if(threeroadextrari != 0&&threemode==12)
         {
    
    
             threemode = 6;
             threeroadextrari = threeroadextrari - 1;

         }

    }

//三岔路口结束

2.辅助函数

//求两数之差绝对值开始
int My_Abs(int a, int b)
{
    
    

            if ((a - b) > 0)
                return ((int)(a - b));
            else return ((int)(b - a));
}
//求两数之差绝对值结束

//三叉路口操作开始
void threemodeop(int modetype)
{
    
    
            int i;
            int j;
            if ((modetype == 1|| modetype ==4)&& threerightdown[1]>=1&& LCenter[end]>=1)  //
            {
    
    
                int endmidddle=0;
                for (i = 0; i < 60; i++)
                {
    
    
                    if (Pixels[i][ 93] == 0)
                    {
    
    
                        endmidddle = i;
                        break;
                    }
                }
                R_black[threerightdown[0] - 1] = (unsigned char)(threerightdown[1] - 1);
                R_black[threerightdown[0]]=(unsigned char)(threerightdown[1]);
                if (endmidddle <= 1) endmidddle = 1;
                    R_black[endmidddle - 1] = 93;
                    R_black[endmidddle] = 94;
                advanced_regression34(2, threerightdown[0] - 1, threerightdown[0], endmidddle - 1, endmidddle);
                R_blackfuzhi(parameterA, parameterB, threerightdown[0] - 1, 50);
                for (i = 0; i < 60; i++)
                {
    
    
                    if (L_black[i] < 130) L_black[i] = 186;
                }
                for (j = 0; j < 70; j++)
                {
    
    
                    LCenter[j] = (unsigned char)((L_black[j] + R_black[j]) / 2);
                }
                for (i = threerightdown[0]-1; i < 60; i++)
                {
    
    
                    if (LCenter[i]<93) LCenter[i] = 93;
                }

            }
            if (modetype == 2||modetype==5)  //
            {
    
    
                int endmidddle = 0;
                for (i = 0; i < 60; i++)
                {
    
    
                    if (Pixels[i][93] == 0)
                    {
    
    
                        endmidddle = i;
                        break;
                    }
                }
                if (endmidddle <= 1) endmidddle = 1;
                R_black[endmidddle - 1] = 90;
                R_black[endmidddle] = 93;
                advanced_regression34(2, 0, 1, endmidddle - 1, endmidddle);
                R_blackfuzhi(parameterA, parameterB, 0, 50);
                for (i = 0; i < 60; i++)
                {
    
    
                    if (L_black[i] < 130) L_black[i] = 186;
                }
                for (j = 0; j < 70; j++)
                {
    
    
                    LCenter[j] = (unsigned char)((L_black[j] + R_black[j]) / 2);
                }
                for (i = 0; i < 60; i++)
                {
    
    
                    if (LCenter[i] < 93) LCenter[i] = 93;
                }
            }
            if ((modetype == 7|| modetype == 10) && threeleftdown[1] >= 1 && LCenter[end] >= 1)  //
            {
    
    
                int endmidddle = 0;
                for (i = 0; i < 60; i++)
                {
    
    
                    if (Pixels[i][ 93] == 0)
                    {
    
    
                        endmidddle = i;
                        break;
                    }
                }
                if (endmidddle <= 1) endmidddle = 1;
                L_black[threeleftdown[0] - 1] = (unsigned char)(threeleftdown[1] - 1);
                L_black[threeleftdown[0]] = (unsigned char)(threeleftdown[1]);
                if (endmidddle <= 1) endmidddle = 1;
                L_black[endmidddle - 1] = 93;
                L_black[endmidddle] = 91;
                advanced_regression34(1, threeleftdown[0] - 1, threeleftdown[0], endmidddle - 1, endmidddle);
                leftlinefuzhi(parameterA, parameterB, threeleftdown[0] - 1, 50);
                for (i = 0; i < 60; i++)
                {
    
    
                    if (R_black[i] > 60) R_black[i] = 0;
                }
                for (j = 0; j < 70; j++)
                {
    
    
                    LCenter[j] = (unsigned char)((L_black[j] + R_black[j]) / 2);
                }
                for (i = threeleftdown[0] - 1; i < 60; i++)
                {
    
    
                    if (LCenter[i] > 93) LCenter[i] = 93;
                }

            }
            if (modetype ==8|| modetype==11)  //
            {
    
    
                int endmidddle = 0;
                for (i = 0; i < 60; i++)
                {
    
    
                    if (Pixels[i][93] == 0)
                    {
    
    
                        endmidddle = i;
                        break;
                    }
                }
                if (endmidddle <= 1) endmidddle = 1;
                L_black[endmidddle - 1] = 93;
                L_black[endmidddle] = 90;
                advanced_regression34(1,0,1, endmidddle - 1, endmidddle);
                leftlinefuzhi(parameterA, parameterB, 0, 50);
                for (i = 0; i < 60; i++)
                {
    
    
                    if (R_black[i] > 60) R_black[i] = 0;
                }
                for (j = 0; j < 70; j++)
                {
    
    
                    LCenter[j] = (unsigned char)((L_black[j] + R_black[j]) / 2);
                }
                for (i = 0; i < 60; i++)
                {
    
    
                    if (LCenter[i] > 93) LCenter[i] = 93;
                }
            }

}
//三岔路口操作结束

void advanced_regression34(int type, int startline1, int endline1, int startline2, int endline2)//最小二乘法拟合曲线，分别拟合中线，左线，右线,type表示拟合哪几条线
{
    
    

    int i = 0;
    int sumlines = endline1 - startline1 + endline2 - startline2;
    int sumX = 0;
    int sumY = 0;
    float averageX = 0;
    float averageY = 0;
    float sumUp = 0;
    float sumDown = 0;
    if (type == 0)      //拟合中线
    {
    
    
        for (i = startline1; i < endline1; i++)
        {
    
    
            sumX += i;
            sumY += LCenter[i];
        }
        if (sumlines != 0)
        {
    
    
            averageX =(float) sumX / sumlines;     //x的平均值
            averageY =(float) sumY / sumlines;     //y的平均值
        }
        else
        {
    
    
            averageX = 0;     //x的平均值
            averageY = 0;     //y的平均值
        }
        for (i = startline1; i < endline1; i++)
        {
    
    
            sumUp += (LCenter[i] - averageY) * (i - averageX);
            sumDown += (i - averageX) * (i - averageX);
        }
        if (sumDown == 0) parameterB = 0;
        else parameterB = sumUp / sumDown;
        parameterA = averageY - parameterB * averageX;
    }
    else if (type == 1)//拟合左线
    {
    
    

        for (i = startline1; i < endline1; i++)
        {
    
    
            sumX += i;
            sumY += L_black[i];
        }
        for (i = startline2; i < endline2; i++)
        {
    
    
            sumX += i;
            sumY += L_black[i];
        }
        if (sumlines == 0) sumlines = 1;
        averageX = (float)sumX / sumlines;     //x的平均值
        averageY = (float)sumY / sumlines;     //y的平均值
        for (i = startline1; i < endline1; i++)
        {
    
    
            sumUp += (L_black[i] - averageY) * (i - averageX);
            sumDown += (i - averageX) * (i - averageX);
        }
        for (i = startline2; i < endline2; i++)
        {
    
    
            sumUp += (L_black[i] - averageY) * (i - averageX);
            sumDown += (i - averageX) * (i - averageX);
        }
        if (sumDown == 0) parameterB = 0;
        else parameterB = sumUp / sumDown;
        parameterA = averageY - parameterB * averageX;
    }
    else if (type == 2)//拟合右线
    {
    
    
        for (i = startline1; i < endline1; i++)
        {
    
    
            sumX += i;
            sumY += R_black[i];
        }
        for (i = startline2; i < endline2; i++)
        {
    
    
            sumX += i;
            sumY += R_black[i];
        }
        if (sumlines == 0) sumlines = 1;
        averageX = (float)sumX / sumlines;     //x的平均值
        averageY = (float)sumY / sumlines;     //y的平均值
        for (i = startline1; i < endline1; i++)
        {
    
    
            sumUp += (R_black[i] - averageY) * (i - averageX);
            sumDown += (i - averageX) * (i - averageX);
        }
        for (i = startline2; i < endline2; i++)
        {
    
    
            sumUp += (R_black[i] - averageY) * (i - averageX);
            sumDown += (i - averageX) * (i - averageX);
        }
        if (sumDown == 0) parameterB = 0;
        else parameterB = sumUp / sumDown;
        parameterA = averageY - parameterB * averageX;

    }
    else if (type == 3)  //左下拉右上
    {
    
    
        for (i = startline1; i < endline1; i++)
        {
    
    
            sumX += i;
            sumY += L_black[i];
        }
        for (i = startline2; i < endline2; i++)
        {
    
    
            sumX += i;
            sumY += R_black[i];
        }
        if (sumlines == 0) sumlines = 1;
        averageX = (float)sumX / sumlines;     //x的平均值
        averageY =(float) sumY / sumlines;     //y的平均值
        for (i = startline1; i < endline1; i++)
        {
    
    
            sumUp += (L_black[i] - averageY) * (i - averageX);
            sumDown += (i - averageX) * (i - averageX);
        }
        for (i = startline2; i < endline2; i++)
        {
    
    
            sumUp += (R_black[i] - averageY) * (i - averageX);
            sumDown += (i - averageX) * (i - averageX);
        }
        if (sumDown == 0) parameterB = 0;
        else parameterB = sumUp / sumDown;
        parameterA = averageY - parameterB * averageX;
    }
    else if (type == 4) //右下拉左上
    {
    
    
        for (i = startline1; i < endline1; i++)
        {
    
    
            sumX += i;
            sumY += R_black[i];
        }
        for (i = startline2; i < endline2; i++)
        {
    
    
            sumX += i;
            sumY += L_black[i];
        }
        if (sumlines == 0) sumlines = 1;
        averageX =(float) sumX / sumlines;     //x的平均值
        averageY = (float)sumY / sumlines;     //y的平均值
        for (i = startline1; i < endline1; i++)
        {
    
    
            sumUp += (R_black[i] - averageY) * (i - averageX);
            sumDown += (i - averageX) * (i - averageX);
        }
        for (i = startline2; i < endline2; i++)
        {
    
    
            sumUp += (L_black[i] - averageY) * (i - averageX);
            sumDown += (i - averageX) * (i - averageX);
        }
        if (sumDown == 0) parameterB= 0;
        else parameterB = sumUp / sumDown;
        parameterA = averageY - parameterB * averageX;
    }
}

void leftlinefuzhi(float A, float B, int8 start_point, int8 end_point)
{
    
    
     int8 m;
     for (m = start_point; m <= end_point; m++)
     {
    
    
         if((B * m + A)>=255) L_black[m]=255;
         if((B * m + A)<=0) L_black[m]=0;
         else if(0<(B * m + A)&&(B * m + A)<255)L_black[m] = (int8)(B * m + A);
     }
}
 void R_blackfuzhi(float A, float B, int8 start_point, int8 end_point)
{
    
    
     int8 m;
     for (m = start_point; m <= end_point; m++)
     {
    
    
         if ((B * m + A) < 0) {
    
    R_black[m] = 0; }
       else if((B * m + A) >=0&&(B * m + A)<=255) R_black[m] = (int8)(B * m + A);
         if((B * m + A)>=255) R_black[m]=255;
     }
}
 //左右线赋值结束
 void zhongxianfuzhi( int8 start_point, int8 end_point)
 {
    
    
             int8 m;
             for (m = start_point; m <= end_point; m++)
             {
    
    
                 LCenter[m] = (int8)(L_black[m]/2 + R_black[m]/ 2);
             }
 }

总结

1.配合上辅助函数就是完整三岔路口判断、补线代码，如有不懂请留言哦。