智能车摄像头算法——圆环元素_智能车摄像头圆环

1.入环的函数

（1）搜上下边线

搜上下边线，处理圆环的时，可以利用上下边线的特点。

uint8_t UpdownSideGet(uint8_t imageInput[OV7725_UART_H][OV7725_UART_W], uint8_t imageOut[2][OV7725_UART_W])
{
    uint8_t i = 0, j = 0;
    uint8_t last = OV7725_UART_H/2;

    imageOut[0][OV7725_UART_W-1] = 0;
    imageOut[1][OV7725_UART_W-1] = OV7725_UART_H-1;
     //从图像中间行    从中到下     从中到上      扫描

    //处理中间单独那一列的上下边线
    for(i = last; i >= 0; i--)
    {
        if(!imageInput[i][OV7725_UART_W/2])
        {
            imageOut[up][OV7725_UART_W/2] = i;
            break;
        }
    }

    for(i = last; i < OV7725_UART_H; i++)
    {
        if(!imageInput[i][OV7725_UART_W/2])
        {
            imageOut[down][OV7725_UART_W/2] = i;
            break;
        }
    }
    //其他列的上下边线
    //从中到左
    for(i = OV7725_UART_W/2-1; i > 0; i--)//遍历每一列
    {
        imageOut[up][i] = 0;
        imageOut[down][i] = OV7725_UART_H-1;

        for(j = imageOut[0][i+1] + 5; j > 0; j--)//一列中的扫描每行  从上列的行数+10开始向上扫描
        {
            if(!imageInput[j][i])
            {
                imageOut[up][i] = j;
                break;
            }
        }
        for(j = imageOut[1][i+1] - 5; j < OV7725_UART_H; j++)
        {
            if(!imageInput[j][i])
            {
                imageOut[down][i] = j;
                break;
            }
        }
    }

    //从中到右
    for(i = OV7725_UART_W/2+1; i < OV7725_UART_W-1; i++)
        {
            imageOut[up][i] = 0;
            imageOut[down][i] = OV7725_UART_H-1;

            for(j = imageOut[0][i-1] + 5; j > 0; j--)
            {
                if(!imageInput[j][i])
                {
                    imageOut[up][i] = j;
                    break;
                }
            }
            for(j = imageOut[1][i-1] - 5; j < OV7725_UART_H; j++)
            {
                if(!imageInput[j][i])
                {
                    imageOut[down][i] = j;
                    break;
                }
            }
        }
    return 0;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77

（2）找凸起的弧

找凸起的弧，用于圆环的检测。如下图红色的线。

RoundaboutGetArc函数中传入的num代表着，要检测的这个圆弧大小，要求这个圆弧包含多少个点。

以左圆环为例：
先判断边线丢不丢线，不丢线再进行下一步。由于是遍历一幅图像左边线数组的每一行，我们就找这样一个特征：它下面连续递减点的个数+它上面连续递减点的个数+跟它横坐标一样大的点的个数 >=我们设定的值。这个就是我们认为的圆弧

/*!
  * @brief    判断左右边线是否存在弧形
  * 输出的 index 圆弧的顶点位置
  * @param    imageInput ： 二值图像信息
  * @param    imageOut   ： 边线数组
  * @param    status     ： 1：左边线  2：右边线
  * @param    num        ： 圆弧的大小 用点数表示  （连续N个增  连续N个减）
  * @return   1 有弧线    0  没弧线

  */
uint8_t RoundaboutGetArc(uint8_t imageSide[OV7725_UART_H][2], uint8_t status, uint8_t num,uint8_t* index)
{
    int i = 0;
    uint8_t inc = 0, dec = 0, n = 0;
    switch(status)
    {
      case 1:
        for(i = START_H-2; i > END_H; i--)
        {
        	//没有丢线  
            if(imageSide[i][0] != 1 && imageSide[i+1][0] != 1)
            {
                if(imageSide[i][0] == imageSide[i+1][0])
                {
                    n++;
                    continue;
                }
                if(imageSide[i][0] > imageSide[i+1][0])
                {
                    inc++;
                    inc+=n;
                    n=0;
                }
                else
                {
                    dec++;
                    dec+=n;
                    n=0;
                }

                /* 有弧线 */
                if(inc > num && dec > num)
                {
                    *index = i + num;
                    return 1;
                }
            }
            else
            {
                inc = 0;
                dec = 0;n=0;
            }
        }

        break;

      case 2:
        for(i = START_H-2; i > END_H; i--)
        {
            if(imageSide[i][1] != OV7725_UART_W-1 && imageSide[i+1][1] != OV7725_UART_W-1)
            {
                if(imageSide[i][1] == imageSide[i+1][1])
                {
                    n++;
                    continue;
                }
                if(imageSide[i][1] > imageSide[i+1][1])
                {
                    inc++;
                    inc+=n;
                    n = 0;
                }
                else
                {
                    dec++;
                    dec+=n;
                    n=0;
                }

                /* 有弧线 */
                if(inc > num && dec > num)
                {
                    *index = i + num;
                    return 1;
                }
            }
            else
            {
                inc = 0;
                dec = 0;n=0;
            }
        }

        break;
    }

    return 0;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98

（3）两点之间补线

/*!
   * @brief    补线处理
   *
   * @param    imageSide  : 边线
   * @param    status     : 1：左边线补线   2：右边线补线
   * @param    startX     : 起始点 列数
   * @param    startY     : 起始点 行数
   * @param    endX       : 结束点 列数
   * @param    endY       : 结束点 行数
   *
   * @return
   *
   * @note     endY 一定要大于 startY
   *
   */
 void ImageAddingLine(uint8_t imageSide[OV7725_UART_H][2], uint8_t status, uint8_t startX, uint8_t startY, uint8_t endX, uint8_t endY)
 {
     int i = 0;

     // 直线 x = ky + b
     float k = 0.0f, b = 0.0f;
     switch(status)
     {
       case 1://左补线
         {
             k = (float)((float)endX - (float)startX) / (float)((float)endY - (float)startY);
             b = (float)startX - (float)startY * k;

             for(i = startY; i < endY; i++)
             {
                 imageSide[i][0] = (uint8_t)(k * i + b);
             }
             break;
         }

       case 2://右补线
         {
             k = (float)((float)endX - (float)startX) / (float)((float)endY - (float)startY);
             b = (float)startX - (float)startY * k;

             for(i = startY; i < endY; i++)
             {
                 imageSide[i][1] = (uint8_t)(k * i + b);
             }
             break;
         }

     }
 }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49

（4）判断上线是否单调

/*!
  * @brief    判断上边线是否单调
  * @param    X1 :起始X点
  * @param    X2 :终止X点              X1 < X2
  * @param    imageIn ： 边线数组
  *
  * @return   0：不单调or错误， 1：单调递增， 2：单调递减
  *
  * @note
  *
  * @see
  *
  * @date     2021/11/30 星期二
  */
uint8_t RoadUpSide_Mono(uint8_t X1, uint8_t X2, uint8_t imageIn[2][OV7725_UART_W])
{
    uint8_t i = 0, num = 0;

    for(i = X1; i < X2-1; i++)
    {
        if(imageIn[0][i] >= imageIn[0][i+1])
            num++;
        else
            num = 0;
        if (num >= (X2-X1)*4/5)
            return 1;
    }

    for(i = X1; i < X2-1; i++)
    {
        if(imageIn[0][i] <= imageIn[0][i+1])
            num++;
        else
            num = 0;
        if (num >= (X2-X1)*4/5)
            return 2;
    }
    return 0;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39

2.找圆环

找圆环的特征，以左圆环为例，要求“左边线有弧，右边线单调”。左边线有弧已经说过了，这里的“右边线单调”：需要右边线在一定的横坐标范围内单调的点数大于我们设定的值，才认为是这里的“右边线单调”。

uint8_t RoadIsRoundabout(uint8_t Upimage[2][OV7725_UART_W], uint8_t imageInput[OV7725_UART_H][OV7725_UART_W], uint8_t image[OV7725_UART_H][2], uint8_t *flag)
{
    uint8_t i = 0;
    errL=0, errR=0;
    leftState = 0, rightState = 0;
     count = 0;
    uint8_t num = 0, py;

    
    // 从车头往前 左边线是否单调
    for(i = START_H-2; i > END_H; i--)
    {
        if(image[i][0] == 1)
            continue;
        if(image[i][0] >= image[i+1][0])    // i是Y坐标值  0 是图像左线X坐标
        {
            if(image[i][0]<OV7725_UART_W/2 - 5)
                                    num++;
            else
                   num = 0  ;

            if(num == 50)
            {
                num = 0;
                leftState = 1;   // 左单调标志
                break;
            }
        }
        else
        {
            num = 0;
        }
        if(i <= END_H+1)  //   清0
            num = 0;
    }


    errL = RoundaboutGetArc(image, 1, round_size, &py);
    errR = RoundaboutGetArc(image, 2, round_size, &py);

    // 右边线是否单调
    for(i = START_H-2; i > END_H; i--)
    {
        if(image[i][1] == OV7725_UART_W-1)
            continue;

        if(image[i][1]<= image[i+1][1])
        {
            if(image[i][1]>OV7725_UART_W/2 + 5)
                           num++;
            else
                    num = 0  ;


            if(num == 50)
            {
                num = 0;
                rightState = 1;
                break;
            }
        }
        else
        {
              num = 0;
        }

        if(i <= END_H+1)
            num = 0;
    }

    // 左边单调， 检测右侧是否是环岛
    if(leftState == 1 && rightState == 0 && errL == 0)
    {
        count = 0;

        if(RoundaboutGetArc(image, 2, round_size, &count))
        {
            *flag = 1;
            return 1;
        }
        else
        {
            return 0;
        }
    }

    /* 右边单调， 检测左侧是否是环岛 */
    if(rightState == 1 && leftState == 0&& errR == 0)
    {
        count = 0;
        if(RoundaboutGetArc(image, 1, round_size, &count))
        {
            *flag = 2;
            return 2;
        }
    }
    return 0;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98

3.补线入环出环

右环为例，左环也是一样，对称写（右环为奇数状态，左环为偶数状态）。目前已经找到圆环，进入状态1。
状态1：找到圆环后，补一条线，保证车子直走。一直检查圆弧还在不在，直到弧消失了，就进入状态3。

状态3：让车子一直往右拐，一直拐到上线单调的时候。进入状态5

这个还没拐成，要继续拐。

拐到这种时候，就已经进入圆环口了，可以不用拐了。
在圆环内，都是这种图像，正常走就行了，之后的状态就是出圆环了。

状态5：出圆环，找右边凸起的位置+下线全是最低点这种情况，然后补一条线，让车子向左拐。一直拐到上线单调的时候，进入状态7.

一直拐
拐到上线单调了，进入状态7。

状态7：加大力度拐，拐到上线不单调的时候，就出圆环正常行驶了。

参考代码
具体要根据车子的速度和摄像头选择图像的大小来确定里面的参数

void RoundaboutProcess(uint8_t imageInput[OV7725_UART_H][OV7725_UART_W], uint8_t imageSide[OV7725_UART_H][2], uint8_t UpdowmSide[2][OV7725_UART_W], uint8_t* state)
{

    uint8_t i = 0, err5 = 0;
    uint8_t pointX = 0, pointY = 0, inc = 0, dec = 0;
    uint8_t flag= 0, Down_flag = 0;
    static uint8_t finderr = 0;
    static uint8_t err1 = 0;

    switch(*state)
    {
    //奇数为入右圆环
        case 1:
          
            // 检查弧线
            err1 = RoundaboutGetArc(imageSide, 2, 3, &pointY);

            // 有弧线 进行补线 连接弧线最右点 和 图像左下角
            if(err1)
            {
                pointX = imageSide[pointY][1];
                ImageAddingLine(imageSide, 2, pointX, pointY, OV7725_UART_W-1, START_H);
                finderr = 1;
            }
            else
            {
                if(finderr)
                    *state = 3;//准备进入环岛
            }

            break;

            /* 发现左环岛 环岛出口处补线 */

       

        case 3:

            for(i=1;i<OV7725_UART_H-1;i++)
               {
                     ImageSide[i][0]= ImageSide[i][0]+50;

               }

            if(RoadUpSide_Mono(30, OV7725_UART_W-1,UpdowmSide) == 1)//上线单调增进入下一步
                  *state = 5;

              break;




        case 5 :

                err5 = RoundaboutGetArc(imageSide, 1, 10, &pointY);

                //检查下线
                for(i = OV7725_UART_W-1; i > 0; i--)
                {
                    if(UpdowmSide[1][i] == 119)
                          inc++;
                    else
                         dec++;
                    if( dec <= 15)
                    {
                        Down_flag = 1;
                        break;
                    }
                 }

                flag = RoadUpSide_Mono(20, OV7725_UART_W,UpdowmSide);

                if(flag && err5 && Down_flag)
                {
                     *state = 7;
                }
                break;


        case 7:

                ImageAddingLine(imageSide, 1, 80, 10, 0, START_H);
                flag = RoadUpSide_Mono(50, OV7725_UART_W,UpdowmSide);

                if(flag==0)
                {

                    *state = 0;
                    finderr = 0;
                    err1 = 0;

                }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92

有问题可以私聊我咯~
可以加企鹅群 658476482 交流
另外承接各种单片机设计~